一種改進(jìn)的迭代無跡卡爾曼濾波算法

陳 波

(新鄉(xiāng)學(xué)院機(jī)電工程學(xué)院 河南 新鄉(xiāng) 453003)

0 引 言

系統(tǒng)狀態(tài)變量的精確估計(jì)對(duì)故障檢測(cè)和控制應(yīng)用有重要的作用。然而，非線性系統(tǒng)是不容易估計(jì)的，需要得到描述該運(yùn)動(dòng)的全概率密度函數(shù)。這種解決方案一般會(huì)受到多狀態(tài)、不對(duì)稱和不連續(xù)性等因素的影響。由于對(duì)全概率密度函數(shù)的形式?jīng)]有限制，在一般情況下，它不能用有限的參數(shù)來表示。因此，任何實(shí)際的估計(jì)必須使用某些類型的近似值。目前許多不同類型的近似值已經(jīng)得到應(yīng)用，可是大多數(shù)的應(yīng)用要么計(jì)算量難以處理，要么需要對(duì)難以實(shí)踐的過程和觀察模型做特殊的設(shè)想。主要由于這些原因，卡爾曼濾波(Kalman Filter,KF)一直是最廣泛使用的估計(jì)算法。

張文等[1]分析了擴(kuò)展卡爾曼濾波(EKF)、無跡卡爾曼濾波(UKF)，說明對(duì)于強(qiáng)非線性系統(tǒng)，UKF比EKF具有更強(qiáng)的優(yōu)越性。張玉峰等[2]提出了一種基于平方根濾波的非線性卡爾曼濾波方法——自適應(yīng)平方根無跡卡爾曼濾波方法，改進(jìn)了傳統(tǒng)的Sage-Husa自適應(yīng)濾波算法，結(jié)合平方根無跡卡爾曼濾波算法進(jìn)行非線性濾波。但其要確保系統(tǒng)狀態(tài)和噪聲方差陣的對(duì)稱性和非負(fù)定性，增加了算法的計(jì)算量。李敏等[3]提出了一種改進(jìn)的強(qiáng)跟蹤平方根UKF算法，通過引入多重自適應(yīng)衰減因子調(diào)節(jié)協(xié)方差矩陣，提高了濾波器的估計(jì)精度，但其估計(jì)精度的提高有限。張園等[4]提出s修正無跡卡爾曼濾波(SUKF)方法，雖然跟蹤精度大幅提高了，但其增加了跟蹤的計(jì)算量。黃平等[5]提出了一種基于量測(cè)一步預(yù)測(cè)信息的在線自調(diào)整的UKF算法，根據(jù)單步濾波的預(yù)測(cè)信息選取濾波時(shí)刻的最優(yōu)濾波參數(shù)，進(jìn)而實(shí)現(xiàn)算法的在線調(diào)整。基于該算法的UKF對(duì)于真實(shí)狀態(tài)的跟蹤效果優(yōu)于固定參數(shù)的UKF。但其存在參數(shù)的取值范圍增加有限，步長(zhǎng)減小幅度有限等缺點(diǎn)，會(huì)導(dǎo)致計(jì)算量的急劇增加、算法的運(yùn)行時(shí)間明顯變長(zhǎng)。Sarkka等[6]對(duì)非線性系統(tǒng)采用省略二階以上高階項(xiàng)得到線性化模型，只能在濾波誤差及一步預(yù)測(cè)誤差較小時(shí)才能適用，是一種次優(yōu)濾波，跟蹤精度的誤差較大。文獻(xiàn)[7]采用非線性函數(shù)傳播估計(jì)系統(tǒng)狀態(tài)的均值和協(xié)方差，但過程繁瑣，需要頻繁調(diào)節(jié)自由參數(shù)才能得到較好的跟蹤效果。

KF在非線性系統(tǒng)中最常見的應(yīng)用是擴(kuò)展KF(Extended KF,EKF)的形式，EKF簡(jiǎn)單地使所有的非線性變換成線性化，并用雅可比矩陣代替KF方程中的線性變換。此外，如果測(cè)量和過程噪聲協(xié)方差矩陣的先驗(yàn)信息是可用的，擴(kuò)展卡爾曼濾波能進(jìn)行非常方便快捷的實(shí)時(shí)處理，而且相當(dāng)容易實(shí)現(xiàn)，通常由泰勒展開式進(jìn)行線性化。由于忽略了高階項(xiàng)，EKF的線性化會(huì)導(dǎo)致出現(xiàn)相應(yīng)的狀態(tài)估計(jì)錯(cuò)誤，為了減少這種錯(cuò)誤，通常采用二階EKF。然而，這種方法在大測(cè)量數(shù)據(jù)問題的計(jì)算上有一定難度，減少這種誤差的另一方法是線性化測(cè)量狀態(tài)空間中的參考軌跡。這種方法被稱為迭代擴(kuò)展卡爾曼濾波(Iterated EKF，IEKF)，該方法適合非線性僅存在于測(cè)量方程的情況[8]。雖然EKF保持了KF的優(yōu)點(diǎn)和計(jì)算上效率遞歸更新的形式，但是它存在一些嚴(yán)重的局限性：(1) 如果誤差可以用線性函數(shù)很好地近似模擬，線性變換就是唯一可靠的；否則該線性近似的效果會(huì)非常差。在最好的情況下，這破壞了濾波器的性能；在最壞的情況下，這會(huì)導(dǎo)致EKF的估計(jì)值全都偏離。然而，要確定這種假設(shè)的有效性是極其困難的，因?yàn)檫@取決于變換本身、當(dāng)前狀態(tài)的估計(jì)以及協(xié)方差的幅度。(2) EKF只有在各隨機(jī)向量近似高斯分布的情況下才會(huì)有較好的近似,還不能滿足復(fù)雜的密度、期望協(xié)方差表示。然而，這并非總是如此，一些系統(tǒng)包含不連續(xù)的其他的包含奇點(diǎn)，而在其他系統(tǒng)中，狀態(tài)本身是固有離散的。(3) 在一些應(yīng)用中，解析發(fā)現(xiàn)雅可比矩陣非常困難，需要使用雅可比矩陣的數(shù)值近似。但是，由于涉及到近似值而不是真實(shí)值，會(huì)導(dǎo)致出現(xiàn)其他類型的問題。初始狀態(tài)估計(jì)的選擇、濾波器參數(shù)的調(diào)整是標(biāo)準(zhǔn)EKF成功收斂的關(guān)鍵。(4) 在EKF中，卡爾曼增益矩陣取決于數(shù)據(jù)，濾波器的穩(wěn)定性不再被假定。此外，濾波器特性的分析非常困難，擴(kuò)展卡爾曼濾波不保證無偏估計(jì)。計(jì)算好的誤差協(xié)方差矩陣，并不一定代表真實(shí)誤差協(xié)方差，這些結(jié)果的分析也是很難處理的，采用無跡變換代替EKF從而得到無跡卡爾曼濾波(UKF)。

目前無跡變換已經(jīng)在EKF框架中使用[9-10]，與任意估計(jì)的非線性函數(shù)或變換相比，它使用直覺更容易估計(jì)的概率分布函數(shù)。狀態(tài)分布是通過高斯密度估計(jì)的，它由一組確定性選擇的采樣點(diǎn)來表示。一些作者已經(jīng)提出了通過一組確定性選擇的采樣點(diǎn)，使用后驗(yàn)密度的高斯分布來表示非線性濾波的思想[11-12]。非線性濾波器的整個(gè)等級(jí)(包括UKF)被稱為線性回歸卡爾曼濾波器，因?yàn)樗鼈兌际腔诮y(tǒng)計(jì)的線性化，而不是分析線性化(如EKF)。統(tǒng)計(jì)線性化是通過回歸點(diǎn)的線性回歸進(jìn)行的，這些樣本點(diǎn)完全地記錄了真實(shí)的均值和高斯密度的協(xié)方差。當(dāng)非線性系統(tǒng)使用時(shí)，它們準(zhǔn)確記錄了任何非線性的二階(泰勒級(jí)數(shù)展開)的真正均值和協(xié)方差[13]。

UKF產(chǎn)生的性能在線性系統(tǒng)上等同于卡爾曼濾波器，而且不用EKF所需的線性化步驟就成功推廣到非線性系統(tǒng)。在嚴(yán)格非線性系統(tǒng)下，該UKF持續(xù)達(dá)到的精度水平比EKF更高。而且UKF和EKF的計(jì)算復(fù)雜性是在相同的指令下完成的。

本文提出了一種改進(jìn)的迭代無跡卡爾曼濾波(Improved Iterated Uncented Kalman Filter,IUKF)算法。在系統(tǒng)方程嚴(yán)重非線性的時(shí)候，它比UKF和IEKF算法具有更好的性能。該方法使用了一步預(yù)測(cè)步驟和過濾的更新步驟的線性統(tǒng)計(jì)分析。

1 卡爾曼濾波算法

UKF是無偏轉(zhuǎn)換到遞歸狀態(tài)估計(jì)的簡(jiǎn)單擴(kuò)展。無偏轉(zhuǎn)換是用于計(jì)算對(duì)經(jīng)歷了非線性變換的隨機(jī)變量進(jìn)行統(tǒng)計(jì)的方法，UKF使用直覺來簡(jiǎn)化概率分布函數(shù)，而不是近似任意的非線性函數(shù)或變換。按照這種直覺，產(chǎn)生一組點(diǎn)，稱為σ點(diǎn)，其樣本均值和樣本協(xié)方差為x(k|k)，第k次迭代的狀態(tài)估計(jì)和P(k|k)狀態(tài)協(xié)方差矩陣。依次對(duì)這些點(diǎn)中的每一個(gè)應(yīng)用非線性函數(shù)以產(chǎn)生變換的樣本，并且從轉(zhuǎn)化的樣本計(jì)算預(yù)測(cè)的平均值和協(xié)方差。

這在表面上類似于蒙特卡羅方法，但是樣本沒有隨機(jī)生成。相反，樣本被確定性地選擇，使得它們捕獲關(guān)于分布的特定信息。一般來說，這種直覺可以應(yīng)用于捕獲關(guān)于多類型多種類的分布信息。在這里，我們考慮捕獲假設(shè)高斯分布的均值和協(xié)方差的特殊情況。

(1)

2 迭代無損卡爾曼濾波

由式(1)選擇的樣本集與x(k)的分布具有相同的樣本均值、協(xié)方差和更高的奇數(shù)中心矩。矩陣平方根和k影響σ點(diǎn)的第四和更高階樣本矩。

考慮以下非線性濾波問題：

x(k+1)=fk(x(k),w(k))

z(k)=hk+1(x(k))+v(k)

(2)

式中：x(k)是系統(tǒng)在時(shí)間步長(zhǎng)k的向量狀態(tài)，w(k)是由干擾和建模誤差引起的向量噪聲過程，z(k) 是觀測(cè)向量，v(k) 是加法測(cè)量噪音，假設(shè)噪聲向量w(k)和v(k)為零均值：

E[w(i)wT(j)]=δijCv(i)

E[v(i)vT(j)]=δijCv(i)

E[v(i)wT(j)]=0?i,j

(3)

然后執(zhí)行如下的預(yù)測(cè)步驟：

(4)

預(yù)測(cè)協(xié)方差矩陣為：

(5)

使用標(biāo)準(zhǔn)向量和矩陣運(yùn)算計(jì)算平均向量和協(xié)方差矩陣，這意味著算法適用于任何選擇的過程模型。該算法實(shí)現(xiàn)方便，不需要評(píng)估EKF中需要的雅可比矩陣，比通過分析線性化確定的預(yù)測(cè)更準(zhǔn)確。

更新步驟：通過觀察測(cè)量z(k)獲得的知識(shí)細(xì)化密度函數(shù)。使用貝葉斯定理對(duì)無記憶感覺系統(tǒng)的條件密度產(chǎn)生：

p(x(k)|Z(k))=p(x(k)|Z(k-1),Z(k))=

(6)

式中：Z(k)是收到的從Z(1)到Z(k)的一組觀察值，c是歸一化常數(shù):

c=∮x(k)∈Xp(Z(k)|x(k),p(x(k)|Z(k-1)))dx(k)

近似的前提是認(rèn)為預(yù)測(cè)狀態(tài)和測(cè)量噪聲都是正態(tài)分布的。這樣，后驗(yàn)概率密度p(x(k)|Z(k))是兩個(gè)高斯分布的結(jié)果，也是一個(gè)高斯分布，因此，MMSE估計(jì)符合MAP估計(jì)。目前的任務(wù)是找到最大的后驗(yàn)概率密度，通過排除不相關(guān)的常量和因子并最大化其對(duì)數(shù)后，可以歸結(jié)為最小化以下函數(shù)：

(7)

(8)

(9)

可以從式(7)中得到F(x)的清晰的雅可比矩陣和Hessian矩陣:

(10)

式(10)和式(9)的差產(chǎn)生如下迭代形式：

(11)

(12)

3 仿真結(jié)果與分析

下面將提出的新算法應(yīng)用于通過線性動(dòng)態(tài)系統(tǒng)方程跟蹤假定目標(biāo)的狀態(tài)估計(jì)問題，其測(cè)量方程中存在一些非線性，并和UKF、IEKF兩種算法的性能進(jìn)行對(duì)比。主要比較新提出的濾波器和UKF、IEKF兩種算法在處理狀態(tài)動(dòng)態(tài)方程和測(cè)量方程存在非線性時(shí)的性能。考慮以下狀態(tài)估計(jì)問題：

x1(k+1)=-kx2(k)+w1(k)

x2(k+1)=-exp[10-2x2(k)]+w2(k)

z(k)=x1(k)x2(k)+v(k)

(13)

設(shè)置采樣時(shí)間T=0.03 s,其中z(k)是接收到的k時(shí)刻的觀測(cè)數(shù)據(jù)，w1(k)、w2(k)、v(k)是獨(dú)立的零均值加性高斯噪聲過程，v(k)的方差為100，且具有：

假設(shè)噪聲向量w(k)和v(k)為零均值獨(dú)立噪聲。從線性狀態(tài)方程中可以得出x1和x2可以分別解釋為目標(biāo)的位置和目標(biāo)的速度。用均方根誤差(Root Mean Square Error,RMSE)表示平均值，作為衡量跟蹤性能的標(biāo)準(zhǔn)，并進(jìn)行80次蒙特卡羅試驗(yàn)。RMSE能較好地表示系統(tǒng)的估計(jì)精度，其表達(dá)式為：

新的改進(jìn)的迭代無跡卡爾曼濾波的優(yōu)越性如圖1和圖2所示。與改進(jìn)的迭代無跡卡爾曼濾波算法相比，IEKF與UKF的性能表現(xiàn)很差，結(jié)果的差異非常顯著。

圖1 位置估計(jì)的RMSE比較

圖2 速度估計(jì)的RSME比較

由圖1、圖2可以得出，運(yùn)動(dòng)狀態(tài)的非線性越嚴(yán)重，通過IEKF和UKF得到的狀態(tài)估計(jì)的錯(cuò)誤越大。而IIUKF算法在狀態(tài)方程中存在嚴(yán)重非線性時(shí)能表現(xiàn)出更好的性能，而且它還具有較好的穩(wěn)態(tài)誤差的能力。

4 結(jié) 語(yǔ)

本文引入了一種改進(jìn)的無跡卡爾曼濾波算法，在運(yùn)動(dòng)狀態(tài)方程合測(cè)量方程出現(xiàn)較嚴(yán)重的非線性時(shí)，由該算法生成的濾波器比無跡卡爾曼濾波算法(UKF)、迭代擴(kuò)展卡爾曼濾波(IEKF)有更高的準(zhǔn)確率，并通過實(shí)驗(yàn)證明了它的有效性。新濾波器比較容易實(shí)現(xiàn)且具有較高的準(zhǔn)確估計(jì)性，比另外兩種算法在非線性方面得到更多的應(yīng)用。