袁思思, 陳 帥, 牛仁杰, 周 興, 程 玉

（南京理工大學(xué), 南京 210094）

0 引言

慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System, SINS）具有自主性強(qiáng)、 動(dòng)態(tài)性能好、 導(dǎo)航信息全面且輸出頻率高等優(yōu)點(diǎn), 但其誤差隨時(shí)間不斷積累, 長(zhǎng)期精度不高［1］。 相比而言, 全球衛(wèi)星定位系統(tǒng)（Global Navigation Satellite System, GNSS）的優(yōu)點(diǎn)是定位精度高并且誤差不隨時(shí)間積累, 但是導(dǎo)航信息不夠全面且容易受到干擾, 在室內(nèi)和遮擋等環(huán)境下無(wú)法使用。 慣性導(dǎo)航和衛(wèi)星導(dǎo)航之間有很強(qiáng)的互補(bǔ)性, 慣性/衛(wèi)星組合導(dǎo)航提高了系統(tǒng)整體的精度和可靠性, 被公認(rèn)為是最佳的組合導(dǎo)航方案［2］。

然而, 由于組合導(dǎo)航系統(tǒng)本質(zhì)上具有的非線性特征, 當(dāng)采用非線性系統(tǒng)模型來(lái)描述導(dǎo)航誤差的傳播特性時(shí), 常規(guī)的Kalman 濾波（KF）就不再適用, 此時(shí)需要采用非線性濾波算法來(lái)解決系統(tǒng)中的狀態(tài)估計(jì)問(wèn)題。 王維等［3］采用自適應(yīng)無(wú)跡Kalman 濾波來(lái)解決系統(tǒng)的非線性問(wèn)題, 對(duì)導(dǎo)航精度有一定的提升效果, 但是采用無(wú)跡變換存在著計(jì)算量隨著狀態(tài)量維度的增加而增大的問(wèn)題, 不適用于實(shí)際的工 程 實(shí)現(xiàn)。 張 園等［4］在GNSS/SINS 組合導(dǎo)航系統(tǒng)中分別應(yīng)用擴(kuò)展Kalman 濾波（EKF）和粒子濾波（Particle Filter, PF）兩種非線性濾波算法進(jìn)行對(duì)比, 結(jié)果表明擴(kuò)展Kalman 濾波更適用于弱非線性情況下的組合導(dǎo)航系統(tǒng), 定位精度更高。 劉江等［5］將 魯 棒 容 積Kalman 濾 波（Cubature Kalman Filter, CKF）應(yīng)用于非線性組合導(dǎo)航系統(tǒng)中, 可以有效抑制異常觀測(cè)值的影響, 但是當(dāng)模型噪聲不確定時(shí)濾波質(zhì)量和效率降低。

另外, 當(dāng)載體處于復(fù)雜環(huán)境中時(shí), 傳感器量測(cè)噪聲的統(tǒng)計(jì)特性難以準(zhǔn)確獲取且具有時(shí)變性［6］。此時(shí)若使用單一固定的濾波器模型對(duì)系統(tǒng)進(jìn)行估計(jì), 則在長(zhǎng)時(shí)間運(yùn)行過(guò)程中必定導(dǎo)致濾波精度的下降［7］。 由此, 很容易想到采用多個(gè)模型并行估計(jì)的方法。 1965 年, Magill 提出了第一代多模型自適應(yīng)估計(jì)算法, 其主要特點(diǎn)是使用固定個(gè)數(shù)的濾波器, 每個(gè)濾波器對(duì)應(yīng)一個(gè)特定的模型, 模型之間不能交互, 對(duì)每個(gè)濾波器的估計(jì)狀態(tài)值賦予固定的不同的權(quán)值, 每個(gè)狀態(tài)估計(jì)值乘以相應(yīng)的權(quán)重相加得到最終的狀態(tài)估計(jì)值。 該方法雖能在一定程度上提高估計(jì)的準(zhǔn)確性, 但由于模型之間缺少交互且權(quán)值固定, 因此適用范圍有限［8］。 在多模型自適應(yīng)估計(jì)的基礎(chǔ)上, Kirubarajan 等［8］提出了交互式多模型（IMM）算法, 這一方法在環(huán)境未知多變的情況下顯示了更好的魯棒性和更高的濾波精度。

IMM 是通過(guò)模型轉(zhuǎn)換概率實(shí)現(xiàn)系統(tǒng)模式的Markov 切換過(guò)程, 相比于廣義偽Bayes 算法, 兼具了計(jì)算復(fù)雜度低和估計(jì)精度高的優(yōu)點(diǎn), 被認(rèn)為是一種有效的混合估計(jì)方案［8］。 交互式多模型濾波算法對(duì)于處理模型參數(shù)不確定情況下的估計(jì)問(wèn)題有著其他方法所不具備的效果, 但估計(jì)精度比較依賴(lài)于模型集［9-10］。

本文引入交互式多模型的思想, 結(jié)合EKF 根據(jù)實(shí)際環(huán)境設(shè)計(jì)多個(gè)不同的模型, 每個(gè)模型對(duì)應(yīng)一個(gè)EKF 子濾波器, 各個(gè)子濾波器同時(shí)進(jìn)行濾波,并通過(guò)似然函數(shù)實(shí)現(xiàn)模型之間的交互, 然后對(duì)各個(gè)子濾波器的濾波結(jié)果進(jìn)行概率加權(quán)融合, 以得到最優(yōu)的狀態(tài)估計(jì)值。 將交互式多模型與擴(kuò)展Kalman 濾波結(jié)合在一起, 充分發(fā)揮非線性濾波適用于非線性系統(tǒng)的特點(diǎn), 更好地匹配實(shí)際環(huán)境中的非線性輸入輸出關(guān)系, 提高系統(tǒng)的定位精度, 增強(qiáng)系統(tǒng)的魯棒性。

1 GNSS/SINS 組合導(dǎo)航系統(tǒng)

GNSS/SINS 組合導(dǎo)航是目前常見(jiàn)的組合導(dǎo)航系統(tǒng), 兩者之間相互取長(zhǎng)補(bǔ)短, 克服了各自的缺點(diǎn), 使得組合后的導(dǎo)航系統(tǒng)精度明顯高于兩個(gè)系統(tǒng)單獨(dú)工作時(shí)的精度。 一般來(lái)說(shuō), 根據(jù)系統(tǒng)狀態(tài)的不同選擇, 可以搭建兩種不同的組合導(dǎo)航系統(tǒng)模型: 第一種是將導(dǎo)航子系統(tǒng)的導(dǎo)航輸出參數(shù)直接作為狀態(tài)向量, 稱(chēng)為直接法濾波; 第二種是將導(dǎo)航子系統(tǒng)的誤差量作為狀態(tài)向量, 稱(chēng)為間接法濾波。 由于直接法中系統(tǒng)狀態(tài)向量的數(shù)量級(jí)相差太大, 比如姿態(tài)與位置信息, 在數(shù)學(xué)計(jì)算上會(huì)產(chǎn)生較大誤差, 實(shí)際應(yīng)用中會(huì)存在較多問(wèn)題, 因此本文使用間接法, 采用松組合, 將GNSS 接收機(jī)和SINS 輸出的位置和速度信息的差值作為觀測(cè)量,經(jīng)濾波器濾波估計(jì)捷聯(lián)式慣性導(dǎo)航系統(tǒng)的誤差,從而對(duì)捷聯(lián)式慣性系統(tǒng)進(jìn)行矯正, 最后得到高精度、 穩(wěn)定、 持續(xù)的載體姿態(tài)速度位置信息。 本文采用地理系（東北天坐標(biāo)系）作為導(dǎo)航坐標(biāo)系, 對(duì)系統(tǒng)模型做如下介紹。

1.1 SINS 非線性誤差模型

在SINS 姿態(tài)解算過(guò)程中, 四元數(shù)方法由于計(jì)算簡(jiǎn)單、 精度高而被廣泛應(yīng)用［11］。 本文通過(guò)四元數(shù)來(lái)表示姿態(tài), 構(gòu)建SINS 誤差模型, 現(xiàn)對(duì)誤差模型的構(gòu)建做簡(jiǎn)要介紹, SINS 非線性誤差模型的具體介紹可以參考文獻(xiàn)［11］。

假設(shè)Q=［q0q1q2q3］T為SINS 真實(shí)的姿態(tài)四元數(shù),為實(shí)際解算的四元數(shù), 則Q和Q～分別滿足如下的微分方程

式（1）中, ?表示四元數(shù)乘積,b表示載體坐標(biāo)系,n表示導(dǎo)航坐標(biāo)系,i表示慣性坐標(biāo)系。式（2）中,

根據(jù)式（1）和式（2）, 可得如下的SINS 姿態(tài)誤差方程

同樣地, 根據(jù)SINS 真實(shí)速度、 實(shí)際測(cè)量速度以及慣導(dǎo)的比力方程可以得到SINS 的速度誤差方程為

式（4）中,Cn b為姿態(tài)矩陣, 其與四元數(shù)Q的關(guān)系為

根據(jù)SINS 位置P=［L λ h］T與SINS 速度Vn之間的關(guān)系可得SINS 位置誤差方程

參數(shù)的具體含義可參考文獻(xiàn)［11］, 在此不做過(guò)多介紹。 由于姿態(tài)轉(zhuǎn)移矩陣與四元數(shù)之間存在的非線性關(guān)系, 使得SINS 誤差方程在姿態(tài)誤差角較大時(shí)含有較強(qiáng)的非線性。

1.2 組合導(dǎo)航系統(tǒng)模型

（1）系統(tǒng)狀態(tài)方程

式（9）中,X（t）為由SINS 誤差變量構(gòu)成的19 維狀態(tài)向量。 其中,δq0、δq1、δq2、δq3分別為捷聯(lián)式慣性導(dǎo)航系統(tǒng)姿態(tài)四元數(shù)誤差,δVE、δVN、δVU分別為東向、 北向、 天向上的速度誤差,δL、δλ、δh分別為緯度、 經(jīng)度、 高度的位置誤差,εbx、εby、εbz分別為陀螺儀在x、y、z三個(gè)軸向上的隨機(jī)常值誤差,εrx、εry、εrz分別為陀螺儀在x、y、z三個(gè)軸向上的一階Markov 偏移誤差,Δx、Δy、Δz分別為加速度計(jì)在x、y、z三個(gè)軸向上的一階Markov偏移誤差。 式（10）中,G（t）為19 ×9 階系統(tǒng)噪聲驅(qū)動(dòng)矩陣,W（t）為9 維系統(tǒng)噪聲隨機(jī)誤差向量。

（2）系統(tǒng)量測(cè)方程

采用松組合時(shí), 系統(tǒng)量測(cè)方程為線性模型。將GNSS 接收機(jī)輸出的速度和位置信息與捷聯(lián)慣性導(dǎo)航系統(tǒng)相應(yīng)的輸出信息做差, 得到量測(cè)方程如下

式（11） 中,Z（t） 為6 維的量測(cè)向量,H（t） 為6 ×19 階量測(cè)矩陣,V（t）為6 維量測(cè)噪聲并且假設(shè)E［Vk］ =0 和, 具體矩陣設(shè)置可參考文獻(xiàn)［12］。

2 擴(kuò)展Kalman 濾波算法

對(duì)于非線性系統(tǒng), 一種常見(jiàn)的解決思路是先進(jìn)行Taylor 級(jí)數(shù)展開(kāi), 之后進(jìn)行線性化截?cái)嗦匀ジ唠A項(xiàng)后近似為線性系統(tǒng), 再做線性Kalman 濾波估計(jì), 這種處理非線性系統(tǒng)的Kalman 濾波方法即稱(chēng)為擴(kuò)展Kalman 濾波（EKF）［13］。 而根據(jù)截?cái)嗟奈恢? 擴(kuò)展Kalman 濾波可以分為一階擴(kuò)展Kalman 濾波和二階擴(kuò)展Kalman 濾波等, 二階的濾波精度并不明顯比一階高, 反而對(duì)于高維系統(tǒng)而言, 二階濾波的計(jì)算量增加了很多［1］, 因此在這里主要討論一階擴(kuò)展Kalman 濾波。 同樣地, 根據(jù)實(shí)際工程應(yīng)用, 主要對(duì)離散狀態(tài)下的系統(tǒng)進(jìn)行討論。

將GNSS/SINS 組合導(dǎo)航系統(tǒng)的狀態(tài)方程和量測(cè)方程離散化, 得到如下方程組

其中,

式（12）、 式（13） 中,Xk為n維 狀 態(tài) 向 量,f（Xk） =［f1（Xk）f2（Xk） …fn（Xk）］T為n維非 線性向量函數(shù),Zk為m維量測(cè)向量,Γk-1為k-1 時(shí)刻的n×p階系統(tǒng)噪聲驅(qū)動(dòng)矩陣,Hk為k時(shí)刻的m×n階量測(cè)矩陣,Wk-1為k-1 時(shí)刻滿足均值為0 的Gauss 白噪聲條件的p維過(guò)程噪聲向量,Vk為k時(shí)刻滿足均值為0 的Gauss 白噪聲條件的m維量測(cè)噪聲向量,Qk為過(guò)程噪聲向量的協(xié)方差矩陣且為非負(fù)定矩陣,Rk為量測(cè)噪聲向量的協(xié)方差矩陣且為正定矩陣,δkj為Kronecker-delta 函數(shù)。δkj滿足

EKF 的一個(gè)核心思想是對(duì)式（12）中的非線性方程做Taylor 展開(kāi)數(shù)學(xué)變換并將結(jié)果再做一階截?cái)?得到一組線性化方程

經(jīng)過(guò)Taylor 展開(kāi)并一階截?cái)嗪? 就將系統(tǒng)變?yōu)榱司€性系統(tǒng), 可以使用基本Kalman 濾波方程完成迭代優(yōu)化的過(guò)程。 至此, EKF 方程如下:

時(shí)間預(yù)測(cè)方程為

量測(cè)更新方程為

3 基于IMM-EKF 的GNSS/SINS 組合導(dǎo)航算法

3.1 交互式多模型算法

交互式多模型算法的核心思想是將系統(tǒng)的不同模型映射為模型集, 其中的每個(gè)模型對(duì)應(yīng)一個(gè)濾波器, 各濾波器經(jīng)過(guò)輸入交互之后并行工作,利用每個(gè)濾波器輸出的殘差信息、 殘差協(xié)方差信息以及各模型的先驗(yàn)信息, 依據(jù)某種假設(shè)檢驗(yàn)規(guī)則, 得到每個(gè)濾波器所對(duì)應(yīng)的模型與真實(shí)系統(tǒng)模型匹配的概率（稱(chēng)為模型概率）, 最后根據(jù)模型概率加權(quán)融合各濾波器估計(jì), 得到系統(tǒng)的最終估計(jì)［14］。 在交互式多模型算法的基礎(chǔ)上, 本文采用EKF 濾波構(gòu)建了IMM-EKF 濾波算法, 該算法采用三個(gè)濾波器來(lái)構(gòu)建多模型, 原理框圖如圖1所示。

圖1 交互式多模型算法原理框圖Fig.1 Block diagram of an interactive multiple models algorithm

（1）輸入交互

對(duì)給定的n個(gè)模型的初始狀態(tài)進(jìn)行輸入交互運(yùn)算, 定位目標(biāo)的初始狀態(tài)估計(jì)值為

狀態(tài)估計(jì)協(xié)方差如下

其中,

（2）狀態(tài)濾波

將上一步得到的初始狀態(tài)和協(xié)方差作為濾波器的輸入, 對(duì)每一個(gè)模型進(jìn)行EKF 濾波, 多個(gè)濾波器并行工作, 得到每個(gè)模型當(dāng)前時(shí)刻的狀態(tài)估計(jì)及其均方誤差陣, 時(shí)間更新過(guò)程和量測(cè)更新過(guò)程同標(biāo)準(zhǔn)EKF 算法。

（3）模型概率更新

模型概率的計(jì)算是假設(shè)檢驗(yàn)過(guò)程, 一般采用Bayes 假設(shè)檢驗(yàn)法, 原理是同時(shí)檢驗(yàn)各個(gè)濾波器的殘差, 狀態(tài)濾波后的新息為

新息協(xié)方差為

計(jì)算各模型的似然函數(shù)值為

式（24）中,為k時(shí)刻模型j的似然函數(shù)值,m為量測(cè)向量的維數(shù)。

采用Bayes 假設(shè)檢驗(yàn)方法進(jìn)行模型概率更新

（4）融合輸出

融合輸出即按照各濾波器估計(jì)值的概率加權(quán)融合, 計(jì)算各模型的聯(lián)合狀態(tài)估計(jì)和狀態(tài)估計(jì)協(xié)方差矩陣, 得到IMM 的最終輸出。 最終得到的狀態(tài)估計(jì)值表達(dá)式為

狀態(tài)估計(jì)值對(duì)應(yīng)的協(xié)方差為

3.2 基于IMM-EKF 的組合導(dǎo)航算法

本文將多模型的思想引入GNSS/SINS 融合算法中, 提出了基于IMM-EKF 的GNSS/SINS 組合導(dǎo)航多模型結(jié)構(gòu), 中心思想是將IMM 算法中的模型集設(shè)置為對(duì)應(yīng)不同量測(cè)噪聲協(xié)方差的EKF 濾波器,構(gòu)造出3 個(gè)測(cè)量噪聲協(xié)方差的濾波模型, 這樣可以避免傳統(tǒng)Kalman 濾波算法中需要事先知道量測(cè)噪聲統(tǒng)計(jì)特性的不足, 可以更好地匹配實(shí)際環(huán)境中的非線性輸入輸出關(guān)系。 本文提出的基于IMMEKF 的GNSS/SINS 組合導(dǎo)航算法原理框圖如圖2所示。

圖2 基于IMM-EKF 的組合導(dǎo)航系統(tǒng)原理框圖Fig.2 Block diagram of integrated navigation system based on IMM-EKF

4 仿真校驗(yàn)及結(jié)果分析

為了驗(yàn)證提出的IMM-EKF 算法的準(zhǔn)確性和自適應(yīng)性, 分別進(jìn)行了仿真校驗(yàn)和實(shí)際道路測(cè)試。

4.1 IMM-EKF 算法仿真校驗(yàn)

為了驗(yàn)證本文算法的量測(cè)噪聲自適應(yīng)跟蹤性能, 以GNSS/SINS 組合導(dǎo)航系統(tǒng)為例, 采用傳統(tǒng)的EKF 和本文算法進(jìn)行Matlab 仿真對(duì)比, 利用軌跡仿真器生成載體運(yùn)動(dòng)軌跡對(duì)應(yīng)的導(dǎo)航數(shù)據(jù), 包括加速、 減速、 勻速直行、 轉(zhuǎn)彎等運(yùn)動(dòng)狀態(tài), 時(shí)長(zhǎng)為900s。 MEMS 參數(shù)如表1 所示。

表1 MEMS 誤差參數(shù)Table 1 Error parameters of MEMS

為了更好地驗(yàn)證模型發(fā)生變化時(shí)IMM-EKF 算法的準(zhǔn)確性和魯棒性, 本次實(shí)驗(yàn)選擇在同一組數(shù)據(jù)中改變GNSS 接收機(jī)的量測(cè)噪聲協(xié)方差來(lái)模擬該情況。 將傳統(tǒng)EKF 的量測(cè)噪聲協(xié)方差設(shè)置如下

IMM-EKF 算法中設(shè)置了3 個(gè)模型, 3 個(gè)子濾波器對(duì)應(yīng)的量測(cè)噪聲協(xié)方差分別為R、 6R和12R。初始模型概率矩陣μk=［1/3 1/3 1/3］, 初始模型轉(zhuǎn)移概率矩陣為

按照實(shí)際動(dòng)態(tài)環(huán)境中GNSS 輸出誤差分布特點(diǎn), 將900s 的數(shù)據(jù)分成三段: 0s ～300s、 300s ～600s 和600s ～900s。 將GNSS 的輸出誤差模型在第一段時(shí)長(zhǎng)中設(shè)置為量測(cè)噪聲協(xié)方差陣為3R的零均值Gauss 白噪聲, 同樣第二段設(shè)置為6R, 第三段設(shè)置為9R。 三段中設(shè)置的量測(cè)噪聲協(xié)方差陣均大于傳統(tǒng)EKF 中設(shè)置的R, 但是仍處于IMM-EKF 量測(cè)噪聲協(xié)方差陣的覆蓋范圍之內(nèi), 以保證所設(shè)模型能夠通過(guò)加權(quán)融合準(zhǔn)確匹配當(dāng)前時(shí)刻的噪聲情況, 并且盡可能多地模擬不同噪聲下的估計(jì)結(jié)果。按照設(shè)置好的仿真條件, 分別使用EKF 和IMMEKF 對(duì)GNSS/SINS 進(jìn)行濾波融合, 并將輸出的結(jié)果與實(shí)際真實(shí)軌跡做差得到位置誤差與速度誤差曲線, 如圖3 ～圖8 所示。

圖3 東向速度誤差對(duì)比Fig.3 Comparison of east-facing speed errors

圖4 北向速度誤差對(duì)比Fig.4 Comparison of north-facing speed errors

圖5 天向速度誤差對(duì)比Fig.5 Comparison of celestial speed errors

圖6 緯度誤差對(duì)比Fig.6 Comparison of latitude errors

圖7 經(jīng)度誤差對(duì)比Fig.7 Comparison of longitude errors

圖8 高度誤差對(duì)比Fig.8 Comparison of height errors

從實(shí)驗(yàn)仿真結(jié)果可知, 由于EKF 設(shè)定的量測(cè)噪聲協(xié)方差固定為R, 先驗(yàn)噪聲協(xié)方差與實(shí)際濾波運(yùn)算過(guò)程中的協(xié)方差不一致, 導(dǎo)致相比于IMMEKF, EKF 的定位精度略低, 而IMM-EKF 由于能夠自適應(yīng)調(diào)整量測(cè)噪聲協(xié)方差矩陣, 因此能夠在一定程度上抑制量測(cè)噪聲變化對(duì)于濾波的影響,增強(qiáng)系統(tǒng)的魯棒性。

由表2 可知, 當(dāng)系統(tǒng)模型發(fā)生變化時(shí), 若噪聲協(xié)方差變化在IMM-EKF 算法模型集的覆蓋范圍內(nèi),則IMM-EKF 算法定位精度較EKF 更高, 誤差變化要更平緩。 由此可推廣至模型其他參數(shù)發(fā)生變化的情況, 只需設(shè)計(jì)相應(yīng)的模型集, 即可在一定程度上提高系統(tǒng)的魯棒性和定位精度。

表2 均方誤差結(jié)果Table 2 Result of mean square error

4.2 實(shí)地道路測(cè)試

為了進(jìn)一步驗(yàn)證本文算法的有效性和優(yōu)越性,設(shè)計(jì)了半物理車(chē)載實(shí)驗(yàn)。 采集真實(shí)車(chē)載環(huán)境下的傳感器數(shù)據(jù)（真實(shí)車(chē)載環(huán)境包括: 載體加速、 減速、變道、 停車(chē)等真實(shí)動(dòng)態(tài)環(huán)境）, 并通過(guò)高精度基準(zhǔn)導(dǎo)航系統(tǒng)作為參考進(jìn)行對(duì)比。 其中, SINS 數(shù)據(jù)由MEMS 慣性導(dǎo)航系統(tǒng)提供, GNSS 數(shù)據(jù)由自研制北斗導(dǎo)航接收機(jī)提供, 高精度基準(zhǔn)導(dǎo)航系統(tǒng)為Novatel 公司推出的SPAN-KVH1750 高精度組合導(dǎo)航系統(tǒng)。 上述設(shè)備的主要性能指標(biāo)如表3 所示, 跑車(chē)實(shí)驗(yàn)設(shè)備如圖9 所示, 跑車(chē)實(shí)驗(yàn)路線如圖10 所示。

圖9 道路測(cè)試實(shí)驗(yàn)設(shè)備Fig.9 Diagram of road test equipments

圖10 跑車(chē)實(shí)驗(yàn)路線Fig.10 Roadmap of vehicle experiment

表3 設(shè)備主要性能參數(shù)Table 3 Main performance parameters of the devices

跑車(chē)時(shí)長(zhǎng)300s, 數(shù)據(jù)通過(guò)上位機(jī)監(jiān)控存入計(jì)算機(jī), 然后分別進(jìn)行IMM-EKF 和EKF 算法數(shù)據(jù)融合離線仿真, 通過(guò)與高精度基準(zhǔn)對(duì)比得出誤差。圖11、 圖12 給出了在實(shí)際跑車(chē)測(cè)試中GNSS/SINS組合在使用EKF 和IMM-EKF 時(shí)的速度誤差對(duì)比曲線和位置誤差對(duì)比曲線, 圖13 給出了兩種算法估計(jì)出的二維位置坐標(biāo)與基準(zhǔn)所給的二維位置坐標(biāo)對(duì)比, 表4 給出了兩種算法的均方誤差對(duì)比。

表4 兩種算法均方誤差對(duì)比Table 4 Comparison of mean square error between two algorithms

圖11 速度誤差對(duì)比Fig.11 Comparison of velocity errors

圖12 位置誤差對(duì)比Fig.12 Comparison of position errors

圖13 兩種算法二維位置與實(shí)際二維位置對(duì)比Fig.13 Comparison of the two algorithms two-dimensional positions and the actual two-dimensional positions

由此可以得出, 傳統(tǒng)的EKF 由于先驗(yàn)信息的不準(zhǔn)確, 使得輸出的參數(shù)誤差波動(dòng)較大, 而IMMEKF 算法可以實(shí)現(xiàn)不同量測(cè)噪聲協(xié)方差之間的相互交互, 自適應(yīng)地調(diào)整模型集, 能夠在一定程度上降低量測(cè)粗差對(duì)濾波精度的影響, 改善了復(fù)雜環(huán)境下因量測(cè)噪聲統(tǒng)計(jì)特性未知多變引起的濾波 精度下降的問(wèn)題, 提高了組合導(dǎo)航定位精度。

5 結(jié)論

本文針對(duì)復(fù)雜環(huán)境下組合導(dǎo)航系統(tǒng)模型非線性和量測(cè)噪聲統(tǒng)計(jì)特性未知多變而引起的定位精度下降的問(wèn)題, 提出了一種基于EKF 的交互式多模型算法。 該算法突破了傳統(tǒng)算法定結(jié)構(gòu)的限制,通過(guò)模型集自適應(yīng)調(diào)整策略, 使得模型能夠快速匹配當(dāng)前的量測(cè)噪聲統(tǒng)計(jì)特性。 以GNSS/SINS 組合導(dǎo)航系統(tǒng)為例, 通過(guò)仿真實(shí)驗(yàn)和實(shí)地道路測(cè)試對(duì)本文算法進(jìn)行了驗(yàn)證, 結(jié)果表明: 本文算法能夠在一定程度上降低量測(cè)噪聲統(tǒng)計(jì)特性未知多變對(duì)組合導(dǎo)航濾波精度的影響, 增強(qiáng)了系統(tǒng)的魯棒性, 提高了系統(tǒng)的定位精度。