趙 巖，王 寧，韋道知

(空軍工程大學(xué) 防空反導(dǎo)學(xué)院， 陜西 西安 710051)

狀態(tài)估計(jì)是從已有信息中提取有用信息[1]，并對(duì)系統(tǒng)模型的某些參數(shù)進(jìn)行校正的方法。狀態(tài)估值的精度和可靠性與系統(tǒng)模型是否準(zhǔn)確描述直接相關(guān)[2]。實(shí)際系統(tǒng)的非線性模型可以有效還原系統(tǒng)的真實(shí)性，但過(guò)于復(fù)雜的模型，計(jì)算大，甚至難以計(jì)算[3]。因此，系統(tǒng)模型應(yīng)對(duì)實(shí)際系統(tǒng)進(jìn)行適當(dāng)簡(jiǎn)化，降低系統(tǒng)的非線性程度。與此同時(shí)，系統(tǒng)不可避免地引入了一定干擾。

目前，常用的非線性系統(tǒng)狀態(tài)估計(jì)方法有無(wú)跡卡爾曼濾波(Unscented Kalman Filtering, UKF)方法、粒子濾波(Particle Filtering, PF)方法以及相關(guān)改進(jìn)方法。UKF通過(guò)Unscented變換，獲得狀態(tài)一步預(yù)測(cè)的一階、二階統(tǒng)計(jì)信息，能夠?qū)^高非線性度系統(tǒng)進(jìn)行有效估計(jì)，估值精度達(dá)到二階以上[4]。但UKF方法對(duì)初始噪聲相對(duì)敏感，不準(zhǔn)確的初始誤差模型會(huì)嚴(yán)重影響UKF方法的濾波性能[5-7]。PF方法能夠處理強(qiáng)非線性系統(tǒng)[8]，PF方法利用蒙特卡洛方法，隨機(jī)產(chǎn)生一組含有權(quán)值的粒子，近似狀態(tài)的后驗(yàn)概率分布。但PF方法也會(huì)面臨粒子匱乏等問(wèn)題，導(dǎo)致?tīng)顟B(tài)估值陷入局部最優(yōu)。這些非線性方法都存在計(jì)算量大、實(shí)時(shí)性相對(duì)較差的缺陷。對(duì)于線性系統(tǒng)，卡爾曼濾波(Kalman Filtering, KF)算法無(wú)疑是最實(shí)用和成熟的方法，廣泛應(yīng)用于航空航天、工業(yè)控制、通信、經(jīng)濟(jì)金融等領(lǐng)域。為了在非線性系統(tǒng)中應(yīng)用KF算法，通過(guò)改進(jìn)得到擴(kuò)展卡爾曼濾波(Extended Kalman Filtering, EKF)算法[9]，該方法利用Taylor級(jí)數(shù)，將非線性系統(tǒng)線性化，然后進(jìn)行KF算法迭代，能夠獲得非線性系統(tǒng)的狀態(tài)估值。

除了系統(tǒng)模型，系統(tǒng)受到的干擾也是影響狀態(tài)估值精度和可靠性的一個(gè)重要因素[10]。一般的方法是將干擾(包括系統(tǒng)內(nèi)部和外部干擾)轉(zhuǎn)換成隨機(jī)高斯噪聲(Stochastic Gaussian Noise, SGN)的形式。但實(shí)際系統(tǒng)的干擾往往相當(dāng)復(fù)雜[11]，單一的SGN并不能較好地模擬系統(tǒng)干擾。受技術(shù)條件限制，早期濾波難以處理非高斯噪聲(Non-Gaussian Noise, NGN)，但隨著濾波技術(shù)的發(fā)展，產(chǎn)生了多種處理不同形式噪聲的方法。

文獻(xiàn)[12]研究了含有互相關(guān)噪聲的多傳感器系統(tǒng)，利用全局最優(yōu)分布式算法對(duì)狀態(tài)進(jìn)行估計(jì)，并將設(shè)計(jì)方法應(yīng)用到目標(biāo)跟蹤模型中進(jìn)行仿真，驗(yàn)證了方法的估計(jì)效果。文獻(xiàn)[13]利用濾波技術(shù)研究了含有自回歸噪聲的雙線性系統(tǒng)的參數(shù)估計(jì)問(wèn)題，根據(jù)辨識(shí)后的模型采用改進(jìn)最小二乘迭代方法獲得了有效估計(jì)值。文獻(xiàn)[14]通過(guò)系統(tǒng)辨識(shí)處理含有未知有色噪聲的雙線性系統(tǒng)，再利用KF算法對(duì)系統(tǒng)參數(shù)進(jìn)行估計(jì)，通過(guò)數(shù)字仿真證實(shí)了這種方法的有效性。文獻(xiàn)[15]為了克服KF算法缺陷，采用最大相關(guān)熵準(zhǔn)則對(duì)KF算法進(jìn)行改進(jìn)，并對(duì)非高斯噪聲系統(tǒng)進(jìn)行狀態(tài)估計(jì)。文獻(xiàn)[16]采用一種回歸方法對(duì)未知統(tǒng)計(jì)特性的有界噪聲系統(tǒng)進(jìn)行參數(shù)估計(jì)。上述文獻(xiàn)利用不同方法對(duì)線性和非線性系統(tǒng)的參數(shù)進(jìn)行估計(jì)，都將干擾噪聲按照同一形式進(jìn)行處理，沒(méi)有考慮系統(tǒng)噪聲的異質(zhì)性對(duì)濾波帶來(lái)的影響。

綜上，本文針對(duì)基于KF框架算法在處理NGN系統(tǒng)時(shí)的局限性，提出一種能夠同時(shí)處理含有隨機(jī)高斯噪聲和未知有界噪聲兩種異類噪聲的改進(jìn)算法。該方法利用最小均方誤差估計(jì)原則，在吸收集員估計(jì)處理未知有界噪聲能力的基礎(chǔ)上，調(diào)節(jié)卡爾曼濾波增益，使改進(jìn)后的算法具有處理非高斯噪聲的能力。

1 KF框架及對(duì)噪聲的要求

1.1 KF框架

KF算法是針對(duì)線性系統(tǒng)狀態(tài)的一種有效估計(jì)方法。通過(guò)賦予狀態(tài)量和協(xié)方差矩陣初值，利用不斷更新的觀測(cè)量，獲得下一時(shí)刻的狀態(tài)估值。但采用標(biāo)準(zhǔn)KF算法計(jì)算系統(tǒng)準(zhǔn)確的狀態(tài)估值是有嚴(yán)格要求的。首先，系統(tǒng)是線性的，而在現(xiàn)實(shí)中，幾乎不存在真正的線性系統(tǒng)。然后，系統(tǒng)模型精確已知，而實(shí)際建立的系統(tǒng)模型總存在不同程度的誤差和干擾，很難給出準(zhǔn)確的模型。另外，對(duì)系統(tǒng)噪聲也有一定的要求。但是，KF算法是一種遞推算法，特別適合計(jì)算機(jī)應(yīng)用，被廣泛應(yīng)用于航空航天、工業(yè)控制、通信、經(jīng)濟(jì)金融等領(lǐng)域。

標(biāo)準(zhǔn)KF算法的計(jì)算流程如圖1所示。KF框架是根據(jù)KF算法流程，利用狀態(tài)量的一、二階矩統(tǒng)計(jì)信息，代入觀測(cè)信息后，通過(guò)狀態(tài)方程和觀測(cè)方程，實(shí)現(xiàn)時(shí)間更新和量測(cè)更新的過(guò)程。采用KF框架的濾波算法主要有KF算法、EKF算法及其改進(jìn)算法。

圖1 標(biāo)準(zhǔn)卡爾曼濾波算法流程Fig.1 Flow chart of the standard Kalman filtering algorithm

EKF算法是KF算法在非線性領(lǐng)域的推廣。采用KF框架，通過(guò)將非線性模型進(jìn)行Taylor展開(kāi)，對(duì)二階以上項(xiàng)(含二階項(xiàng))進(jìn)行截?cái)?，獲得狀態(tài)矩陣和觀測(cè)矩陣的Jacobian矩陣，然后再利用KF框架進(jìn)行計(jì)算。雖然線性化過(guò)程產(chǎn)生了誤差，但是解決了KF算法在非線性系統(tǒng)中的應(yīng)用問(wèn)題。對(duì)于非線性度較大的系統(tǒng)模型，采用EKF算法進(jìn)行計(jì)算會(huì)導(dǎo)致?tīng)顟B(tài)估值發(fā)散。

1.2 基于KF框架的算法對(duì)系統(tǒng)噪聲的要求

基于KF框架的濾波算法對(duì)系統(tǒng)的噪聲有嚴(yán)格的要求。

當(dāng)噪聲是已知的高斯噪聲時(shí)，基于KF框架的算法是一種遞推最小均方誤差(Mean Square Error, MSE)估計(jì)準(zhǔn)則。當(dāng)噪聲分布為非高斯、且前二階矩已知時(shí)，該估計(jì)器為最優(yōu)線性估計(jì)器。但在實(shí)際情況中，系統(tǒng)噪聲分布具有不確定性，且統(tǒng)計(jì)信息難以準(zhǔn)確獲得，因此很難滿足對(duì)噪聲的這一要求[17]。盡管基于KF框架的算法在實(shí)際應(yīng)用的條件很苛刻，但適合計(jì)算機(jī)運(yùn)算，且計(jì)算量相對(duì)較小。因此，將實(shí)際模型簡(jiǎn)化后，KF算法仍然是目前應(yīng)用最為廣泛的、獲得次優(yōu)狀態(tài)估值的高效方法。

對(duì)于某一系統(tǒng)模型，采用基于KF框架的濾波算法進(jìn)行解算時(shí)，系統(tǒng)的過(guò)程噪聲Wk和觀測(cè)噪聲Vk應(yīng)同時(shí)滿足：

(1)

式中：Qk為系統(tǒng)噪聲序列的方差陣，Rk為觀測(cè)噪聲序列的方差陣，且Qk與Rk相互獨(dú)立；δkj為Kronecher函數(shù)。

2 集員濾波及未知有界噪聲

如果系統(tǒng)噪聲分布不能滿足高斯分布，基于KF框架的濾波算法性能會(huì)顯著下降，甚至發(fā)散。在實(shí)際應(yīng)用中，大多數(shù)噪聲是不滿足這種假設(shè)的，有的噪聲甚至難以用數(shù)學(xué)語(yǔ)言描述。

針對(duì)這類隨機(jī)噪聲，為了獲得更加準(zhǔn)確、可靠的系統(tǒng)狀態(tài)估值，集員濾波(Set Membership Filtering, SMF)算法進(jìn)入了研究人員的視野。SMF算法是一種在未知但有界(Unknown But Bounded, UBB)噪聲假設(shè)下的估計(jì)方法，其估計(jì)目的是尋求所有與模型結(jié)構(gòu)、觀測(cè)數(shù)據(jù)和噪聲有界假設(shè)相容的狀態(tài)或參數(shù)組成的可行集合(Feasible Set, FS)[16]。通過(guò)該方法得到的估計(jì)結(jié)果不是一個(gè)準(zhǔn)確值，而是一個(gè)集合，即可行集。狀態(tài)參數(shù)可行集中每個(gè)元素都是系統(tǒng)有效的狀態(tài)估值。一般將可行集的中心作為狀態(tài)的點(diǎn)估計(jì)，可行集的測(cè)度作為衡量SMF算法有效性的標(biāo)準(zhǔn)。

2.1 集員濾波的系統(tǒng)噪聲

如前，集員濾波器是將UBB噪聲轉(zhuǎn)換到橢球集合中，用橢球集合表示這些參數(shù)的可能分布。

定義1一個(gè)橢球集合可表示為：

E(a,P,σ)={x∈Rn:(x-a)T(P)-1(x-a)≤σ2}

(2)

式中：a為橢球E(a,P,σ)的中心；P表示橢球E(a,P,σ)的形狀矩陣，且為正定矩陣；σ為橢球E(a,P,σ)半徑。

對(duì)于系統(tǒng)的UBB過(guò)程噪聲wk和觀測(cè)噪聲vk，用橢球集合可以表示為：

(3)

2.2 集員濾波框架

SMF算法分為時(shí)間更新和量測(cè)更新兩步，如圖2所示。時(shí)間更新即通過(guò)前一時(shí)刻狀態(tài)橢球和由狀態(tài)方程獲得的橢球(含過(guò)程噪聲橢球集合)進(jìn)行Minkowski和計(jì)算，得到的狀態(tài)一步預(yù)測(cè)橢球集合的過(guò)程。量測(cè)更新即把得到的狀態(tài)一步預(yù)測(cè)橢球和觀測(cè)方程橢球(含觀測(cè)噪聲橢球集合)進(jìn)行交集計(jì)算，得到當(dāng)前時(shí)刻狀態(tài)橢球集合的過(guò)程。

圖2 集員濾波算法流程Fig.2 Flow chart of the set-membership filtering

狀態(tài)xk-1屬于橢球集合E(k-1)，表示為：

(4)

狀態(tài)一步預(yù)測(cè)xk,k-1的集合為：

xk,k-1∈Fk-1E(k-1)+Wk-1

(5)

式中，F(xiàn)k為系統(tǒng)的狀態(tài)矩陣，Wk-1為過(guò)程噪聲橢球集合。

定義2兩個(gè)橢球集合E(a1,P1,σ1)與E(a2,P2,σ2)的Minkowski和包含于的外定界橢球集合為：

E(a,P,σ)?E(a1,P1,σ1)+E(a2,P2,σ2)

(6)

式中：

其中：p的值需要根據(jù)選擇最小化橢球集合的方法而定。常用的方法有最小容積橢球和最小跡橢球[18]，但無(wú)論采用哪種最小化橢球的方式，σ2最終可被消除。式(6)可最終表示為：

E(a,P*,1)?E(a1,P1,σ1)+E(a2,P2,σ2)

(7)

式中：

(8)

橢球集合Fk-1E(k-1)和過(guò)程噪聲橢球集合Wk-1的Minkowski和一般不為橢球集合，為使xk,k-1包含于一個(gè)標(biāo)準(zhǔn)的橢球集合，根據(jù)定義2，狀態(tài)一步預(yù)測(cè)xk,k-1的集合為：

xk,k-1∈E(k,k-1)?Fk-1E(k-1)+Wk-1

(9)

觀測(cè)橢球集合為：

(10)

式中，Hk為系統(tǒng)的觀測(cè)矩陣，Vk觀測(cè)噪聲橢球集合。

量測(cè)更新橢球集合為：

xk∈E(k)=E(k,k-1)∩S(k)

(11)

定義3兩個(gè)橢球集合E(a1,P1,σ1)與E(a2,P2,σ2)的交集包含于的外包界橢球集合為：

E(a,P,σ)=E(a1,P1,σ1)∩E(a2,P2,σ2)

(12)

式中：

根據(jù)式(12)，式(11)可表示為：

(13)

3 基于KF框架的優(yōu)化增益異類噪聲處理方法

針對(duì)EKF算法在處理非線性系統(tǒng)時(shí)，受到系統(tǒng)模型準(zhǔn)確性和高斯噪聲模型已知的要求限制，設(shè)計(jì)一種基于KF框架的異類噪聲處理方法。首先區(qū)分系統(tǒng)中的異類噪聲，然后對(duì)濾波增益進(jìn)行優(yōu)化，最后給出改進(jìn)算法。

3.1 異類噪聲

模型是真實(shí)系統(tǒng)的高度抽象，建立與實(shí)際非線性系統(tǒng)一致的數(shù)學(xué)模型是極其困難的。即使能夠根據(jù)物理特性構(gòu)建系統(tǒng)的模型，但系統(tǒng)仍然會(huì)受到外界環(huán)境的未知干擾。其中有些干擾能夠獲得統(tǒng)計(jì)分布，而大多數(shù)干擾難以獲得。加之非線性系統(tǒng)線性化的截?cái)嗾`差，使得EKF算法對(duì)非線性系統(tǒng)狀態(tài)進(jìn)行估計(jì)時(shí)，無(wú)法獲得狀態(tài)的最優(yōu)估值。因此，將系統(tǒng)無(wú)法準(zhǔn)確描述的線性化截?cái)嗾`差、外界未知干擾和系統(tǒng)噪聲進(jìn)行處理，轉(zhuǎn)變成UBB噪聲，與系統(tǒng)中可以描述的高斯噪聲共同構(gòu)成本文研究異類噪聲的主體。因此對(duì)系統(tǒng)可能出現(xiàn)的異類噪聲進(jìn)行分類，如表1所示。

表1 系統(tǒng)異類噪聲分類Tab.1 Classification of the system heterogeneous noise

3.2 濾波增益優(yōu)化

通過(guò)調(diào)整濾波增益，使EKF算法能夠同時(shí)處理這兩類異類噪聲。

引理1若存在矩陣A、B和X，則下列矩陣函數(shù)的跡對(duì)X求導(dǎo)的等式成立。

(14)

定理1若EKF算法的濾波增益Kk滿足

(15)

證明：

假設(shè)系統(tǒng)狀態(tài)估值為：

(16)

類似地，假設(shè)觀測(cè)量為：

(17)

則狀態(tài)的MSE為：

(18)

存在

(19)

根據(jù)標(biāo)準(zhǔn)KF狀態(tài)估值方程，得到：

(20)

計(jì)算MSE，并將式(20)代入，則：

(21)

再將式(17)代入式(21)，可得：

(22)

(23)

并將式(23)代入式(22)，得到：

(24)

(25)

式(25)右邊第三項(xiàng)利用式(7)可得：

?E(0,Pk(p),1)

(26)

根據(jù)式(8)可得：

(27)

因?yàn)?/p>

(28)

所以

(29)

根據(jù)式(18)和式(25)可知：

(30)

(31)

(32)

□

3.3 改進(jìn)算法

綜上，基于KF框架的優(yōu)化增益異類噪聲處理方法如式(33)所示。

(33)

4 數(shù)字仿真實(shí)驗(yàn)

4.1 仿真系統(tǒng)模型

建立粗糙的車(chē)輛運(yùn)動(dòng)模型作為導(dǎo)航系統(tǒng)的狀態(tài)方程。

xk=Fk-1xk-1+Wk-1

(34)

其中，sE,sN,vE,vN,aE,aN分別表示東向位置、北向位置、東向速度、北向速度、東向加速度和北向加速度，t為采樣時(shí)間。

zk=hk(xk)+vk

(35)

其中：zk=[d,θ]T；d,θ分別為里程計(jì)輸出的距離信息和方位陀螺輸出的方位角信息；hk為非線性觀測(cè)函數(shù)，其Hessian矩陣為Hk。

4.2 仿真結(jié)果分析

采用本文設(shè)計(jì)的濾波方法和EKF算法對(duì)4.1節(jié)中的車(chē)輛導(dǎo)航模型進(jìn)行仿真，經(jīng)處理得到東向、北向的位置和速度誤差結(jié)果，如圖3～8所示。

圖3和圖4為兩種濾波算法對(duì)系統(tǒng)位置誤差的估計(jì)曲線。由于前30個(gè)歷元兩種算法估值接近，為了更加清楚地比較位置誤差估計(jì)結(jié)果，分別在圖3和圖4左下角繪制出前30個(gè)歷元東向和北向的位置誤差估計(jì)曲線。由圖3和圖4可知，EKF算法可將位置誤差均值控制在15 m以內(nèi)，且估計(jì)精度逐漸變差；本文算法具有較穩(wěn)定的估計(jì)精度，能夠使位置誤差均值控制在4 m以內(nèi)。造成這樣的原因是里程計(jì)和陀螺儀傳感器的誤差積累使算法計(jì)算精度有所下降，而本文算法具有較好的克服系統(tǒng)誤差的能力。

圖3 東向位置誤差曲線Fig.3 Eastern position error curves

圖4 北向位置誤差曲線Fig.4 Northern position error curves

圖5和圖6為兩種濾波算法對(duì)系統(tǒng)速度誤差的估計(jì)曲線。同樣，分別在圖5和圖6左下角繪制出前30個(gè)歷元東向和北向的速度誤差估計(jì)曲線。由圖5和圖6可知，EKF算法可將速度誤差均值控制在3 m/s以內(nèi)，且估計(jì)精度逐漸變差；本文算法具有較穩(wěn)定的估計(jì)精度，能夠使速度誤差均值控制在0.3 m/s以內(nèi)。

圖5和圖8為50次蒙特卡洛的后輸出的位置和速度的均方誤差曲線，從圖中可以看出，本文提出的基于異類噪聲處理機(jī)制的KF算法精度優(yōu)于EKF算法的計(jì)算精度。仿真得到的位置和速度誤差的均值和標(biāo)準(zhǔn)差如表2所示。本文算法的狀態(tài)估值均值和標(biāo)準(zhǔn)差均小于EKF算法的計(jì)算結(jié)果。EKF算法的計(jì)算時(shí)間為0.165 s，本文算法的計(jì)算時(shí)間為0.132 s。算法耗時(shí)是在Windows 10系統(tǒng)主頻2.2 GHz處理器條件下，由MATLAB R2015a軟件計(jì)算得到的。通過(guò)比較可以看出，兩種算法的計(jì)算量相當(dāng)，本文算法的計(jì)算時(shí)間略短于EKF算法。因此，本文算法的估計(jì)精度高于EKF算法，且具有更好的抗誤差擾動(dòng)能力，一定程度上能夠提高車(chē)輛導(dǎo)航定位的精度。

圖5 東向速度誤差曲線Fig.5 Eastern speed error curves

圖6 北向速度誤差曲線Fig.6 Northern speed error curves

圖7 位置均方誤差Fig.7 Position of the RMSE

圖8 速度均方誤差Fig.8 Speed of the RMSE

表2 水平位置誤差比較Tab.2 Horizontal position error compare

5 結(jié)論

本文提出一種基于卡爾曼濾波框架的優(yōu)化異類噪聲處理算法，在吸收集員濾波處理UBB噪聲優(yōu)點(diǎn)的基礎(chǔ)上，通過(guò)調(diào)整濾波增益使系統(tǒng)狀態(tài)估值均方誤差達(dá)到最小，且能夠同時(shí)處理含有高斯噪聲和未知有界噪聲的系統(tǒng)。并將該算法和EKF算法應(yīng)用到車(chē)輛導(dǎo)航系統(tǒng)模型中，在給定相同噪聲的條件下，當(dāng)UBB噪聲上限相對(duì)較小時(shí)，通過(guò)數(shù)字仿真實(shí)驗(yàn)發(fā)現(xiàn)本文方法對(duì)系統(tǒng)噪聲且具有更好的抗誤差擾動(dòng)能力。