基于冗余偽觀測的自適應(yīng)濾波算法

蔣劉洋，張 海

(北京航空航天大學(xué)自動化科學(xué)與電氣工程學(xué)院，北京 100191)

0 引言

經(jīng)典的卡爾曼濾波要求在確定的系統(tǒng)模型和觀測方程下，對系統(tǒng)噪聲和量測噪聲的統(tǒng)計特性具有準(zhǔn)確的先驗知識[1]，因此噪聲分布在卡爾曼濾波器的狀態(tài)估計中起著非常重要的作用。然而在多數(shù)實際應(yīng)用中，噪聲的統(tǒng)計特性通常是未知的，往往被經(jīng)驗值替代；此外還會因為受到環(huán)境等因素的影響而發(fā)生改變。上述情況都會使預(yù)設(shè)的噪聲方差不能準(zhǔn)確地反映當(dāng)前系統(tǒng)模型的隨機噪聲特性，導(dǎo)致估計精度下降，甚至濾波器發(fā)散。自適應(yīng)濾波是一種具有抑制濾波器發(fā)散作用的方法，其在濾波計算過程中，同時對未知的或不確切的系統(tǒng)模型參數(shù)和噪聲統(tǒng)計參數(shù)進行估計和修正。自適應(yīng)濾波方法有很多，從濾波器結(jié)構(gòu)的角度劃分，大體上可分為兩類：基于多模型的自適應(yīng)估計(Multiple Model Adaptive Estimation，MMAE)和基于新息/殘差的自適應(yīng)估計(Innovation-based Adaptive Estimation，IAE/Residual-based Adaptive Estima-tion，RAE)[2-3]。

MMAE是由Magill于1965年提出的[4]，當(dāng)系統(tǒng)模型未知或不確定時，該方法仍能給出較準(zhǔn)確的狀態(tài)估計結(jié)果。標(biāo)準(zhǔn)的MMAE濾波器同時運行一組卡爾曼濾波器，其中的每個濾波器使用不同的系統(tǒng)動態(tài)模型或隨機噪聲模型，然后基于Bayes方法通過新息或殘差計算每個模型是正確模型的概率，最后對每個濾波器的輸出結(jié)果做加權(quán)和得到最優(yōu)的狀態(tài)估計[5-6]。在實際應(yīng)用中，當(dāng)系統(tǒng)模型由于某些未知原因發(fā)生變化時，該方法仍能給出較高精度的狀態(tài)估計結(jié)果，因此，故障診斷是其最具潛力的應(yīng)用領(lǐng)域之一[7-8]。

IAE/RAE方法是對經(jīng)典卡爾曼濾波方法的一個擴展，通過額外的程序?qū)^程噪聲方差矩陣與觀測噪聲方差矩陣進行自適應(yīng)估計和調(diào)節(jié)，使其不斷地適應(yīng)系統(tǒng)的變化[9]。近年來，IAE/RAE算法取得了很多新的進展：文獻[10]提出了一種使用新息向量及其滯后時刻向量的自協(xié)方差函數(shù)來估計過程噪聲方差矩陣與觀測噪聲方差矩陣的最小二乘回歸法。文獻[11]提出了一種具有魯棒性的抗差自適應(yīng)卡爾曼濾波(Robust Adaptive Kalman Filtering，RAKF)算法，RAKF利用一個自適應(yīng)因子來調(diào)節(jié)狀態(tài)預(yù)測誤差的協(xié)方差矩陣。該自適應(yīng)因子是通過比較實際的新息協(xié)方差矩陣與其對應(yīng)的理論值來計算獲得的，且其值大于1，故RAKF為新的量測值提供了更多的權(quán)重，增強了濾波器的魯棒性。文獻[12]基于Sage-Husa自適應(yīng)濾波,引入改進的時變噪聲估計器實時估計噪聲的統(tǒng)計特性。此外，文獻[13]基于新息和殘差的相關(guān)性推導(dǎo)出了一個包含過程噪聲協(xié)方差的線性矩陣方程，從而實時解決過程噪聲方差矩陣。

值得注意的是，上述幾種算法都是基于新息和殘差向量完成的自適應(yīng)調(diào)節(jié)，但是作為數(shù)據(jù)基礎(chǔ)的新息/殘差向量耦合了狀態(tài)量的估計誤差。因此，當(dāng)狀態(tài)估計誤差較大時，調(diào)節(jié)因子的計算會受到影響，進一步影響到系統(tǒng)噪聲方差陣或狀態(tài)誤差協(xié)方差陣的估計和調(diào)節(jié)，從而導(dǎo)致濾波器輸出的不準(zhǔn)確甚至發(fā)散[14]。

為了克服這一問題，有學(xué)者提出了基于冗余測量的二階互差分(Second Order Mutual Difference，SOMD)算法來估計觀測噪聲方差矩陣[15-16]。該算法的主要思想是利用一個觀測序列作為參考來估計另一個觀測序列的觀測噪聲方差矩陣，獲得的噪聲方差矩陣只與測量系統(tǒng)自身相關(guān)，而與濾波系統(tǒng)的其余參數(shù)無關(guān)，因此與狀態(tài)估計誤差解耦。在此基礎(chǔ)上，文獻[17]研究了同時估計兩種觀測系統(tǒng)噪聲方差矩陣的算法。然而，由于成本的約束，多數(shù)實際系統(tǒng)不能滿足冗余測量的前提條件，限制了該算法的應(yīng)用。

考慮到SOMD算法的優(yōu)越性，為了避免冗余測量條件的限制，本文通過構(gòu)建偽測量序列，將SOMD算法擴展到只具有單一測量系統(tǒng)的應(yīng)用中，使其適用于更廣泛的應(yīng)用場景。

1 SOMD算法簡介

本節(jié)介紹了一種基于冗余觀測利用SOMD序列估計觀測噪聲方差陣的算法[15]，其主要思想是當(dāng)被觀測對象存在2個不同的觀測系統(tǒng)時，根據(jù)測量序列的變化關(guān)系，分別計算2個序列的一階自差分序列以及SOMD序列，并通過統(tǒng)計的方法獲得觀測系統(tǒng)的噪聲統(tǒng)計特性。

針對如下描述的線性隨機離散系統(tǒng)

Xk=Φk,k-1Xk-1+Wk-1
Zk=HkXk+Vk

(1)

式中，Xk為系統(tǒng)狀態(tài)；Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣；Zk為量測量；Hk為量測矩陣。Wk-1為過程噪聲，Vk為量測噪聲，且有

(2)

式中，E[]為數(shù)學(xué)期望算子；δkj為Kronecker-δ函數(shù)。

SOMD算法具體如下：

(3)

構(gòu)造2個測量系統(tǒng)的一階自差分序列以及SOMD序列如下

(4)

如果2個測量系統(tǒng)的精度存在極差，即

(5)

那么方差較大的噪聲方差為

(6)

SOMD算法主要有如下特點：

1)測量系統(tǒng)的確定性誤差通常在短時間內(nèi)比較穩(wěn)定，通過求取一階自差分序列可將其消除。

2)被測對象在短時間內(nèi)有可能存在狀態(tài)突變的現(xiàn)象，在一階自差分序列中不能被消除，但是SOMD序列可將其完全抵消掉，進而只保留隨機噪聲項。這也是該方法最突出的特點。

3)與傳統(tǒng)的自適應(yīng)濾波方案不同，SOMD算法將噪聲估計作為一個獨立的問題進行研究，脫離了對濾波系統(tǒng)的依賴，實現(xiàn)噪聲估計與濾波狀態(tài)估計的解耦。因此，該方法對噪聲特性估計的魯棒性更強。

但是，定理1要求具有冗余測量系統(tǒng)，并且2個測量系統(tǒng)的精度之間存在極差，限制了其應(yīng)用。若要將定理1擴展到單一測量系統(tǒng)，必須提供一個額外的觀測系統(tǒng)作為冗余觀測，并且確保其噪聲是與直接觀測系統(tǒng)的測量噪聲不相關(guān)的白噪聲。

2 基于偽觀測的SOMD算法

2.1 基于偽觀測的噪聲方差矩陣的估計

在本節(jié)中，提出了一種構(gòu)造偽觀測的方法，并利用偽觀測在SOMD算法的基礎(chǔ)上給出了觀測噪聲方差矩陣的估計(PSOMD)。

定理2.對于線性離散隨機系統(tǒng)(1)，定義相鄰2個時刻的偽觀測以及其一階自差分序列如下

(7)

如果采樣時間Ts比較小，那么單一測量系統(tǒng)的觀測噪聲方差可以通過式(8)獲得

(8)

證明：根據(jù)系統(tǒng)(1)，直接觀測的一階自差分序列為

ΔZk=Zk-Zk-1

=(HkXk+Vk)-(Hk-1Xk-1+Vk-1)

(9)

(10)

(11)

那么偽觀測的一階自差分序列為

=HkΦk,k-2Xk-2-Hk-1Φk-1,k-2Xk-2-

=Hk(Xk-Φk,k-1Wk-2-Wk-1)-Hk-1(Xk-1-

(12)

式中

(13)

結(jié)合式(9)，可得如下SOMD序列

(14)

(15)

因此

(16)

對于時不變系統(tǒng)，Hk=Hk-1。一般而言，Hk≈Hk-1在較短的時間間隔內(nèi)也成立，因此式(13)可以簡化為

(17)

Pk-2[Hk(Φk,k-1-I)Φk-1,k-2]T

(18)

(19)

將式(19)代入式(16)中，可以得到式(8)。定理得證。

由式(19)可知，狀態(tài)估計誤差的影響在偽觀測的一階差分序列中可以忽略，利用偽觀測估計的噪聲方差矩陣的準(zhǔn)確性只與系統(tǒng)的過程噪聲方差矩陣有關(guān)。在已知過程噪聲方差的先驗知識的情況下或存在模型補償時，上述算法的準(zhǔn)確性可以得到保障。

注2：由于經(jīng)典的IAE自適應(yīng)濾波算法中，觀測噪聲方差矩陣的估計中也包含減法運算，其公式為

(20)

(21)

(22)

由式(22)可知，相對于IAE自適應(yīng)濾波算法，本文所提算法更不容易產(chǎn)生非正定的觀測噪聲方差估計，也就是說，其計算穩(wěn)定性優(yōu)于IAE算法。

2.2 PSOMD算法的實現(xiàn)

在實際應(yīng)用中，使用整體數(shù)據(jù)作為樣本會造成體現(xiàn)噪聲分布變化的動態(tài)信息淹沒于大量的歷史數(shù)據(jù)中。為了及時適應(yīng)噪聲的變化，以連續(xù)滑動開窗的方式，利用當(dāng)前窗口長度內(nèi)的數(shù)據(jù)進行估計，具體如下

(23)

式中，w為滑動窗口的長度。

此外，為了抑制滑動窗口樣本不足的影響，采用衰減記憶的方法，引入一個遺忘因子對歷史信息進行加權(quán)，具體實施如下

(24)

此外， 所提算法對觀測噪聲方差矩陣的估計效果與采樣頻率密切相關(guān)，采樣頻率越高，獲得的估計精度越高。由于較高的采樣頻率對計算機的要求也較高，工程應(yīng)用時可首先嘗試采用能夠滿足要求的低采樣頻率。

3 仿真驗證

在本節(jié)中，采用目標(biāo)跟蹤問題作為應(yīng)用來驗證所提算法的有效性，并且通過與擴展卡爾曼濾波(Extended Kalman Filter，EKF)、RAE以及RAKF算法的比較來證明所提算法的有效性。

考慮在3D笛卡爾坐標(biāo)系中具有直線運動和機動運動的目標(biāo)軌跡?？偟母檿r間為140s，前50s目標(biāo)以恒定的速度200m/s做直線運動，50s～100s之間以相同速度做周期為100s的勻速圓周運動，最后仍以200m/s的速度沿直線飛回，目標(biāo)軌跡如圖1所示。

圖1 目標(biāo)運動軌跡Fig.1 Target trajectory

采用三維位置和速度組成系統(tǒng)狀態(tài)，建立連續(xù)的定常速度的運動模型系統(tǒng)，其過程噪聲的功率譜密度為

(25)

使用雷達觀測距離、俯仰角和方位角，觀測周期Ts=0.1s。為了驗證所提算法的效果，將觀測噪聲方差設(shè)置為

(26)

由于RAE算法也自適應(yīng)地估計噪聲方差矩陣，故將其與所提算法獲得的噪聲方差矩陣作對比，結(jié)果如圖2～圖4所示。 由圖可知，RAE給出的噪聲方差估計在機動運動時段嚴重偏離真值，這是由于其依賴于狀態(tài)估計誤差導(dǎo)致的誤差累積造成的。然而， PSOMD幾乎能夠在整個跟蹤時段給出較準(zhǔn)確和穩(wěn)定的估計(除了噪聲方差突然變大之后的過渡階段，這是由采樣頻率較低引起的，增大采樣頻率即可解決)。

圖2 距離的噪聲方差估計Fig.2 Estimation of R on distance

圖3 方位角的噪聲方差估計Fig.3 Estimation of R on azimuth

圖4 俯仰角的噪聲方差估計Fig.4 Estimation of R on pitch

所有參與比較的自適應(yīng)濾波算法的位置估計誤差如圖5和圖6所示。結(jié)果表明，所有算法給出的估計結(jié)果在前50s沒有明顯的差異，這是因為在該時間段內(nèi)直線運動的模型準(zhǔn)確，觀測噪聲也沒有發(fā)生突變。然而，在50s后幾種算法之間產(chǎn)生了明顯的差異：RAE在目標(biāo)機動運動期間表現(xiàn)最差，這是因為其基于殘差序列并且與狀態(tài)估計誤差耦合；RAKF通過膨脹估計誤差協(xié)方差可以補償模型誤差，故在50s～100s之間模型不準(zhǔn)時給出了誤差較小的估計，可是由于其依賴觀測使得估計結(jié)果比較振蕩，尤其是在100s之后觀測噪聲方差變大的時段；然而，本文所提的算法在整個跟蹤時段都給出了比其他算法更準(zhǔn)確的估計。

表1列出了連續(xù)進行100 次Monte Carlo仿真實驗，所有算法在不同的運動模式下的均方根誤差(Root Mean Squared Eerrors，RMSEs)。誤差數(shù)據(jù)也表明了所提算法的有效性。

圖5 X軸速度誤差Fig.5 X-axis position errors

圖6 Y軸位置誤差Fig.6 Y-axis position errors

表1 位置估計的誤差Tab.1 Average RMSEs of position estimation

表2比較了幾種算法的仿真時間。由表2可知，RAE和PSOMD比RAKF和EKF用時較長，這是因為這兩種算法都采用了滑動開窗來估計當(dāng)前時刻的觀測噪聲方差；而本文所提算法相較于RAE又用時稍長0.01s，這是因為PSOMD為了抑制滑動窗口樣本不足的影響采用了衰減記憶的方法。

表2 仿真時間Tab.2 Simulation time

針對目標(biāo)做螺旋運動的軌跡，在相同的采樣頻率下進行了仿真實驗。結(jié)果表明， PSOMD算法仍能給出較準(zhǔn)確的濾波結(jié)果。由于該算法是基于滑動窗口實現(xiàn)的，如果軌跡更為機動，可以通過提高采樣頻率來實現(xiàn)快速跟蹤。

4 結(jié)論

本文介紹了一種基于冗余偽觀測的自適應(yīng)卡爾曼濾波算法來估計觀測噪聲的方差，并且通過目標(biāo)跟蹤問題驗證了該算法的有效性。對本文工作的總結(jié)如下：

1)通過構(gòu)建偽測量將現(xiàn)有的SOMD算法推廣到只具有單一測量的系統(tǒng)中，使其適用于更廣泛的應(yīng)用場景。

2)仿真結(jié)果表明，即使在采樣頻率較低時，所提算法仍能夠準(zhǔn)確反映測量系統(tǒng)的噪聲特性，在準(zhǔn)確性和魯棒性方面優(yōu)于其他參考算法。

3)所提算法對觀測噪聲方差矩陣的估計中包含了過程噪聲方差矩陣。雖然當(dāng)采樣周期較小時，估計結(jié)果受狀態(tài)估計誤差的影響可以忽略，但仍受過程噪聲方差的影響。只有過程噪聲方差確切已知時才能準(zhǔn)確地估算觀測噪聲方差，這使得該算法的應(yīng)用受到了限制。因此，找到一種方法使過程噪聲方差對觀測噪聲方差估計的影響也可以被忽略，值得進一步研究。