基于擴展卡爾曼濾波的水下目標測量系統

李智生,余義德,杜寅峰,李 釗

(1.中國人民解放軍91550部隊,遼寧大連116023;2.中國人民解放軍65547部隊,遼寧海城114200;3.第二炮兵駐石家莊地區軍事代表室,河北石家莊050002)

0 引言

水下目標運動測量系統用于精確測量目標航跡的大地坐標和目標相對大地的航速。由于目標聲納基陣波動和測量引起的誤差,使得測得的目標位置圍繞真值上下波動。為此,系統所測得數據仍是一種隨機變量,若不對該數據近一步的處理,就無法得到滿意的精度[1]。

研究應用擴展卡爾曼濾波方法對測量數據進行精度評估,推導了相應的濾波公式,建立了一種擴展卡爾曼濾波算法。通過系統的仿真結果表明該算法收斂速度和估計精度能夠滿足系統要求。

1 卡爾曼濾波公式的推導

卡爾曼濾波是以最小均方誤差為準則的最優估計,是一種對動態系統進行數據處理的有效方法。用于目標跟蹤定位的卡爾曼濾波器通常是一種典型的非線性濾波器,它具有下列矩陣方程[2,3]:

狀態方程:

量測方程:

預報估計:

濾波估計:

預報誤差協方差陣:

濾波誤差協方差陣:

最佳增益矩陣:

式中,φK/K-1為N×N狀態轉移矩陣;WK為動態噪聲;VK為觀測噪聲,且WK與VK是互不相關的零均值白噪聲序列;ΓK為N×N噪聲轉移矩陣,即對所有k,j,有EWK=0,EVK=0;ZK為N×1測量向量。

根據上述遞推公式之后,如果已知K-1時刻的狀態估計 ^XK-1及估計誤差的協方差陣 PK-1,就可以根據K時刻的量測值ZK得到K時刻的估計^XK和相應的誤差協方差矩陣PK。

2 卡爾曼濾波在系統中的應用

測量系統利用線纜拖曳加載GPS天線的移動通信平臺,GPS天線能夠實時獲取移動通信平臺坐標參數,同時移動通信平臺上裝載的水聽器能夠實時接收目標發出的聲納信號,通過計算聲納脈沖的時延時間和目標的方位信息,便可實時計算目標相對移動通信平臺的相對距離和方位,從而可以準確推算目標的位置。

2.1 系統模型

建立恰當的數學模型是卡爾曼濾波模型的濾波成敗的關鍵。一個好的數學模型既要反映目標的動態特征,又要足夠簡化,以保證計算簡便和速度。這里針對目標的運動狀態來建立相應的模型,目標在水下航行過程可基本視為勻速運動,且大體上按某個設定的深度航行。因此,把狀態方程和量測方程都設置在直角坐標系中[4]。

在直角坐標系中,可用一個線性動態模型和一個非線性觀測模型來建立目標的運動模型。在直角坐標系中,把移動通信平臺O和目標都看作質點來處理,則系統的動態模型為:

式中,狀態向量X=(x,y,z,vx,vy,vz)T為目標相對于移動通信平臺O的相對位置;v為相對目標速度。

考慮如下情形:移動通信平臺相對靜止不動,目標在水下某一平面作勻速直線運動,則在測量時間間隔內,通過移動通信平臺O上的被動聲納設備可測得目標與移動通信平臺的相對距離俯仰角和方位角的數據序列。設測量時噪聲為高斯白噪聲,因為移動通信平臺靜止,所以此時v為目標的絕對速度,其中vz為0,于是目標的狀態向量為X=(x,y,z,vx,vy)T。

目標在tK時刻的位置可以表示為:

式中,C為未知參數。則目標的狀態方程為:

轉移矩陣為:

目標與移動通信平臺之間距離的量測方程為:

該距離是利用聲納脈沖的時延值計算得到的。

目標的俯仰角測量方程為:

目標的方位角測量方程為:

寫成向量形式為:

將量測方程中的觀測矩陣H(tK)線性化,需要求其雅可比矩陣,即

至此,得出了目標的狀態式(10)和量測方程式(14),根據式(3)～式(7),給出恰當的X(0/0)和P(0/0),以及測量誤差方差,即可在線、實時地計算下去,并不斷地得到目標運動要素的最小方差估計及估計誤差方差。

2.2 濾波初值的設定

式中,T為信號周期,在本系統中,T=0.1 s,而誤差協方差陣的定義為:

則狀態估值初始協方差陣為:

3 實例仿真

以移動通信平臺為坐標參考點,在水深30 m處,水下航行器距離移動通信平臺60 m處勻速直航靠近移動通信平臺,航速為4 m/s,水下航行器相對移動通信平臺的初始方位為45°,狀態更新頻率為0.1 Hz。假設測量噪聲符合零均值、方差 δ2=3的高斯分布,則目標的初始狀態為:X0=(21.21,21.21,51.96,1.4,1.4)。

利用r0,e0,A0的數據加噪聲干擾,作為測量值,根據測量值,用本文的卡爾曼濾波方法計算所有時刻X=(x,y,z,vx,vy)T的估計值。目標真實航跡和經卡爾曼濾波后的估計軌跡,其中虛線表示濾波后的估計軌跡如圖1所示。x方向位置的估計誤差如圖2所示。航速的軌跡誤差如圖3所示。從圖1可知:濾波算法在第60個濾波周期開始收斂,且比較平穩。圖2可以看出,整個航程內,x方向誤差都在10 m以內,符合系統設計要求。由圖3可知,航速估計在第10個周期內開始收斂,且誤差在1 m內,能夠滿足系統要求。

圖1 目標真實航跡和經卡爾曼濾波后的估計軌跡

圖2 x方向位置的估計誤差

圖3 航速的軌跡誤差

4 結束語

對基于擴展卡爾曼濾波的水下目標航跡測量系統的濾波算法進行研究,建立了水下目標運動狀態方程和測量方程,針對非線性測量方程,采用雅克比矩陣進行了線性化處理。仿真結果表明,該濾波算法有較快的收斂性和較好的估計精度,能夠滿足系統要求。該方法也可以擴展應用到其他工程領域。

[1]AIDALA V J.Kalman Filter Behavior in Bearings-only Tracking Applications[J].IEEE trans.Aerospace and Engineering Systems,1979,15(1):29-39.

[2]SONG T L,SPEYER J L.A Stochastic Analysis of a Modified Gain Extended Kalman Filter with Application to Estimation with Bearings-only Measurements[J].IEEE Trans.Automatic Control,1985,30(10):940-949.

[3]周 豐.基于卡爾曼濾波水下彈道測量系統研究[J].彈道學報,2004,16(3):66-69.

[4]劉 偉,王昌明,趙 輝.基于卡爾曼濾波水下近距目標運動分析[J].彈道學報,2008,20(4):28-31.