鄭 藝，王明洲，胡友峰

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

0 引 言

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

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

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

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

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

1 問題描述和濾波方法

1.1 單站純方位目標(biāo)跟蹤

對(duì)于水下目標(biāo)，在許多情況下，目標(biāo)的運(yùn)動(dòng)是非機(jī)動(dòng)的。因此，近似等速（Nearly Constant Velocity，NCV）模型[5,20]適用于水下被動(dòng)目標(biāo)跟蹤場(chǎng)景。本文考慮二維平面運(yùn)動(dòng)模型，非機(jī)動(dòng)目標(biāo)和機(jī)動(dòng)的觀測(cè)站。

目標(biāo)的運(yùn)動(dòng)狀態(tài)可以表示為向量：

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

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

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

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

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

1.2 中心差分卡爾曼濾波

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

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

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

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

步驟2采樣點(diǎn)和對(duì)應(yīng)權(quán)值

步驟3時(shí)間更新

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

步驟5采樣點(diǎn)點(diǎn)集更新

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

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

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

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

2 本文所提方法

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

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

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

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

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

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

常規(guī)的平方根方法采用Cholesky分解計(jì)算狀態(tài)協(xié)方差的平方根。Cholesky分解要求被分解的矩陣是正定的,而對(duì)于純方位單站目標(biāo)跟蹤，尤其是近距離時(shí)，方位噪聲擾動(dòng)大和觀測(cè)站機(jī)動(dòng)的影響，常規(guī)的平方根CDKF在狀態(tài)協(xié)方差平方根更新有時(shí)可能出現(xiàn)非正定的情形，導(dǎo)致濾波不能正常進(jìn)行。奇異值分解不受被分解矩陣正定性的限制，相比于Cholesky分解更加穩(wěn)定和易于實(shí)現(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的奇異值。

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

P是一個(gè)對(duì)稱矩陣，對(duì)其進(jìn)行奇異值分解后可得U=V，故可得：

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

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

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

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

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

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

其中，chol代表Cholesky分解。

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

步驟3時(shí)間更新

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

步驟5采樣點(diǎn)集更新

步驟6量測(cè)更新

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

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

3 仿真實(shí)驗(yàn)

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

用均方根誤差（RMSE）來(lái)衡量估計(jì)偏差。定義均方根誤差如下：

情形1：直線攔截目標(biāo)軌跡。

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

圖1 情形 1 運(yùn)動(dòng)態(tài)勢(shì)圖Fig.1 Movement situation map of case 1

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

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

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

情形2：觀察者近距離提前角追蹤目標(biāo)。

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

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

圖3 情形 2 運(yùn)動(dòng)態(tài)勢(shì)圖Fig.3 Movement situation map of case 3

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

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

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

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

圖5 情形 3 運(yùn)動(dòng)態(tài)勢(shì)圖Fig.5 Movement situation map of case 3

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

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

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

4 結(jié) 語(yǔ)

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