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

孫立珍 趙樂樂 劉廣忱

（1.內(nèi)蒙古建筑職業(yè)技術(shù)學(xué)院 呼和浩特 010070）（2.內(nèi)蒙古工業(yè)大學(xué) 呼和浩特 010080）

1 引言

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

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

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

2 ASEKF的濾波原理

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

對(duì)于如式（1）的非線性離散系統(tǒng)。

ASEKF 執(zhí)行流程如圖1 所示。ASEKF 的濾波過程是對(duì)系統(tǒng)狀態(tài)量不斷預(yù)測和修正的過程，先對(duì)系統(tǒng)狀態(tài)量進(jìn)行預(yù)測，然后利用觀測量對(duì)狀態(tài)量預(yù)測值修正，獲得狀態(tài)量估計(jì)值，完成一個(gè)濾波過程。該方法只需要上一次狀態(tài)量估計(jì)值和當(dāng)前觀測量來更新計(jì)算出新的狀態(tài)量估計(jì)值，適用于系統(tǒng)狀態(tài)量的實(shí)時(shí)估計(jì)。

圖1 ASKEF的流程圖

3 基于ASEKF的濾波仿真

3.1 空中拋射物的運(yùn)動(dòng)跟蹤模型

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

圖2 拋射物的運(yùn)動(dòng)觀測圖

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

1）當(dāng)vy≥0、即vy是垂直向上或等于0 時(shí)，阻尼力mky垂直向下，系統(tǒng)離散狀態(tài)方程F（X）如式（5）所示。

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

3.2 仿真分析

根據(jù)ASEKF 的遞推原理和方法，利用拋射物的運(yùn)動(dòng)跟蹤模型可以推導(dǎo)出相應(yīng)的濾波如式（8）所示。

式（8）包括兩個(gè)部分，一是狀態(tài)量、誤差協(xié)方差和測量量的預(yù)測方程，二是ASEKF 濾波增益、狀態(tài)量和誤差協(xié)方差更新方程。該公式對(duì)狀態(tài)量和測量量的預(yù)測進(jìn)行二階近似，誤差協(xié)方差的預(yù)測采用一階近似，利用測量量對(duì)狀態(tài)量預(yù)測值進(jìn)行修正，同時(shí)不斷更新卡爾曼濾波增益和誤差協(xié)方差。

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

圖3 拋射物的運(yùn)動(dòng)軌跡

圖4 拋射物的運(yùn)動(dòng)跟蹤誤差

4 結(jié)語

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