非線性離散系統(tǒng)基于擴(kuò)展Kalman濾波的魯棒融合狀態(tài)估計(jì)

馬光鵬,孫小君,2,*

(1.黑龍江大學(xué) 電子工程學(xué)院,哈爾濱 150080;2.黑龍江省信息融合估計(jì)與檢測重點(diǎn)實(shí)驗(yàn)室,哈爾濱 150080)

0 引 言

眾所周知，在實(shí)際生活中非線性環(huán)節(jié)普遍存在，尤其是在各種定位系統(tǒng)中。通常狀態(tài)方程是建立在笛卡爾坐標(biāo)下的，觀測方程是建立在極坐標(biāo)下的，這使整個(gè)定位系統(tǒng)呈現(xiàn)一種非線性特質(zhì)。處理這些非線性環(huán)節(jié)的方法為線性逼近。

多傳感器信息融合[1]也稱多傳感器數(shù)據(jù)融合，是20世紀(jì)70年代以來由于戰(zhàn)爭及高科技領(lǐng)域的迫切需要發(fā)展起來的一門新興邊緣學(xué)科，目前已成為備受人們關(guān)注的熱門領(lǐng)域[2]。第二次世界大戰(zhàn)期間，人們就已經(jīng)把兩傳感器數(shù)據(jù)融合應(yīng)用于火炮系統(tǒng)，增強(qiáng)了系統(tǒng)抗干擾能力，目前國內(nèi)外已經(jīng)提出了諸多算法，有在最小二乘法和最小方差意義下的集中式分布和混合式融合算法[3-5]，極大似然最優(yōu)信息融合準(zhǔn)則[6]，加權(quán)觀測融合估計(jì)算法[7]等，但是非線性系統(tǒng)的融合算法結(jié)構(gòu)和估計(jì)性能很大程度上取決于所采用的局部濾波方法，如擴(kuò)展 Kalman 濾波 (Extend Kalman fifilter, EKF)[8]、無跡濾波 (Unscented Kalman fifilter,UKF)[9]、粒子濾波 (Particle fifilter, PF)[10-12]和求容積濾波 (Cubature Kalman fifilter, CKF)[13]等。基于不同的局部非線性濾波器所獲得同類融合算法的性能也存在差別。EKF算法有很多缺點(diǎn)和不足[14-19]，它的近似線性化方法就是略去二階以上Taylor級數(shù)展開式，大量信息被略去，這導(dǎo)致估計(jì)結(jié)果產(chǎn)生較大偏差。將針對這一問題進(jìn)行研究。選擇的CI融合算法不需要求解局部估計(jì)誤差互協(xié)方差。提出的多傳感器非線性系統(tǒng)的CI融合Kalman濾波算法將提高多傳感器非線性系統(tǒng)的狀態(tài)估計(jì)精度。仿真實(shí)驗(yàn)結(jié)果表明，所提出的非線性系統(tǒng)融合狀態(tài)估計(jì)方法有效可行。

1 問題描述

離散非線性系統(tǒng)動態(tài)方程可以表示為

X(k+1)=f[X(k),k]+G(k)W(k)

(1)

Zi(k)=h[X(k),k]+Vi(k)，i=1,…,L

(2)

當(dāng)過程噪聲W(k)和觀測噪聲Vi(k)恒為零時(shí)，系統(tǒng)模型的解為非線性模型的理論解，又稱為“標(biāo)稱軌跡”，而把非線性系統(tǒng)的真實(shí)解稱為“真軌跡”。

假設(shè)1過程噪聲W(k)是均值為零的高斯白噪聲，且矩陣G(k)是已知的。

假設(shè)2 觀測噪聲Vi(k)是均值為零的高斯白噪聲，并且過程噪聲W(k)和觀測噪聲Vi(k)相互獨(dú)立。

2 局部擴(kuò)展Kalman濾波器

(3)

X(k+1)=Φi(k)X(k)+G(k)W(k)+φi(k)

(4)

(5)

Zi(k)=Hi(k)X(k)+yi(k)+Vi(k)

(6)

由射影定理可知

(7)

(8)

對(5)式求射影定理得

(9)

引出新息表達(dá)式

(10)

記濾波和預(yù)報(bào)估值誤差及方差陣為

(11)

(12)

(13)

(14)

則由式(6)和式(9)可得

(15)

由式(4)和式(7)有

(16)

(17)

由式(16)得到

(18)

因?yàn)?/p>

(19)

有

E[εi(k+1)X(i)T(k+1|k)]=0

(20)

于是由式(15)得新息方差陣為

(21)

根據(jù)式(17)得

(22)

因?yàn)?/p>

(23)

因?yàn)樯溆岸ɡ碚恍?/p>

(24)

(25)

(26)

現(xiàn)在用Ki(k+1)的表達(dá)式簡化Pi(k+1|k+1)得

Pi(k+1|k+1)=[In-Ki(k+1)Hi]Pi(k+1|k)

(27)

這是根據(jù)線性Kalman濾波基本方程[20]得到的擴(kuò)展Kalman濾波遞推方程[21-23]。根據(jù)上述推理則遞推擴(kuò)展Kalman濾波器如下：

定理1多傳感器非線性系統(tǒng)(1)和(2)，在假設(shè)1和假設(shè)2下，有局部擴(kuò)展Kalman濾波器

(28)

(29)

(30)

(31)

(32)

Pi(k+1|k+1)=[In-Ki(k+1)Hi]Pi(k+1|k)

(33)

狀態(tài)轉(zhuǎn)移矩陣和觀測矩陣由f和h的雅可比矩陣代替。假設(shè)狀態(tài)變量有n維，即X=[x1x2…xn]T，則相應(yīng)的雅可比矩陣的求法[24]如下：

(34)

3 魯棒CI融合估計(jì)算法

(35)

(36)

注意0≤ω≤1，則有

(37)

引出u∈R，故R?RP1?RP2成立，注意

(38)

(39)

則R就是PCI(k+1|k+1)的協(xié)方差橢圓，即

(40)

(41)

(42)

(43)

(44)

(45)

將式(42)代入式(43)和式(44)得到CI融合擴(kuò)展Kalman濾波算法。通過上述文字描述和公式推理得到的CI融合算法如下：

定理2多傳感器非線性系統(tǒng)(1)和(2)，在假設(shè)1和假設(shè)2下，有CI融合擴(kuò)展Kalman濾波器

(46)

(47)

4 仿真模型和結(jié)果分析

實(shí)驗(yàn)結(jié)果表明，當(dāng)系統(tǒng)存在非線性環(huán)節(jié)時(shí)，使用局部線性化擴(kuò)展Kalman濾波時(shí)有較大的誤差，而在擴(kuò)展Kalman濾波基礎(chǔ)上在進(jìn)行CI融合估計(jì)可以有效地降低系統(tǒng)誤差，相較其他觀測融合估計(jì)計(jì)算量較小且提高了多傳感器的系統(tǒng)狀態(tài)估計(jì)的精度。

考慮非線性阻尼諧振子模型[26]，設(shè)X為二維向量，狀態(tài)分量分別為位置(m)、速度(m/s)即X(k)=[x1(k),x2(k)]T，相應(yīng)的兩傳感器非線性離散系統(tǒng)為

X(k)=f(X(k-1))+ΓW(k-1)

(48)

Zi(k)=HiX(k)+Vi(k),i=1,2

(49)

狀態(tài)的真實(shí)值與局部和CI融合擴(kuò)展Kalman濾波器的對比曲線見圖1。由圖1可見，CI融合有效提高了估計(jì)精度。

CI融合最優(yōu)參數(shù)見圖2，CI融合和局部濾波器的精度關(guān)系仿真見圖3。在仿真程序的調(diào)試過程中，發(fā)現(xiàn)選取的系統(tǒng)參數(shù)不同時(shí)，圖3的精度關(guān)系也會出現(xiàn)融合估計(jì)精度低壓局部某個(gè)融合估計(jì)的情況，這也正是CI融合的魯棒性所在。

圖1 狀態(tài)X(k)=[x1(k),x2(k)]T的真實(shí)值與局部和CI融合擴(kuò)展Kalman濾波器的對比曲線Fig.1 Comparison curve between the true value of X(k) and the local CI fusion extended Kalman filter

圖2 參數(shù)ω的仿真Fig.2 Simulation diagram of parameter ω

圖3 CI融合和局部濾波器的精度關(guān)系仿真Fig.3 Simulation diagram of the accuracy relation ship between CI fusion and local filter

5 結(jié) 論

本文對多傳感器非線性離散系統(tǒng)提出了魯棒CI融合擴(kuò)展Kalman濾波算法。通過兩傳感器二維系統(tǒng)仿真實(shí)驗(yàn)表明，引入魯棒CI信息融合算法提出的CI融合擴(kuò)展Kalman濾波器有效提高了多傳感器非線性系統(tǒng)的狀態(tài)估計(jì)精度，且算法形式簡單，便于工程應(yīng)用。