改進的中心差分卡爾曼濾波水下被動目標跟蹤

鄭 藝，王明洲，胡友峰

(1.中國船舶集團有限公司 第 705 研究所，陜西 西安 710077；2.中國船舶集團有限公司 第705研究所昆明分部，云南 昆明 650118)

0 引 言

對水下目標進行識別和跟蹤對于潛艇、魚雷、水下無人潛航器等來說至關重要，是保證它們安全作業(yè)的重要環(huán)節(jié)。為了保證自身安全，不主動向外輻射能量的前提下，被動地對目標運動狀態(tài)進行估計是很有必要的。而單站純方位目標跟蹤所需觀測者少、隱蔽性強，應用場景廣泛[1-2]。距離目標較近時由于方位抖動影響更大，使得濾波容易出現(xiàn)不穩(wěn)定、甚至發(fā)散的情況。同時，這種情形下的一個難點是目標狀態(tài)的潛在不完全可觀測性，因此狀態(tài)估計難度更大[3-4]。

單站純方位目標跟蹤是非線性問題，非線性濾波是解決這一問題常用的有效方法之一。擴展卡爾曼濾波（Extanded Kalman Filter，EKF）[5]是較早應用于目標跟蹤的非線性濾波方法，但其僅為1階近似，線性化誤差較大，濾波的精度和穩(wěn)定性較差[6]。采樣型方法是實現(xiàn)非線性濾波的另一類方法，包括確定性采樣和隨機采樣。以粒子濾波[7]（Particle Filter，PF）為代表的隨機性采樣計算量過大，且可能會出現(xiàn)粒子退化或貧乏的問題，在工程應用中具有局限性[8]。

確定性采樣方法包括無跡卡爾曼濾波（Unscented Kalman Filter，UKF）[9]、容積卡爾曼濾波[10]（Cubature Kalman Filter，CKF）、中心差分卡爾曼濾波（Central Difference Kalman Filter，CDKF）[11-12]等。其中UKF 是應用最廣泛的一種方法[13-14]，可達至少 2 階近似，但需要根據(jù)運動或觀測模型的非線性來選擇α，β，λ三個參數(shù)，難以找到參數(shù)的最優(yōu)值。在UKF基礎上的平方根無跡卡爾曼濾波（Square Root Unscented Kalman Filter，SR-UKF）也是一種用途廣泛的濾波方法，具有較好的穩(wěn)定性，是一種經(jīng)典的平方根類的方法，在水下目標跟蹤中有較好的效果[15-16]。因此本文也將SR-UKF作為一種對比方法加入到仿真中。

CDKF也是確定性采樣方法中的一種，它選取采樣點的方式與UKF不同。CDKF基于Sterling 多項式插值公式，對非線性函數(shù)按中心差分形式逼近，可達至少2階近似。CDKF精度與UKF相當，且只需調(diào)整1 個參數(shù)h。因此 CDKF 在目標跟蹤[17-18]、導航系統(tǒng)[19]等方面都有較好應用。但單站純方位目標跟蹤系統(tǒng)的可觀測性差、觀測噪聲影響大，致使濾波器不穩(wěn)定[3-4]，尤其在目標距離近的情況下這種現(xiàn)象更常見。

本文目的是解決在近距離的純方位觀測情況中，單站純方位目標跟蹤中CDKF容易出現(xiàn)的濾波不穩(wěn)定問題。因此提出了一種CDKF的改進方法，在采樣點的協(xié)方差和量測協(xié)方差中采用QR分解計算協(xié)方差平方根，而在協(xié)方差平方根更新時使用更為穩(wěn)定的奇異值分解（Singular Value Decomposition，SVD）進行計算，提高算法的穩(wěn)定性和估計精度。

1 問題描述和濾波方法

1.1 單站純方位目標跟蹤

對于水下目標，在許多情況下，目標的運動是非機動的。因此，近似等速（Nearly Constant Velocity，NCV）模型[5,20]適用于水下被動目標跟蹤場景。本文考慮二維平面運動模型，非機動目標和機動的觀測站。

目標的運動狀態(tài)可以表示為向量：

其中：xk，yk表示目標位置對應的坐標，表示目標的速度分量。對于NCV模型，目標勻速直線運動，系統(tǒng)的狀態(tài)方程可表示為：

其中： Φ 為狀態(tài)轉(zhuǎn)移矩陣；w(k)為均值為0方差為Q的系統(tǒng)噪聲：

系統(tǒng)的觀測方程可表示如下：

其中：v(k)為均值為0方差為R的觀測噪聲。對于單站純方位目標跟蹤系統(tǒng)，觀測函數(shù)為：

純方位目標跟蹤是典型的非線性濾波問題，式（1）和式（4）組成了系統(tǒng)狀態(tài)空間模型。對于單站純方位量測來說，觀測站機動是系統(tǒng)完全可觀測的必要條件而非充分條件，觀測站的機動情況對濾波效果起著關鍵作用，但在近距離追擊目標的情況下，觀測站較大機動容易丟失目標，因此只能小機動或者不機動，因此系統(tǒng)可觀測性受限。

1.2 中心差分卡爾曼濾波

假設系統(tǒng)狀態(tài)變量是服從高斯分布的，且均值和協(xié)方差已知。CDKF的核心思想是利用斯特林多項式插值公式，將非線性方程按中心差分形式展開，無需計算函數(shù)的雅可比矩陣。非線性函數(shù)f(x)在x=xˉ處展開的2階斯特林多項式插值公式為：

其中：h為中心差分半步長，其取值可決定采樣點的分布區(qū)間，適合于高斯分布的h最佳取值為為均值為0，與x具有相同協(xié)方差的隨機變量。式（6）可以看作是用中心差分運算代替泰勒級數(shù)展開中的求導運算，用1階和2階中心差分算子來代替1階和2階導數(shù)。

文獻[11]中的中心差分濾波器和文獻[12]中的差分濾波器都是基于斯特林多項式插值公式，本質(zhì)上是相同的，也就是CDKF。CDKF通過確定性加權(quán)的采樣點（Sigma點）來近似狀態(tài)變量的分布函數(shù)，并通過采樣點的非線性變換，估計非線性變換后狀態(tài)變量的均值和協(xié)方差。L維的狀態(tài)向量需要構(gòu)造個Sigma采樣點，Sigma點與真實的狀態(tài)向量有著相同的均值、方差和高階中心距。常規(guī)CDKF用于目標跟蹤的具體步驟如下：

步驟1k=0時，初始化狀態(tài)向量和協(xié)方差

步驟2采樣點和對應權(quán)值

步驟3時間更新

步驟4采樣點協(xié)方差矩陣

步驟5采樣點點集更新

步驟6量測及其協(xié)方差更新

步驟7卡爾曼增益和目標狀態(tài)更新

步驟8狀態(tài)協(xié)方差更新

在k=1,2···K時，循環(huán)步驟2到步驟8，即可完成CDKF濾波。

2 本文所提方法

CDKF方法是一種應用廣泛的確定性采樣的非線性濾波方法，但在單站純方位目標跟蹤中，容易出現(xiàn)濾波不穩(wěn)定甚至發(fā)散的情形。為了解決這一問題，本文提出一種奇異值分解平方根中心差分卡爾曼濾波（Singular Value Decomposition Square Root Central Difference Kalman Filter，SVDSR-CDKF）。不同于常規(guī)的平方根方法，它在計算采樣點的協(xié)方差和量測協(xié)方差中采用QR分解，而在計算狀態(tài)協(xié)方差更新階段使用奇異值分解。

2.1 協(xié)方差的QR分解

與常規(guī)的平方根方法相同，本方法同樣對采樣點協(xié)方差和量測協(xié)方差進行QR分解。QR分解可表示為A=QR，它將矩陣A分解為一個正規(guī)正交矩陣Q和一個上三角矩陣R的乘積，則容易推導出：

用采樣點協(xié)方差矩陣QR分解得到的代替，則

其中，QR{}表示QR分解，返回結(jié)果為分解后得到的上三角矩陣。用量測協(xié)方差矩陣的QR分解代替，則

2.2 奇異值分解協(xié)方差平方根更新

常規(guī)的平方根方法采用Cholesky分解計算狀態(tài)協(xié)方差的平方根。Cholesky分解要求被分解的矩陣是正定的,而對于純方位單站目標跟蹤，尤其是近距離時，方位噪聲擾動大和觀測站機動的影響，常規(guī)的平方根CDKF在狀態(tài)協(xié)方差平方根更新有時可能出現(xiàn)非正定的情形，導致濾波不能正常進行。奇異值分解不受被分解矩陣正定性的限制，相比于Cholesky分解更加穩(wěn)定和易于實現(xiàn)。因此本文提出在CDKF的狀態(tài)協(xié)方差更新階段，用奇異值分解的方法構(gòu)造狀態(tài)協(xié)方差的平方根。

若A∈Rm×n(m≥n)，則矩陣A的奇異值分解為：

其 中 ，U∈Rm×m，T∈Rn×n， Λ ∈Rm×n，S=diag(s1,s2,······sr)，s1≥ s2≥ ······≥ sr≥ 0是A的奇異值。

單站純方位目標跟蹤系統(tǒng)，狀態(tài)方程的協(xié)方差矩陣為：

P是一個對稱矩陣，對其進行奇異值分解后可得U=V，故可得：

然后，對更新后的狀態(tài)協(xié)方差進行奇異值分解：

其中，U,S(d),V分別對應式(27)中的U,Λ,T。最后令協(xié)方差矩陣平方根為：

用式（30）～式（32）代替CDKF中的步驟8，可以得到一種基于奇異值分解的狀態(tài)協(xié)方差平方根更新方式。由于奇異值非負，因此總是有意義的。即使式（30）的協(xié)方差是非正定的，也可以完成奇異值分解，并計算得到Sk。

2.3 奇異值分解平方根CDKF流程

本文所提出的奇異值分解平方根的中心差分卡爾曼濾波方法建立在1.2節(jié)所述的CDKF方法的框架之上，具體步驟如下：

步驟1k=0時，初始化狀態(tài)向量和協(xié)方差分解陣

其中，chol代表Cholesky分解。

步驟2采樣點及其權(quán)值

步驟3時間更新

步驟4采樣點協(xié)方差的QR分解

步驟5采樣點集更新

步驟6量測更新

步驟7量測協(xié)方差陣QR分解

步驟8卡爾曼增益和目標狀態(tài)更新

3 仿真實驗

為了驗證本文所提方法在單站純方位目標跟蹤中的有效性和優(yōu)勢，在3種常見的近距離跟蹤軌跡情形下進行仿真。初始狀態(tài)觀測站獲取的距離誤差5%，速度誤差5%，初航向誤差的均方差為3°。方位估計周期為100 ms，系統(tǒng)的觀測噪聲協(xié)方差R=3，過程噪聲強度。分別用CDKF、常見的平方根類方法SRUKF以及本文所提的奇異值分解平方根CDKF方法進行100次蒙特卡羅實驗。

用均方根誤差（RMSE）來衡量估計偏差。定義均方根誤差如下：

情形1：直線攔截目標軌跡。

目標以50 kn速度向東做勻速直線運動。觀測站初始位置坐標為（480，128），以50 kn的速度向南偏西60°方向運動，觀測時間10 s。這種情形下的運動態(tài)勢如圖1所示。圖2為100次蒙特卡羅實驗統(tǒng)計的3種方法的均方根誤差比較。

圖1 情形 1 運動態(tài)勢圖Fig.1 Movement situation map of case 1

圖2 情形 1 均方根誤差Fig.2 Root mean square error of case 1

為了驗證不同觀測噪聲協(xié)方差下所提方法的有效性，在觀測噪聲協(xié)方差為1°～5°的條件下分別進行100次蒙特卡洛實驗，統(tǒng)計3種方法的平均RMSE，結(jié)果如表1所示。

表1 不同觀測噪聲協(xié)方差下的各方法比較（情形1）Tab.1 Comparison of methods in different observation noise covariance (Case 1)

情形2：觀察者近距離提前角追蹤目標。

目標以50 kn速度向東勻速直線運動，觀測站在目標北偏東60°方向距目標250 m處，以50 kn的速度、7°的提前角追蹤目標，觀測時長為7 s。圖3展示了這一情形下的目標和觀測站的運動態(tài)勢。統(tǒng)計100次蒙特卡羅實驗結(jié)果，3種方法的均方根誤差比較如圖4所示。

在觀測噪聲協(xié)方差為1°～5°的條件下，統(tǒng)計100次實驗中3種方法的平均RMSE，如表2所示。

圖3 情形 2 運動態(tài)勢圖Fig.3 Movement situation map of case 3

圖4 情形 2 均方根誤差Fig.4 Root mean square error of case 2

表2 不同觀測噪聲協(xié)方差下的各方法比較（情形2）Tab.2 Comparison of methods in different observation noise covariance (case 2)

情形3：觀測站迎面攔截態(tài)勢。

目標以50 kn速度向東做勻速直線運動。觀測站初始位置坐標為（454，145），以50 kn的速度向南偏西45°方向勻速直線運動。8 s后觀測站轉(zhuǎn)向正西方向以50 kn速度向目標勻速直線運動，觀測時長共計14 s。情形3的運動態(tài)勢如圖5所示。3種方法100次蒙特卡羅實驗后的均方根誤差如圖6所示。3種方法觀測噪聲協(xié)方差為1°～5°時，100次實驗的平均RMSE如表3所示。

圖5 情形 3 運動態(tài)勢圖Fig.5 Movement situation map of case 3

圖6 情形 3 均方根誤差Fig.6 Root mean square error of case 3

表3 不同觀測噪聲協(xié)方差下的各方法比較（情形3）Tab.3 Comparison of methods in different observation noise covariance (case 3)

綜合3種情形下各濾波器的仿真結(jié)果，對于同一目標不同的跟蹤軌跡得到的跟蹤誤差是不同的，這說明對于單站純方位目標跟蹤而言，觀測站的機動直接影響估計效果。但實際中肩負其他作業(yè)任務的觀測站不一定可以執(zhí)行最優(yōu)觀測軌跡，這對濾波方法的性能提出了更高要求。而3種不同的觀測軌跡中，對比3種濾波方法的誤差可以得到相似的結(jié)論：常規(guī)的CDKF方法由于容易出現(xiàn)發(fā)散，導致平均誤差較大，各種情形下其均方根誤差都是最大的。SR-UKF方法作為一種經(jīng)典的平方根類的方法，用于水下純方位目標跟蹤具有較好效果，各種情形下誤差均低于CDKF方法。本文所提的SVDSR-CDKF方法解決了CDKF在幾種情形中容易發(fā)散的問題，且濾波誤差最低。在3種仿真情形下和各種不同的觀測噪聲協(xié)方差下，本文提出的SVDSR-CDKF方法均具有最低的均方根誤差。

4 結(jié) 語

針對單站純方位目標分析中有時容易出現(xiàn)的濾波器不穩(wěn)定、易發(fā)散的情況，本文提出一種SVDSR-CDKF方法。該方法以CDKF方法為基礎，在計算采樣點協(xié)方差和量測協(xié)方差時采用QR分解計算協(xié)方差平方根，而在狀態(tài)協(xié)方差更新階段使用奇異值分解。通過2種不同的方式計算協(xié)方差的平方根代替協(xié)方差矩陣參與運算，增強算法的穩(wěn)定性。為驗證所提方法效果，進行了3組不同情形的仿真實驗，比較常規(guī)CDKF方法、經(jīng)典的平方根類方法SR-UKF方法和本文所提的SVDSR-CDKF方法，并在每種情形下調(diào)整不同的觀測噪聲進行各方法的均方根誤差比較。結(jié)果表明，本文所提方法避免了常規(guī)CDKF容易發(fā)散的情形，且具有比常規(guī)CDKF和SR-UKF更低的濾波誤差。綜合各項仿真結(jié)果表明，本文提出的SRSVDCDKF方法是一種精度高、穩(wěn)定性好的純方位目標跟蹤方法。對于本文的未盡之處，未來考慮在以下方向進行研究：一是探索該方法對于跟蹤機動目標的效果；二是考慮該方法對于三維模型中目標的跟蹤。