基于擴(kuò)展卡爾曼濾波的水下目標(biāo)運(yùn)動(dòng)軌跡分析算法?

劉兆才 劉 杰 徐學(xué)偉

（91388部隊(duì)41分隊(duì) 湛江 524022）

1 引言

對水下目標(biāo)的探測一般使用聲學(xué)方式進(jìn)行探測，聲納探測器收集的水下目標(biāo)信息能夠反應(yīng)目標(biāo)運(yùn)動(dòng)性質(zhì)，但目標(biāo)回波往往參雜了噪聲信號(hào)使探測數(shù)據(jù)有一定的隨機(jī)誤差［1］。一般方法不能很好地對噪聲信號(hào)進(jìn)行濾波處理，導(dǎo)致對目標(biāo)運(yùn)動(dòng)的數(shù)據(jù)處理精度降低甚至發(fā)散。經(jīng)典卡爾曼濾波對噪聲特征變化的適應(yīng)能力差，并且目標(biāo)運(yùn)動(dòng)方程往往具有不同程度的非線性特性。因此考慮使用擴(kuò)展卡爾曼濾波的算法將離散信號(hào)進(jìn)行濾波處理，還原出目標(biāo)運(yùn)動(dòng)軌跡［2］。本文探討一種基于擴(kuò)展卡爾曼濾波的水下目標(biāo)數(shù)據(jù)處理方法，并與原始目標(biāo)真實(shí)運(yùn)動(dòng)軌跡數(shù)據(jù)進(jìn)行比較，驗(yàn)證此方法的有效性，為探測系統(tǒng)生成水下目標(biāo)態(tài)勢提供支撐。

2 擴(kuò)展卡爾曼濾波算法

擴(kuò)展卡爾曼算法（EKF）的基本思想是將非線性系統(tǒng)線性化，也就是將非線性函數(shù)進(jìn)行泰勒展開并略去高階項(xiàng)，然后進(jìn)行卡爾曼濾波［4］。

圖1 濾波流程示意圖

2.1 非線性系統(tǒng)模型

在非線性系統(tǒng)中，狀態(tài)向量與其系數(shù)是不能分離的。系統(tǒng)狀態(tài)方程［3］如下:

W與V分別代表過程噪聲和測量噪聲，數(shù)學(xué)期望均為0，方差分別為Q、R，x代表系統(tǒng)狀態(tài)，且W與V非線性相關(guān)。

利用非線性函數(shù)的泰勒展開，將系統(tǒng)模型截?cái)嘀炼雾?xiàng)，忽略其余高階項(xiàng)，相比一階線性化截?cái)嗫捎行p少由于線性化引起的估計(jì)誤差。

圖2 目標(biāo)運(yùn)動(dòng)示意圖

2.2 線性化求解

由于目標(biāo)運(yùn)動(dòng)軌跡是連續(xù)的，可以將非線性系統(tǒng)的動(dòng)態(tài)特征變成線性的動(dòng)態(tài)特征［6］。線性卡爾曼濾波中的狀態(tài)轉(zhuǎn)移矩陣A和觀測矩陣H由f和h函數(shù)的雅可比矩陣代替，n維狀態(tài)的求法如下:

線性化后，依據(jù)卡爾曼濾波的迭代方法［5］計(jì)算，計(jì)算過程如下式:

2.3 線性化后的濾波模型流程［9］

線性化后的濾波模型流程如圖3所示。

圖3 濾波處理流程圖

3 目標(biāo)軌跡信息估計(jì)

平面運(yùn)動(dòng)物體的運(yùn)動(dòng)狀態(tài)主要由位置、速度、加速度等決定，引入狀態(tài)向量如下:X=[x,vx,ax,y,vy,ay]T，式中 x、vx、ax、y、vy、ay分別表示目標(biāo)在X、Y坐標(biāo)系中的位置、速度、加速度分量。系統(tǒng)t+T時(shí)刻狀態(tài)方程為［7］

測量向量選取x、y坐標(biāo)軸上的點(diǎn)，表示為Z=[x,y]T加入系統(tǒng)過程誤差和測量誤差，則系統(tǒng)方程表示為［8］

系統(tǒng)過程噪聲W及測量噪聲V均為不相關(guān)的白噪聲，即其期望值為0。其中Φ、h如下式［10］:

令過程噪聲方差矩陣為Qi=E(WiWTi)，系統(tǒng)測量噪聲方差矩陣為Ri=E(ViVTi)，計(jì)算得到Q與R。根據(jù)卡爾曼濾波流程，在方差矩陣及Φi+1|i已知的情況下，可通過 X?0、P0及i時(shí)刻觀測值Zi，即可得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)［11］。

4 試驗(yàn)及分析

為對上述算法性能進(jìn)行分析驗(yàn)證，使用Matlab工具模擬水下目標(biāo)航跡，并通過加入隨機(jī)觀測誤差Ri的方式來模擬實(shí)際的測量值zi。假設(shè)水下目標(biāo)初始段航向30°，末段做120°的機(jī)動(dòng)旋回。數(shù)據(jù)采樣周期為20s，采樣點(diǎn)數(shù)為80點(diǎn)，測量噪聲誤差 ≤0.5。

設(shè)置系統(tǒng)初始位置x?0及協(xié)方差矩陣P0為

濾波前真實(shí)目標(biāo)軌跡與測量軌跡如圖4。

對測量值zi濾波后的軌跡圖如圖5。

通過計(jì)算系統(tǒng)測量值zi與真實(shí)目標(biāo)軌跡一次差及濾波后軌跡一次差，濾波前后的一次差如圖6所示。濾波前軌跡測量值的二階原點(diǎn)距約為0.41，濾波后軌跡的二階原點(diǎn)距約為0.15（中段15點(diǎn)統(tǒng)計(jì)值）。與濾波后的軌跡濾波后的目標(biāo)軌跡與實(shí)際軌跡的二階原點(diǎn)距減小了約65%，表明此方法可有效降低因觀測誤差導(dǎo)致的水下目標(biāo)軌跡跳變大的影響，同時(shí)可有效提高軌跡測量精度，為后續(xù)水下目標(biāo)的運(yùn)動(dòng)要素解算提供支持。

圖4 真實(shí)—測量目標(biāo)軌跡圖

圖5 濾波后目標(biāo)軌跡圖

圖6 濾波前后一次差

5 結(jié)語

本文針對水下目標(biāo)軌跡測量存在的觀測誤差大且運(yùn)動(dòng)非線性的特點(diǎn)，利用擴(kuò)展卡爾曼濾波器（EKF）完成了水下運(yùn)動(dòng)目標(biāo)軌跡的濾波算法設(shè)計(jì)。同時(shí)通過實(shí)驗(yàn)分析，表明了在目標(biāo)做簡單機(jī)動(dòng)時(shí)，擴(kuò)展卡爾曼濾波（EKF）算法能夠很好地對目標(biāo)運(yùn)動(dòng)軌跡進(jìn)行估計(jì)，可以較準(zhǔn)確還原水下目標(biāo)的運(yùn)動(dòng)軌跡，為進(jìn)一步的目標(biāo)運(yùn)動(dòng)要素等的數(shù)據(jù)處理提供支撐。但是這一方法并未考慮水下目標(biāo)變深機(jī)動(dòng)的情況，下一步將研究如何將此方法應(yīng)用到水下目標(biāo)三維運(yùn)動(dòng)軌跡的處理中。