楊 波，劉 楓，薛 亮，單 斌

（火箭軍工程大學(xué) 導(dǎo)彈工程學(xué)院，西安 710025）

卡爾曼濾波是一種遞推最小方差估計算法，算法通過時間更新和量測更新兩個過程充分融合系統(tǒng)先驗信息和實時量測信息，具備計算量小、便于計算機(jī)執(zhí)行等優(yōu)勢，目前已廣泛應(yīng)用于導(dǎo)航定位、圖像處理、目標(biāo)跟蹤等領(lǐng)域[1-3]。卡爾曼濾波算法基于系統(tǒng)驅(qū)動噪聲和量測噪聲為互不相關(guān)的零均值高斯白噪聲前提假設(shè)推導(dǎo)建立[4-6]。然而在實際工程系統(tǒng)中，常常會存在系統(tǒng)噪聲或量測噪聲是有色噪聲的情況，此時若直接應(yīng)用基本卡爾曼濾波對數(shù)據(jù)進(jìn)行濾波計算，容易導(dǎo)致濾波發(fā)散造成估計誤差增大的問題[7-10]。

針對有色噪聲情況下如何適用卡爾曼濾波問題，許多文獻(xiàn)提出了較為有效的解決辦法[11-13]。文獻(xiàn)[12]基于Tustin 生成函數(shù)對系統(tǒng)進(jìn)行離散化，而后利用增廣向量法，將有色噪聲作為新的增廣狀態(tài)向量，并提出一種分?jǐn)?shù)階卡爾曼濾波器。文獻(xiàn)[14]采用系統(tǒng)的代價函數(shù)構(gòu)建狀態(tài)方程和量測方程，并通過量測增廣法對有色噪聲進(jìn)行白化以滿足標(biāo)準(zhǔn)卡爾曼濾波的應(yīng)用條件。文獻(xiàn)[15]通過設(shè)計改進(jìn)的差分卡爾曼濾波器，用以降低有色噪聲對導(dǎo)航系統(tǒng)的影響，同時根據(jù)量測殘差構(gòu)建多重自適應(yīng)調(diào)節(jié)因子，進(jìn)一步增強(qiáng)濾波器的跟蹤性能。文獻(xiàn)[16]利用有色噪聲非相鄰歷元間的相關(guān)特性，建立噪聲協(xié)方差模型，用以消除高階噪聲的影響，同時應(yīng)用抗差估計方法得到改進(jìn)的增益矩陣和狀態(tài)更新方程，提出了一種有色噪聲下的抗差卡爾曼濾波算法。上述文獻(xiàn)均對有色噪聲條件下的濾波問題提出了行之有效的解決方案，但是其所適用模型的量測噪聲統(tǒng)計特性是已知的，而通常實際工程系統(tǒng)中的量測噪聲較難確定，因此需要進(jìn)一步改進(jìn)濾波算法對有色量測噪聲的適應(yīng)能力[17-19]。

本文以捷聯(lián)慣導(dǎo)系統(tǒng)（Strapdown Inertial Navigation System,SINS）/多普勒雷達(dá)組合導(dǎo)航系統(tǒng)為應(yīng)用對象，針對多普勒雷達(dá)測速噪聲為缺少先驗信息的有色噪聲條件下的組合導(dǎo)航濾波問題開展研究。首先，對多普勒測速雷達(dá)測量噪聲進(jìn)行一階馬爾可夫建模，并基于此模型建立SINS/多普勒雷達(dá)組合導(dǎo)航系統(tǒng)的狀態(tài)方程和量測方程；然后，采用重構(gòu)量測方程法消除量測有色噪聲的影響，并通過進(jìn)一步重構(gòu)狀態(tài)方程消除其與重構(gòu)后量測方程的相關(guān)影響；最后基于重構(gòu)后的狀態(tài)空間模型建立有色噪聲條件下的改進(jìn)卡爾曼濾波算法，并通過仿真實驗驗證所提濾波算法的有效性。

1 SINS/多普勒雷達(dá)組合導(dǎo)航系統(tǒng)模型

1.1 傳感器誤差模型

陀螺儀和加速度計完成標(biāo)定后，在組合導(dǎo)航過程中，一般只需考慮其逐次啟動誤差和快變誤差。逐次啟動誤差與啟動時刻的環(huán)境條件和電器參數(shù)的隨機(jī)特性等條件有關(guān)，一旦啟動完成就保持在某一固定值上，通常用隨機(jī)常數(shù)來描述；快變誤差可以描述為白噪聲過程。因此陀螺儀和加速度計的誤差模型為：

其中，?i為加速度計隨機(jī)誤差；?bx、?by、?bz分別為加速度計隨機(jī)常值偏置在載體坐標(biāo)系x、y、z軸上的投影；wax、way、waz分別為其對應(yīng)的白噪聲；εi為陀螺儀隨機(jī)誤差；εbx、εby、εbz分別為陀螺儀常值漂移在載體坐標(biāo)系下x、y、z軸上的投影；wgx、wgy、wgz分別為其對應(yīng)的白噪聲。根據(jù)陀螺儀和加速度計誤差模型，并結(jié)合SINS 力學(xué)編排方程，可以進(jìn)一步推導(dǎo)出SINS 誤差模型，很多文獻(xiàn)已經(jīng)研究，本文不再贅述。

多普勒雷達(dá)通常沿車體軸固定于載車安裝支架上，其與載體坐標(biāo)系之間必然存在一定的安裝誤差。安裝誤差可以通過標(biāo)定完成補償，因此，多普勒雷達(dá)的測速誤差可描述為一階馬爾可夫過程。

其中，δD為多普勒雷達(dá)測速誤差；τk為馬爾可夫過程相關(guān)時間；wD為激勵白噪聲。

1.2 狀態(tài)空間模型

1）狀態(tài)方程

選取東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系。根據(jù)SINS、多普勒雷達(dá)的誤差模型，選取SINS/多普勒雷達(dá)組合導(dǎo)航系統(tǒng)狀態(tài)向量為：

其中，F(xiàn)、G、W分別為系統(tǒng)的狀態(tài)矩陣、噪聲驅(qū)動矩陣、白噪聲序列，W＝[wgxwgywgzwaxwaywaz]T，F(xiàn)、G均來自于上文建立的誤差模型。

2）量測方程

車載多普勒雷達(dá)通常沿著載體坐標(biāo)系的軸向進(jìn)行安裝，其輸出為載車相對地面的絕對運動速度，而SINS 輸出的速度是載車運動速度在導(dǎo)航坐標(biāo)系下的投影形式。

為了利用上述兩個系統(tǒng)的輸出速度構(gòu)造量測，需要利用SINS 輸出的載體姿態(tài)矩陣，將多普勒雷達(dá)速度由載體坐標(biāo)系（b系）轉(zhuǎn)換到導(dǎo)航坐標(biāo)系（n系）輸出。假設(shè)多普勒雷達(dá)速度輸出為，SINS 速度輸出在實際導(dǎo)航坐標(biāo)系下的投影為，SINS 輸出的姿態(tài)矩陣為，則可構(gòu)造量測方程為：

結(jié)合式(4)所選取狀態(tài)，可將量測方程寫為：

其中，ε是有色量測噪聲；H為量測矩陣，H=[H1H203×9]。

至此，SINS/多普勒雷達(dá)組合導(dǎo)航的狀態(tài)空間模型已經(jīng)建立完畢。為了能夠直接利用計算機(jī)進(jìn)行濾波計算，可采用歐拉法、零階保持法等離散化方法將連續(xù)的系統(tǒng)狀態(tài)方程進(jìn)行離散化，而量測方程已是離散的。

2 有色噪聲條件下的改進(jìn)卡爾曼濾波

在第1 節(jié)建立的SINS/多普勒雷達(dá)組合導(dǎo)航狀態(tài)空間模型中，量測噪聲為有色噪聲，此時若直接使用卡爾曼濾波，則會導(dǎo)致濾波精度下降甚至發(fā)散。為此，針對量測噪聲為有色噪聲情況下組合導(dǎo)航濾波問題，需要改進(jìn)卡爾曼濾波算法。量測噪聲為有色噪聲的離散型狀態(tài)空間模型為：

其中，Wk和Vk為互不相關(guān)的白噪聲，且滿足

其中，Qk為系統(tǒng)噪聲方差矩陣；Rk為有色噪聲的激勵白噪聲方差矩陣。由于此時量測噪聲為有色噪聲，無法直接應(yīng)用卡爾曼濾波進(jìn)行處理，通過重構(gòu)將有色量測噪聲白化：

將式(13)代入式(15)，整理后可得：

將重構(gòu)后的量測方程簡寫為：

由式(19)可知，此時的量測噪聲不再是有色噪聲，而轉(zhuǎn)化為Wk和Vk復(fù)合后的白噪聲，可以進(jìn)一步求出重構(gòu)后的量測噪聲方差矩陣為：

可見系統(tǒng)噪聲與重構(gòu)后的量測噪聲存在相關(guān)性，兩者之間的協(xié)方差為：

式(21)說明系統(tǒng)噪聲和量測噪聲之間的協(xié)方差不等于0，因此，此時無法直接利用標(biāo)準(zhǔn)卡爾曼濾波完成濾波計算。需要進(jìn)一步重構(gòu)狀態(tài)方程，消除系統(tǒng)噪聲與量測噪聲之間的相關(guān)性：

此時，狀態(tài)方程與量測方程之間的相關(guān)性已經(jīng)通過重構(gòu)狀態(tài)空間模型消除，應(yīng)用標(biāo)準(zhǔn)卡爾曼濾波即可進(jìn)行濾波估計。但在實際工程系統(tǒng)中，通常難以獲得準(zhǔn)確的Rk統(tǒng)計信息，由式(20)和式(28)可知都會受到Rk的影響，因此，直接應(yīng)用基本卡爾曼濾波難以獲得很好的濾波結(jié)果。通過自適應(yīng)卡爾曼濾波可以在Rk難以確定的情況下，自適應(yīng)地估計從而獲得更好的濾波效果。根據(jù)重構(gòu)后的狀態(tài)空間模型，改進(jìn)的卡爾曼濾波算法為：

1）狀態(tài)空間模型重構(gòu)

其中，b為遺忘因子，通常為0.95 ＜b＜ 0.99；dk的值只與b相關(guān)。

3 仿真驗證

3.1 仿真實驗環(huán)境

本文根據(jù)常見的載車機(jī)動形式設(shè)計其運動軌跡，仿真時長為1200 s。載車的初始經(jīng)度、緯度、高度分別為108 °、34 °、400 m；初始速度為10 m/s；初始航向角、俯仰角、滾動角均為0 °，載車的運動狀態(tài)如表1所示。運動軌跡如圖1 所示，圖中的標(biāo)記表示每隔50 s載車所處的位置。根據(jù)載車運動軌跡仿真生成對應(yīng)的陀螺儀、加速度計、多普勒雷達(dá)理想輸出數(shù)據(jù)（不含傳感器誤差），然后在仿真生成的傳感器輸出中加入與之對應(yīng)的測量誤差，從而得到傳感器的實際輸出數(shù)據(jù)。假設(shè)陀螺儀的常值漂移為0.02 °/h，白噪聲隨機(jī)游走為加速度計常值偏置為50 μg，白噪聲隨機(jī)游走為多普勒雷達(dá)測速誤差為有色噪聲，有色噪聲的驅(qū)動白噪聲的均值為0，方差為1 m/s，相鄰時間點上噪聲的相關(guān)性為0.8，噪聲過程如圖2所示。假設(shè)陀螺儀和加速度計的輸出頻率為200 Hz，多普勒雷達(dá)輸出頻率為1 Hz。SINS 采用雙子樣算法，其解算周期為0.1 s，濾波器融合周期為1 s。SINS 的初始俯仰、滾動誤差為2 '，初始航向誤差為5 '，初始速度誤差為0.1 m/s，初始位置誤差為20 m。

圖2 多普勒雷達(dá)有色測速噪聲Fig.2 Doppler radar colored velocity measurement noise

3.2 仿真實驗結(jié)果

為了驗證本文提出的自適應(yīng)有色噪聲卡爾曼濾波算法（Adaptive Colored Noise Kalman Filter,ACNKF）的優(yōu)勢，在相同的實驗環(huán)境下，采用自適應(yīng)卡爾曼濾波（Adaptive Kalman Filter,AKF）、有色噪聲卡爾曼濾波（Colored Noise Kalman Filter,CNKF）和本文算法進(jìn)行SINS/多普勒雷達(dá)組合導(dǎo)航濾波計算，對比三種算法下的組合導(dǎo)航定位誤差，定位誤差曲線如圖3-5所示。

圖3 東向位置誤差對比Fig.3 Comparison of eastward position error

圖4 北向位置誤差對比Fig.4 Comparison of northward position error

圖3-4 分別為東向和北向位置誤差曲線對比圖。從圖中可以看出，當(dāng)量測噪聲為缺少先驗信息的有色噪聲時，ACNKF 算法的定位精度高于CNKF 和AKF算法。三種濾波算法在導(dǎo)航初始時刻的定位精度差別較小，隨著導(dǎo)航時長的增加，三種算法的定位誤差精度差距不斷加大，體現(xiàn)了ACNKF 算法在導(dǎo)航定位濾波過程中對缺少先驗信息的有色量測噪聲具有較好的適應(yīng)估計。

圖5 為天向位置誤差曲線對比圖。從圖中可以看出，三種濾波算法的天向定位差別很小，位置誤差曲線幾乎重合，這是由于SINS/多普勒雷達(dá)組合導(dǎo)航模型中的天向位置誤差δh的可觀測度較低所造成的。

圖5 天向位置誤差對比Fig.5 Comparison of upward position error

表2 為量測噪聲為有色噪聲下三種濾波算法的導(dǎo)航定位均方根誤差（Root Mean Square Error,RMSE）。與AKF 和CNKF 算法相比，ACNKF 算法的東向定位精度提高了34.67%、16.08%，北向定位精度提高了13.27%、25.93%?？梢?，當(dāng)量測噪聲為缺少統(tǒng)計特性的有色噪聲時，ACNKF 算法仍可以保持較高的穩(wěn)定性，獲得較高精度的導(dǎo)航定位結(jié)果，且其水平定位精度高于AKF 算法和CNKF 算法，平均水平定位精度分別提升了23.97%、21.01%。

表2 位置RMSE 誤差對比Tab.2 Position RMSE error comparison

為了進(jìn)一步評估本文算法的定位性能，模擬多普勒雷達(dá)有色驅(qū)動噪聲的均值非零的情況。設(shè)置有色噪聲的驅(qū)動白噪聲均值為0.1，其余條件與前文實驗環(huán)境相同，得到的位置誤差曲線如圖6 所示。

圖6 位置誤差曲線圖Fig.6 Position error curve

從圖6 可知，當(dāng)有色噪聲的均值非零時，在長時間組合后，東向位置誤差和北向位置誤差會出現(xiàn)緩慢發(fā)散情況，天向位置誤差也存在一定發(fā)散，但相較于東向和北向位置誤差，其發(fā)散程度較輕。在處理非零均值噪聲時，本文算法存在緩慢發(fā)散情況，可以在下一步研究中對這個缺陷進(jìn)行改進(jìn)，使算法具有更強(qiáng)的適應(yīng)能力。

4 結(jié)論

本文研究了量測噪聲為缺少先驗信息的有色噪聲條件下SINS/多普勒雷達(dá)組合導(dǎo)航濾波問題。首先，建立組合導(dǎo)航系統(tǒng)傳感器誤差模型，并進(jìn)一步建立SINS/多普勒雷達(dá)組合導(dǎo)航系統(tǒng)狀態(tài)空間模型；然后，通過重構(gòu)狀態(tài)空間模型消除有色量測噪聲對濾波估計的不利影響，并利用自適應(yīng)濾波進(jìn)一步提升對缺乏先驗信息量測噪聲的適應(yīng)能力；最后進(jìn)行了仿真實驗，實驗結(jié)果表明，本文提出的濾波算法相較于傳統(tǒng)方法的定位精度更高，適應(yīng)性更好，對于有色量測噪聲條件下的濾波問題具有一定的參考價值。