非等間隔采樣條件下卡爾曼濾波器設(shè)計與實現(xiàn)

楊 森 羅利強 高 劍 張西平

(西安電子工程研究所 西安 710100)

0 引言

卡爾曼濾波算法廣泛應(yīng)用于導(dǎo)航、通信等領(lǐng)域，該算法通過遞推方式獲得新的狀態(tài)估計，實時獲取估計信息，而不需要存儲大量的歷史信息。在雷達數(shù)據(jù)處理過程中，需利用當前時刻的量測信息來預(yù)測下一時間的估計信息，因此卡爾曼濾波算法在雷達數(shù)據(jù)處理中得到了廣泛的應(yīng)用。

在工程實際運用中，雷達采樣間隔并不都是等周期的，此時若仍采用恒定的采樣間隔進行計算，可能造成系統(tǒng)的初始狀態(tài)和初始協(xié)方差的假設(shè)值偏差過大，導(dǎo)致濾波發(fā)散。為了克服非等間隔采樣條件下跟蹤精度不佳的問題，本文提出了一種非等間隔采樣條件下卡爾曼濾波器設(shè)計與實現(xiàn)方法，該方法通過算式推導(dǎo)，從原理上獲得了協(xié)方差的精確解，完成濾波迭代，不僅避免了由于協(xié)方差近似解造成的濾波發(fā)散的風險，而且可同時應(yīng)用于非等間隔、等間隔采樣條件下卡爾曼濾波器的設(shè)計與實現(xiàn)，且計算量沒有明顯變化，拓寬了已有濾波算法應(yīng)用場合。

1 非等間隔采樣條件下卡爾曼濾波器的設(shè)計

以三坐標雷達的數(shù)據(jù)處理問題為例，非等間隔采樣條件下卡爾曼濾波器的設(shè)計包含以下兩部分內(nèi)容：一是非等間隔采樣條件下濾波器參數(shù)的初始化；二是非等間隔采樣條件下的濾波器迭代過程。其中，濾波器參數(shù)的初始化主要包括初始狀態(tài)估計和初始協(xié)方差估計兩個部分；濾波器迭代過程中，主要包括狀態(tài)、協(xié)方差預(yù)測，量測預(yù)測，新息、增益等的計算，最終完成狀態(tài)更新以及協(xié)方差更新。

1.1 非等間隔采樣條件下濾波器參數(shù)初始化

(1)

其中，、、分別為極坐標系下距離、方位、俯仰的量測值。

(2)

式(2)中，為量測(0)與(1)的時間間隔，為量測(1)與(2)的時間間隔。

在直角坐標系下，時刻的量測噪聲協(xié)方差()可表示為

(3)

式(3)中，為徑向距離測量誤差的方差，和分別為方位角和俯仰角測量誤差的方差，而

(4)

初始協(xié)方差(2|2)為

(5)

式(5)中，、、、、、為分塊矩陣，且

(=1,2,3;=1,2,3)

(6)

1.2 濾波器迭代

在完成濾波器初始化的基礎(chǔ)上，利用后續(xù)更新量測，依次完成狀態(tài)、協(xié)方差預(yù)測，量測預(yù)測，新息、增益等的計算，完成濾波器狀態(tài)更新迭代。

在非等間隔采樣條件下，濾波器迭代過程與等間隔采樣條件下基本一致，其中，狀態(tài)轉(zhuǎn)移矩陣()、量測噪聲的協(xié)方差(+1)以及過程噪聲的協(xié)方差()的取值與時間間隔有關(guān)，因此在迭代過程中，需實時更新以上結(jié)果并代入。

離散時間系統(tǒng)的狀態(tài)方程可表示為式(7)。

(+1)=()()+()()+()

(7)

量測方程可表示為

(+1)=(+1)(+1)+(+1)

(8)

狀態(tài)的一步預(yù)測為

(9)

量測的預(yù)測為

(10)

新息表示為式(11)。

(11)

協(xié)方差的一步預(yù)測為

(+1|)=()(|)′()+()

(12)

量測的新息協(xié)方差表示為

(+1)=(+1)(+1|)·
′(+1)+(+1)

(13)

增益為

(+1)=(+1|)·
′(+1)(+1)

(14)

可得狀態(tài)更新方程和協(xié)方差更新方程為

(15)

(+1|+1)=(+1|)-
(+1)(+1)′(+1)

(16)

卡爾曼濾波流程圖如圖1所示。

圖1 卡爾曼濾波流程圖

2 性能驗證

2.1 仿真分析

1)場景設(shè)置

模擬運動目標在空間做等高勻加速運動，初始速度為200m/s、加速度為1m/s，飛行高度1500m；雷達測距精度70m，測角精度(方位角、俯仰角)均為03°，采樣點數(shù)取100；雷達工作在扇掃工作模式(扇掃周期為2s)，此時相鄰兩次掃描到同一目標的時間間隔不同，其中第一、二個掃描時刻的時間間隔約為15s，第二、三個掃描時刻的時間間隔約為05s。

2)非等間隔采樣條件

采用文中提出的方法，在濾波迭代過程中，實時更新采樣間隔，即實時代入=15s、=05s。其中一次仿真結(jié)果的示意圖如圖2所示。

圖2 真實軌跡、觀測軌跡與濾波估計

500次蒙特卡洛仿真條件下，距離、方位角和俯仰角誤差的精度曲線如圖3至圖5所示。

圖3 距離誤差的精度曲線

圖4 方位角誤差的精度曲線

圖5 俯仰角誤差的精度曲線

在仿真過程中，測距精度為50m，測角精度均為0.3°。對500次蒙特卡洛仿真結(jié)果求均值，距離精度為31.9045m，方位角精度為0.1747°，俯仰角精度為0.1812°。

3)等間隔采樣條件

采用等間隔采樣卡爾曼濾波器作為對比，在濾波迭代過程中，用扇掃周期的一半近似表示采樣間隔，即T=T=1s。

500次蒙特卡洛仿真條件下，距離、方位角和俯仰角誤差的精度曲線如圖6至圖8所示。

圖6 距離誤差的精度曲線

圖7 方位角誤差的精度曲線

圖8 俯仰角誤差的精度曲線

對500次蒙特卡洛仿真結(jié)果求均值，距離精度為56.8797m，方位角精度為0.1750°，俯仰角精度為0.1817°。

2.2 實測結(jié)果

兩種模式下誤差的精度對比結(jié)果如表1所示。

表1 兩種濾波方式結(jié)果對比

非等間隔采樣條件下(=1.5s，=0.5s)進行迭代濾波，距離、方位角和俯仰角誤差的精度與預(yù)設(shè)值相比均變小，符合工程實際運用要求。觀察采樣間隔==1s時濾波結(jié)果，可以看出，雖然方位角、俯仰角的濾波效果與前者基本持平，但是距離的濾波效果不佳，甚至出現(xiàn)濾波精度不如觀測精度的現(xiàn)象。

3 結(jié)束語

本文提出了一種非等間隔采樣條件下卡爾曼濾波器設(shè)計與實現(xiàn)方法，該方法在非等間隔采樣條件下完成濾波器參數(shù)初始化以及濾波器迭代，在計算量沒有明顯增加的情況下，濾波結(jié)果精度更高，算法有效性和可靠性高，且易于實現(xiàn)，具有一定的工程應(yīng)用價值。