狀態變換卡爾曼濾波的進一步解釋及應用

王茂松，吳文啟，何曉峰，潘獻飛

（國防科技大學智能科學學院，長沙 410073）

如果捷聯慣性組合導航系統建模合理，并且傳感器均能良好工作的情況下，大部分基于擴展卡爾曼濾波甚至其它非線性濾波的組合導航系統[1-3]均能較好的工作。但是，當導航平臺處于不利的環境下時，如在準靜態的環境下，即處于靜態的載體仍然受到振動、風動等外部力的影響，一貫的捷聯慣性組合導航濾波方式很容易出現方差估計不一致的問題。例如，雖然在準靜態環境下，東方向陀螺和水平方向加表是不可觀測的，即它們的方差在理論上應該是保持不變的，但是由于振動的存在，會使組合導航系統中它們的方差仍然會繼續減少，導致方差的理論值與實際估計值不符，進而導致導航精度的下降甚至是濾波的發散[4]。

為了解決方差估計不一致的問題，學者們提出了一些方法論，如施密特卡爾曼濾波器[5]（Schmidt- Kalman Filter,SKF），將不可觀的狀態零化，也就是不可觀狀態對應的方差不進行更新。施密特卡爾曼濾波器實際上為次優濾波器，但是，它可以保證不可觀狀態估計的一致性，避免估計發散，并成功的應用于目標追蹤[6]以及太空探測[7]。Huang 等人[8]指出，在EKF-SLAM 系統中，如果過程模型和量測模型的雅克比矩陣以最近的狀態估計進行線性化，將會導致可觀測子空間的維度比實際的維度要高，進而使得狀態空間中本來不可觀測的狀態進行錯誤的校正，產生不一致的問題。所以，作者提出了選擇合適的線性化點來使系統的可觀測性與實際相符的卡爾曼濾波方式，稱為觀測約束擴展卡爾曼濾波（Observability Constraint Extended Kalman Filter,OCEKF），通過實驗驗證了所提出濾波方法的一致性。不變卡爾曼濾波（Invariant Extended Kalman Filter,IEKF）[9]是國外學者最新提出的可以應用于組合導航系統中的濾波方法，核心內容是基于李群李代數的理論，可以防止組合導航系統出現方差估計不一致的問題，在同時定位與構圖領域亦得到應用[10-11]。

文獻[12]指出導致捷聯慣性組合導航系統方差估計不一致問題的主要原因在于系統矩陣中的比力項，通過速度誤差狀態的嚴格變換，可以避免系統矩陣中含有比力項，進而克服方差估計不一致的問題。實際上，速度誤差的狀態變換在本質上與速度誤差狀態定義的坐標系一致性有異曲同工之妙[13-15]。本文對文獻[12]中提出的速度誤差從坐標系一致的角度作進一步的解釋，并將其成果進一步擴展應用于捷聯慣性導航的初始對準和慣性/衛星松組合導航。

1 NED 坐標系基于ST-EKF 的慣性/衛星松組合系統方程

當地導航系下定義的速度微分方程為：

其中，fb為比力向量，gn當地重力矢量，為地球相對慣性系的角速率在導航系的投影，為北東地(NED)坐標系相對地球運動產生的角速率在導航系的投影，∧代表將向量轉化為斜對稱矩陣，Cbn為姿態矩陣。

其中，φn和分別為姿態誤差和速度誤差向量，和的定義見文獻[12]，δfb為加速度計誤差向量。

新定義的速度誤差的微分方程的詳細推導如下：

其中，εb和?b分別為陀螺和加速度計的零偏；wg和wa分別為陀螺和加速度計誤差的白噪聲向量；是姿態誤差的微分：

陀螺儀和加速度計誤差可以建模為隨機游走過程：

根據雅克比恒等式可以得到：

顯然，從式(9)可以看出，新的速度誤差微分方程中不再含有比力項，而是由重力向量替代。由于IMU 在一般情況下的輸出都是比力增量而不是比力本身[13]，并且比力增量的值由于器件等原因會摻雜著量化噪聲，所以傳統基于EKF 的組合導航系統的系統矩陣的計算會被動態環境下的比力fn嚴重影響，進而導致方差不一致性。但是，新的系統矩陣中不再有比力項fn，而是被重力項gn所替代。新的基于狀態變換的卡爾曼濾波方式可以避免由于比力量化噪聲較大帶來的系統矩陣計算不精確的問題，因為對一般載體的當地局部導航來說，gn的變化很小，幾乎為常值。

根據新定義的速度誤差向量，可以推出位置誤差微分方程為：

其中，

根據式(5)(6)(9)(10)可以推導出基于ST-EKF 的組合導航系統在NED 坐標系下的慣導誤差狀態方程為：

其中，系統矩陣為F，慣導誤差狀態向量為x，噪聲轉移矩陣為G，過程噪聲向量為w，其對應的協方差矩陣為Q。它們的定義如下：

2 NED 坐標系基于ST-EKF 的慣性/衛星松組合觀測方程

基于ST-EKF 的慣性/衛星松組合觀測模型可以設計為δz=H x+υ，其中：

式(23)中的H矩陣是根據式(24)得到。

其中，δzv和δzr分別為慣導與衛星之間的速度誤差和位置誤差，υv,3×1和υr,3×1為對應的觀測白噪聲。由于卡爾曼濾波估計的是新的速度誤差狀態，所以，濾波之后速度狀態的更新應該根據式(25)進行：

姿態和位置的校正則與傳統的EKF 相同[13]。

3 實驗結果與分析

3.1 單位置對準實驗

本節通過基座振動環境的單位置精對準實驗來驗證所提出的狀態變換卡爾曼濾波的有效性。慣性測量單元為激光陀螺IMU，性能指標參數如表1所示。

表1 激光陀螺IMU 性能指標參數 Tab.1 Specifications of the RLG IMU

捷聯慣導系統放置在測試臺上靜態測試，時間為3600 s，慣性測量單元采樣率為200 Hz。為了驗證擴展卡爾曼濾波EKF 和狀態變換卡爾曼濾波ST-EKF在15 狀態情況下即狀態不完全可觀時的基座振動環境下的對準結果，對原始的比力測量數據加1 mm/s,10 Hz 的正弦線振動。GPS 的觀測頻率為1 Hz，并在速度和位置觀測中加入速度誤差0.01 m/s(1σ)，位置誤差1 m(1σ)。EKF 和ST-EKF 的預測頻率和更新頻率均設置為1Hz。實際上，提高EKF 的預測頻率可以改善其估計精度，但是這種方案并不是完全有效的[12]。

為了對比15 狀態EKF 和15 狀態ST-EKF 的結果，以5 狀態（3 個姿態角誤差，2 個水平速度誤差） EKF精對準的結果作為對比參考值，此時狀態是完全可觀的。5 狀態EKF、15 狀態EKF 和15 狀態ST-EKF均在3 min 解析粗對準之后開始卡爾曼濾波精對準。由于對準過程中往往航向角是最難估計的，所以這里僅對比觀察航向角的收斂結果。EKF 和ST-EKF 的航向角估計結果如圖1所示。

圖1 單位置精對準EKF 和ST-EKF 的航向角估計曲線 Fig.1 Yaw curves of EKF and ST-EKF in single-position fine alignment

圖1表明，在1800 s 以內，5 狀態EKF、15 狀態EKF 和15 狀態ST-EKF 的航向角估計均正常。實際上5 狀態ST-EKF 的航向角估計結果與5 狀態EKF 結果類似，這里不再過多展示。但是，在1800 s 之后，15狀態EKF 所估計的航向角逐漸發散，最終在3600 s 時，15 狀態EKF 與5 狀態EKF 的航向角差為0.0247°，而15狀態ST-EKF與5狀態EKF的航向角差僅為0.0009°。

圖2 東向陀螺漂移誤差標準差曲線 Fig.2 Standard deviation curve of the eastward gyro bias error

圖2表明，15 狀態EKF 的估計中，東向陀螺漂移誤差的標準差是逐漸減少的，而理論上不應該減少。準靜態的基座振動環境下，東向陀螺漂移是不可觀測的，而且東向陀螺漂移和航向角誤差是緊密相關的，由于東向陀螺漂移誤差的標準差進行了錯誤的估計，進 而導致15 狀態EKF 的航向角在1800 s 之后發散。但是，15 狀態ST-EKF 具有較好的東向陀螺漂移誤差標準差估計特性，如圖2所示，保持在0.01 (°)/h 附近，所以東向陀螺漂移誤差的標準差保持特性使得ST-EKF 的航向角估計不會因為不可觀的狀態產生錯誤的校正。

盡管5 狀態EKF 在對準過程中不會出現濾波不一致的問題，但是5 狀態卡爾曼濾波不適合對準之后的組合導航過程，因為需要通過15 狀態組合導航濾波進行初始對準和在組合導航過程中估計陀螺儀、加速度計的零偏誤差，提高組合導航精度。當長時間沿直線行駛或停車時，15 狀態組合導航濾波誤差狀態不完全可觀。傳統卡爾曼濾波在計算方差陣時，由于狀態轉移矩陣中的比力計算誤差，造成方差陣的計算誤差，使本來不可觀的等效東向陀螺漂移得到錯誤的修正，從而造成圖2的現象。沿直線行駛或停車時間越長，現象越明顯。類似現象及描述見文獻[4]和[16]。而15狀態ST-EKF 適用于對準之后直接進行組合導航，不需要進行濾波狀態的切換。

3.2 雙位置對準實驗

本節進一步通過雙位置對準實驗來驗證ST-EKF的對準性能，包括光纖陀螺IMU 3 min 雙位置對準實驗和激光陀螺IMU 3 min 雙位置對準實驗。這里的雙位置是指IMU 在一個位置靜止80 s 采集數據，再繞天向軸在20 s 內轉動180°之后，在另一個位置采集數據80 s，目的是為了提高慣性器件誤差的可觀測性，提高對準的精度。其中光纖陀螺的零偏優于0.003 (°)/h，激光陀螺的零偏優于0.01 (°)/ h，采樣頻率均為200 Hz。15 狀態EKF 和15 狀態ST-EKF 的預測頻率和更新頻率均設置為1 Hz。重復6 次實驗，光纖陀螺IMU 和激光陀螺IMU 的對準結果分別如表2和表3所示。

從表2可以看出，光纖陀螺IMU 的六次實驗中，EKF 航向角估計的標準差為0.1170°，而ST-EKF 的航向角估計標準差為0.0238°，ST-EKF 的精度要遠高于EKF 的雙位置對準精度。從表3可以看出，激光陀螺IMU 實驗中，EKF 航向角估計的標準差為0.0382°，而ST-EKF 的航向角估計標準差為0.0209°。

表2 光纖陀螺IMU 雙位置對準實驗的航向角估計結果 Tab.2 Estimation results of yaw angles in fiber gyro IMU's two-position alignment experiment

表3 激光陀螺IMU 雙位置對準實驗的航向角估計結果 Tab.2 Estimation results of yaw angles in laser gyro IMU's two-position alignment experiment

兩種不同的IMU 系統均表明，ST-EKF 具有更優的雙位置對準精度。這是由于在IMU 轉動過程中EKF系統矩陣中的比力計算不準確造成了方差陣的估計不準確，影響航向角的估計精度。ST-EKF 則解決了這個問題，航向角估計精度較高，可以在對準之后直接進行組合導航，不需要進行濾波狀態的切換。

3.3 激光陀螺IMU/衛星組合導航跑車實驗

3.1 節和3.2 節分別從單位置對準和雙位置對準實驗驗證了ST-EKF的優越性能。本節通過激光陀螺IMU/衛星動態跑車松組合導航實驗來驗證ST-EKF 的性能。激光陀螺IMU 參數如表1所示，IMU 數據輸出頻率為100 Hz，GPS 輸出位置速度頻率為1 Hz。跑車軌跡的谷歌地球顯示如圖3所示。在組合導航過程中，將衛星信號人為取多段進行斷開90 s，觀察水平位置誤差的精度。EKF 和ST-EKF 的預測頻率和更新頻率均設置為1 Hz，4 h 組合導航的水平位置誤差如圖4所示。

由圖4可以看出，在衛星信號斷開的6 個時間段內，EKF 具有比ST-EKF 更大的最大水平位置誤差。同時，ST-EKF 的總體水平位置誤差曲線都在EKF 的水平位置誤差曲線下面。所以，對于導航級IMU，ST-EKF 具有比EKF 更優的組合導航水平位置精度。

圖3 激光陀螺IMU/衛星組合導航實驗軌跡 Fig.3 Google curve of RLG-IMU/GNSS integration experiment

圖4 激光陀螺IMU/衛星組合導航EKF 和ST-EKF 的水平位置誤差對比 Fig.4 Comparisons on horizontal position errors between EKF and ST-EKF of RLG-IMU/GNSS integration experiment

3.4 MEMS-IMU/衛星組合導航跑車實驗

為了進一步驗證ST-EKF 的組合導航性能，本節設計了STIM300 MEMS-IMU/衛星組合導航實驗。STIM300 MEMS-IMU 的性能參數如表4所示。

表4 STIM300 IMU 性能指標參數 Tab.4 Specifications of the STIM300 IMU

IMU 輸出頻率為200 Hz，GPS 輸出位置速度頻率為1 Hz。跑車實驗時長為3600 s，跑車軌跡的谷歌地球顯示如圖5所示。EKF 和ST-EKF 的預測頻率和更新頻率均設置為1 Hz，在完全相同的初始參數設置條件下，EKF 和ST-EKF 的組合導航水平位置誤差如圖6所示。

圖5 MEMS-IMU/衛星組合導航實驗軌跡 Fig.5 Google curve of the MEMS-IMU/GNSS integration experiment

圖6表明，在多處斷開GPS 的情況下，EKF 的最大水平位置誤差達到了55 m，而ST-EKF 的水平位置誤差均能保持30 m 以下。所以，在3.3 節和3.4 節相應實驗中，ST-EKF 不僅適用于導航級IMU 與衛星的組合導航，同時也適用于MEMS-IMU/衛星組合導航。ST- EKF 比EKF 具有更好的動態適應性，導航精度更高。

圖6 MEMS-IMU/衛星組合導航EKF 和ST-EKF的水平位置誤差對比 Fig.6 Comparisons on horizontal position errors between EKF and ST-EKF of MEMS-IMU/GNSS integration experiment

4 結 論

本文從速度誤差定義的坐標系一致性角度，對狀態變換卡爾曼濾波的合理性作出了進一步的解釋，并推導了捷聯慣導的初始對準和組合導航的松組合模型。在狀態變換卡爾曼濾波的框架下，組合導航濾波的預測過程不需要以犧牲系統矩陣的高頻預測來提高濾波的精度，預測過程和更新過程可以同時執行。同時，對準過程和組合導航過程可以采用統一的15 狀態模型，不需要切換組合導航狀態，提高組合導航的精度和穩定性。

激光陀螺IMU 的動基座單位置對準實驗驗證了狀態變換卡爾曼濾波具有比傳統擴展卡爾曼濾波更好的航向估計穩定性和精度。激光陀螺IMU 和光纖陀螺IMU 雙位置對準實驗亦驗證了狀態變換卡爾曼濾波具有比傳統濾波更高的對準精度。

激光陀螺IMU/衛星組合導航實驗表明，在多處衛星信號斷開的情況下，狀態變換卡爾曼濾波比傳統的濾波方案定位精度更高。MEMS-IMU/衛星組合導航實驗驗證了，在多處衛星信號斷開的情況下，傳統卡爾曼濾波的最大水平位置誤差達到了55 m，而狀態變換卡爾曼濾波的水平位置誤差均在30 m 以下。