基于近似二階EKF 的非線性濾波仿真*

孫立珍 趙樂樂 劉廣忱

（1.內蒙古建筑職業技術學院 呼和浩特 010070）（2.內蒙古工業大學 呼和浩特 010080）

1 引言

濾波問題是基于某系統的狀態空間模型，通過對存在誤差的實際觀測量的處理，估計出系統狀態量或參數的問題［1］。濾波技術先后出現有多種被用于科學研究和工程實際［2］，文獻［3～6］分別利用最小二乘法、極大似然法、Wiener法、粒子群算法進行濾波估計。Kalman 濾波是一種適用于線性系統的線性最小方差估計的最優遞推算法，在滿足其使用條件下，建立系統的狀態空間模型，通過對觀測量的更新來修正當前狀態量的預測值，進而實現對當前狀態量的估算［7］。由于工程實際涉及的系統多為非線性系統，于是應用在線性化的非線性系統濾波的擴展Kalman濾波法（EKF）被提出。

EKF 是將非線性函數的Taylor 展開式的二階及二階以上的高階項忽略掉，進行一階線性化，而線性化引起了一定的誤差，特別是系統強非線性時，較大的誤差可能造成濾波發散［8］。因此，在滿足線性系統、高斯白噪聲、所有隨機變量均服從高斯（GAUSSIAN）分布的三個假設條件時，EKF 是最小方差準則下的次優濾波算法，其性能主要依賴于系統的非線性程度［9］。為了最大限度地利用系統非線性變換的前二階矩的信息，減小由于線性化所引起的估計誤差，降低濾波發散的可能性，又有人提出了近似二階擴展卡爾曼濾波法（ASEKF）。

本文以空中拋射物為研究對象，建立拋射物的運動跟蹤系統的狀態方程并離散化，基于Matlab，利用ASEKF 估計系統狀態量，并計算系統輸出量，對物體進行跟蹤，進一步了解ASEKF 的濾波原理，分析其效率，并驗證其精度，也為ASEKF 的工程實際應用和Matlab仿真提供參考。

2 ASEKF的濾波原理

ASEKF 是基于線性最小方差遞推濾波框架，應用狀態量和觀測量均值變換的二階近似，方差和協方差進行一階近似得到非線性系統的遞推濾波框架［10］。ASEKF 可以在綜合考慮估計精度和運算量的情況下，先對非線性系統方程做近似二階非線性變換，再利用Kalman 濾波法估計系統的狀態量［11］。

對于如式（1）的非線性離散系統。

ASEKF 執行流程如圖1 所示。ASEKF 的濾波過程是對系統狀態量不斷預測和修正的過程，先對系統狀態量進行預測，然后利用觀測量對狀態量預測值修正，獲得狀態量估計值，完成一個濾波過程。該方法只需要上一次狀態量估計值和當前觀測量來更新計算出新的狀態量估計值，適用于系統狀態量的實時估計。

圖1 ASKEF的流程圖

3 基于ASEKF的濾波仿真

3.1 空中拋射物的運動跟蹤模型

假設從空中向某一方向拋射一個質量為m 的物體，其初始水平速度為vx（0），初始垂直速度為vy（0），初始位置坐標（x（0），y（0））；物體運動過程中受重力mg、阻尼力、不確定的零均值白噪聲干擾力的影響，且阻尼力與速度平方成正比關系，水平和垂直兩個方向的阻尼系數分別為kx、ky，噪聲干擾力分別為mδax、mδay［13］。在二維坐標系中，從坐標原點處觀測運動物體的實時位置，距原點的距離為r，與y 軸的夾角為α，存在的零均值白噪聲分別為δr、δα［14］。對某一拋射物的運動觀測如圖2所示。

圖2 拋射物的運動觀測圖

根據拋射物的運動特點，選擇橫向位置、水平速度、縱向位置和垂直速度作為狀態量，選擇距離r和夾角α作為觀測量，建立物體運動跟蹤系統的狀態方程和觀測方程。由于空中拋射物所受阻尼力的方向與速度的方向相反［15］，因此物體斜向上運動時的系統狀態方程與斜向下運動時的狀態方程不同。

1）當vy≥0、即vy是垂直向上或等于0 時，阻尼力mky垂直向下，系統離散狀態方程F（X）如式（5）所示。

式（5）、（6）、（7）中，Ts為采樣時間，k-1 和k 分別表示k-1時刻、k時刻。

3.2 仿真分析

根據ASEKF 的遞推原理和方法，利用拋射物的運動跟蹤模型可以推導出相應的濾波如式（8）所示。

式（8）包括兩個部分，一是狀態量、誤差協方差和測量量的預測方程，二是ASEKF 濾波增益、狀態量和誤差協方差更新方程。該公式對狀態量和測量量的預測進行二階近似，誤差協方差的預測采用一階近似，利用測量量對狀態量預測值進行修正，同時不斷更新卡爾曼濾波增益和誤差協方差。

在Matlab 中，基于拋射物的運動跟蹤的ASEKF 濾波公式編寫程序［16］，主要包括真實狀態量模擬、觀測量構造、ASEKF 濾波和誤差計算這幾個模塊。根據空中拋射物的運動特點和仿真需要，設定合適的狀態量初值X0、誤差協方差初值P0、過程噪聲、觀測噪聲、仿真時長t 和步長Ts等參數，并運行仿真程序，拋射物的模擬真實運動軌跡、構造觀測量和濾波跟蹤結果如圖3 所示，構造觀測量誤差和濾波跟蹤誤差如圖4 所示。從仿真結果看出，構造的觀測量與模擬的真實運動軌跡之間的誤差較大，難以確定拋射物的真實運動狀態和軌跡，而ASEKF 濾波結果與真實運動軌跡之間的誤差較小，基本在2%以內，能夠濾掉較大的系統噪聲，較準確地實現對拋射物的運動跟蹤。

圖3 拋射物的運動軌跡

圖4 拋射物的運動跟蹤誤差

4 結語

本文首先對ASEKF 進行理論分析，了解其濾波方法和特點；然后以空中拋射物為濾波仿真研究對象，建立運動跟蹤系統離散化狀態方程，并根據ASEKF 的遞推原理和方法推導出相應的濾波公式；最后基于拋射物的運動跟蹤的濾波公式編寫Matlab 程序進行仿真分析。仿真結果表明，在觀測量誤差較大的情況下，通過ASEKF 對拋射物的運動跟蹤系統的狀態量進行估計，并計算系統輸出量，濾波過估計程收斂快，收斂后的誤差小，只有不到2%的誤差，能夠較準確地實現對拋射物的運動跟蹤，說明了ASEKF濾波法具有較高的估算精度。