徐曉蘇，劉心雨

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

一種改進(jìn)的粒子濾波算法在SINS初始對(duì)準(zhǔn)中的應(yīng)用

徐曉蘇1,2，劉心雨1,2

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

在實(shí)際工程環(huán)境中，針對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）大失準(zhǔn)角初始對(duì)準(zhǔn)中噪聲統(tǒng)計(jì)特性未知的問題，設(shè)計(jì)了一種基于H∞濾波算法的魯棒無跡粒子濾波算法（RUPF）。通過將無跡卡爾曼濾波算法（UKF）和魯棒環(huán)節(jié)引入到粒子濾波(PF)的重要性密度函數(shù)中，得到了RUPF算法，提高了算法的魯棒性。通過半物理實(shí)驗(yàn)，將RUPF算法與無跡粒子濾波算法（UPF）在SINS靜基座大失準(zhǔn)角對(duì)準(zhǔn)中的性能進(jìn)行了比較，在不同實(shí)驗(yàn)條件下，航向失準(zhǔn)角精度至少提高了40%，對(duì)準(zhǔn)精度優(yōu)于0.05°，對(duì)準(zhǔn)時(shí)間減少了約 50 s。實(shí)驗(yàn)結(jié)果表明，RUPF算法可以以較高的精度和較快的速度完成大失準(zhǔn)角初始對(duì)準(zhǔn)，且對(duì)準(zhǔn)精度和對(duì)準(zhǔn)速度均優(yōu)于UPF算法。

魯棒無跡粒子濾波；捷聯(lián)慣性導(dǎo)航系統(tǒng)；初始對(duì)準(zhǔn)；大失準(zhǔn)角

在捷聯(lián)慣性導(dǎo)航（SINS）中，初始對(duì)準(zhǔn)是一項(xiàng)關(guān)鍵技術(shù)，其精度直接影響到慣性導(dǎo)航的精度。當(dāng)初始失準(zhǔn)角較小時(shí)，SINS 的誤差模型可以簡(jiǎn)化成線性模型，卡爾曼( Kalman) 濾波器是有效的濾波工具; 當(dāng)初始失準(zhǔn)角較大時(shí)，需建立 SINS非線性誤差模型，需利用非線性濾波器才能準(zhǔn)確估計(jì)出失準(zhǔn)角。

目前使用較為廣泛的非線性濾波器有三大類：擴(kuò)展卡爾曼濾波（EKF）[1]、無跡卡爾曼濾波（UKF）[2]和粒子濾波（PF）[3]。EKF算法是將非線性函數(shù)進(jìn)行泰勒級(jí)數(shù)展開并保留線性項(xiàng)以獲取線性模型，對(duì)于弱非線性系統(tǒng)可以取得較好的濾波效果，但對(duì)于強(qiáng)非線性系統(tǒng)濾波效果較差，EKF算法會(huì)不可避免地引入截?cái)嗾`差且雅可比矩陣計(jì)算較為復(fù)雜。UKF是利用UT變換得到一組 sigma采樣點(diǎn)，經(jīng)過非線性函數(shù)映射，得到隨機(jī)變量的均值與方差，這種算法避免了線性化誤差，具有較高的估計(jì)精度，但是隨著系統(tǒng)維數(shù)的增大，UKF算法估計(jì)精度急劇下降，甚至可能會(huì)發(fā)散。PF算法是一種基于隨機(jī)采樣的濾波方法，它是一個(gè)完全的非線性器，該方法直接根據(jù)概率密度計(jì)算條件均值，理論上不受模型線性和高斯假設(shè)的限制，可用于非線性非高斯的隨機(jī)系統(tǒng)。

PF算法的核心是合理選擇重要性密度函數(shù)，重要性密度函數(shù)與真實(shí)的密度越接近，濾波效果就越好，反之則越差，甚至有可能發(fā)散。文獻(xiàn)[4]和文獻(xiàn)[5]通過將粒子濾波與EKF或UKF組合起來的方法，改善粒子濾波器性能。在這些方法中，重要性密度函數(shù)由EKF或UKF來確定，這樣既可以解決粒子退化的問題，又能使粒子更新時(shí)獲得量測(cè)量的最新驗(yàn)后信息，有利于粒子移向似然比高的區(qū)域[6]。

然而以上濾波方法都需要知道噪聲的一些統(tǒng)計(jì)信息。但慣性系統(tǒng)的固有特性以及系統(tǒng)初始對(duì)準(zhǔn)時(shí)的實(shí)際工作環(huán)境具有隨機(jī)不確定性，因此其噪聲統(tǒng)計(jì)特性的數(shù)學(xué)描述具有明顯的不確定性，那么在噪聲統(tǒng)計(jì)信息未知情況下，濾波精度極有可能出現(xiàn)急劇下降甚至發(fā)散。針對(duì)模型的非線性以及未知模型噪聲統(tǒng)計(jì)特性的問題，本文將魯棒性較強(qiáng)的H∞濾波算法與無跡粒子濾波算法（UPF）相結(jié)合得到魯棒無跡粒子濾波算法（RUPF），用來提高初始對(duì)準(zhǔn)的估計(jì)精度與魯棒性。

1 簡(jiǎn)化UPF算法

由UKF產(chǎn)生PF的重要性密度函數(shù)稱為無跡粒子濾波器(UPF)，由UKF產(chǎn)生的重要性密度函數(shù)與真實(shí)狀態(tài)概率密度函數(shù)的支集重疊部分更大，估計(jì)精度較高。在這個(gè)算法中，每個(gè)量測(cè)量時(shí)刻采用UKF對(duì)粒子進(jìn)行迭代，并利用這些量測(cè)值對(duì)粒子進(jìn)行重采樣。這就好比同時(shí)運(yùn)用N個(gè)卡爾曼濾波器（每一個(gè)粒子對(duì)應(yīng)一個(gè)濾波器），在每次量測(cè)后再進(jìn)行一步重采樣。

設(shè)系統(tǒng)方程和量測(cè)方程分別為

式中：狀態(tài)方程為非線性方程，量測(cè)方程為線性方程；Xk是系統(tǒng)狀態(tài)向量； f(·)是非線性函數(shù)；Hk為量測(cè)陣；Vk為量測(cè)噪聲序列，Wk為系統(tǒng)激勵(lì)噪聲序列，其中Wk和 Vk為互不相關(guān)的高斯分布白噪聲，均值為零，方差陣分別為Qk和Rk。

文獻(xiàn)[7]說明了簡(jiǎn)化UKF算法，使用簡(jiǎn)化UKF算法產(chǎn)生PF的重要性密度函數(shù)，由此可以推出簡(jiǎn)化UPF算法。該算法步驟如下：

步驟 1：初值確定（k= 0）。根據(jù)先驗(yàn)概率密度p(X0)采樣生成粒子初始值X，i= 1,2,…,N 。

步驟2：簡(jiǎn)化UKF濾波（k=1,2…）。根據(jù)簡(jiǎn)化UKF算法對(duì)每個(gè)粒子進(jìn)行時(shí)間更新和量測(cè)更新，計(jì)算簡(jiǎn)化UKF濾波后的系統(tǒng)狀態(tài)估計(jì) 以及協(xié)方差Pki。

步驟3：重要性采樣（k=1,2…）。依據(jù)簡(jiǎn)化UKF濾波結(jié)果構(gòu)造建議分布函數(shù)：

步驟4：二次采樣。采用SIR法或殘差二次采樣法對(duì)原始粒子集進(jìn)行二次采樣，重新計(jì)算權(quán)重系數(shù)Wkj=，從而得到新的支撐粒子集：

步驟5：根據(jù)二次采樣粒子得到濾波值。

2 H∞濾波與RUPF算法

2.1 H∞濾波算法

H∞濾波算法是從H∞線性控制理論的基礎(chǔ)上發(fā)展起來的，它不需要知道環(huán)境噪聲的統(tǒng)計(jì)特性等先驗(yàn)信息。其本質(zhì)就是建立一個(gè)從干擾輸入到誤差輸出的H∞范數(shù)最小的濾波器，用于解決濾波系統(tǒng)和外界干擾存在不確定性的問題，因此該算法對(duì)系統(tǒng)模型誤差和外界干擾具有很強(qiáng)的魯棒性[8-9]?？紤]如下隨機(jī)線性離散時(shí)間系統(tǒng)：

Xk是系統(tǒng)狀態(tài)向量；Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣；Hk為量測(cè)陣； Vk為量測(cè)噪聲序列；Wk為系統(tǒng)激勵(lì)噪聲序列； Lk是自定義矩陣（假設(shè)它滿秩）。根據(jù)文獻(xiàn)[10]，H∞濾波算法狀態(tài)方程如下：

步驟1：初值確定：

步驟2：狀態(tài)估計(jì)：

步驟3：估計(jì)狀態(tài)線性組合：

步驟4：濾波增益：

步驟5：計(jì)算R,ek：

步驟6：計(jì)算Riccati方程：

為了將UPF濾波算法應(yīng)用于H∞濾波器，首先對(duì)線性H∞濾波方法中實(shí)現(xiàn)狀態(tài)估計(jì)協(xié)方差陣遞推的Riccati方程進(jìn)行轉(zhuǎn)換，狀態(tài)協(xié)方差陣Pk的H∞魯棒更新形式可以寫成[11]：

下標(biāo)k|k-1表示由k-1時(shí)刻求得k時(shí)刻的一步預(yù)測(cè)。

矩陣這樣變換后，H∞濾波便與卡爾曼濾波具有相同的形式，不同之處是在卡爾曼濾波算法基礎(chǔ)上引入了γ來調(diào)節(jié)系統(tǒng)的魯棒性，只有當(dāng)γ滿足如下條件時(shí)，非線性的H∞濾波算法才能夠正常工作。

γ是調(diào)節(jié)算法魯棒性與穩(wěn)定性的“調(diào)節(jié)因子”。如果γ過大，則算法的魯棒性將會(huì)降低；如果γ取的過小，在濾波過程中，有可能無法滿足上式中的限制條件，造成濾波器無法正常工作。

對(duì)于非線性系統(tǒng)模型，利用UPF算法求解H∞濾波器中隨機(jī)變量經(jīng)非線性變換之后的均值和方差，就可得到RUPF非線性濾波算法。

2.2 RUPF算法

設(shè)系統(tǒng)方程和量測(cè)方程如式(1)所示，不同的是Wk和Vk分別為統(tǒng)計(jì)特性未知的系統(tǒng)噪聲和觀測(cè)噪聲。假設(shè)Wk和Vk滿足：

其中：δij為δ函數(shù)；Qk為系統(tǒng)噪聲的方差陣，Rk為量測(cè)噪聲的方差陣，實(shí)際上這兩者統(tǒng)計(jì)特性未知，這只是估計(jì)值，使用H∞濾波算法可以提高算法的魯棒性，使估計(jì)值接近真實(shí)值，以適應(yīng)不同的環(huán)境噪聲統(tǒng)計(jì)特性。

RUPF濾波算法流程如下：

步驟1：初始化

假設(shè)初始狀態(tài)的概率密度函數(shù)p(X0) 已知，方差陣為P0?；趐(X0)生成粒子初值 χ，并選取ω=p(χ0(i))，i=1,2,…,N。選擇N時(shí)需綜合考慮計(jì)算量與估計(jì)精度。為了簡(jiǎn)化計(jì)算，假設(shè)每個(gè)粒子都服從正太分布N(,P0i)，其中，0(i)=χ0(i)，=P0。對(duì)于i=1,2,…,N ，k=1,2,3,…執(zhí)行。

步驟2：對(duì)于采樣時(shí)刻 k=1,2,3,…進(jìn)行RUKF濾波計(jì)算：

1）計(jì)算σ樣本點(diǎn)：

2）時(shí)間更新

正態(tài)分布β=2.

3）量測(cè)更新

步驟3：選取重要性函數(shù)

步驟4：計(jì)算如下權(quán)重系數(shù)：

步驟6：根據(jù)二次采樣粒子計(jì)算濾波值：

RUPF算法通過調(diào)整γ值來調(diào)整Pk，以犧牲一定的精度為代價(jià)來?yè)Q取濾波算法的魯棒性。參數(shù)γ控制狀態(tài)估計(jì)在最不利條件下的估計(jì)誤差，約束水平γ越小，則系統(tǒng)的魯棒性越強(qiáng)。

3 SINS大失準(zhǔn)角非線性誤差模型

本文將RUPF算法應(yīng)用于捷聯(lián)慣導(dǎo)系統(tǒng)中。本節(jié)將依據(jù)參考文獻(xiàn)[12]建立以 SINS誤差方程為基礎(chǔ)的組合導(dǎo)航系統(tǒng)非線性誤差方程和線性量測(cè)方程。

選取“東北天”地理坐標(biāo)系為導(dǎo)航坐標(biāo)系n系；選取載體“右前上”坐標(biāo)系為載體坐標(biāo)系b系。n系先后經(jīng)過3次歐拉角轉(zhuǎn)動(dòng)至b系，三個(gè)歐拉角記為航向角，縱搖角，橫搖角；n系與b系之間的姿態(tài)矩陣記為 C；真實(shí)姿態(tài)角記為；真實(shí)速度記為；真實(shí)地理坐標(biāo)系為P=[L λ H]T，其中，L是緯度，λ是經(jīng)度，H是高度；SINS解算出的姿態(tài)角記為，速度記為，地理坐標(biāo)系為；SINS解算出的數(shù)學(xué)平臺(tái)記為n′系，n′系與 b系之間的姿態(tài)矩陣記為 C；記姿態(tài)角誤差為；速度誤差為，位置誤差為。則非線性誤差模型如下：

cφi和sφi分別代表cosφi和sinφi，i=n,u,e。

非線性誤差方程建立過程如下：

以采樣周期T作為濾波周期，可以使用四階龍格-庫(kù)塔積分方法，以T為步長(zhǎng)將其離散化，記離散后狀態(tài)濾波方程為

線性量測(cè)方程建立過程如下：

對(duì)SINS的水平速度輸出作如下分解：

式中：VE和VN為載體的理想速度。若載體有線運(yùn)動(dòng)時(shí)，該速度可由GPS等設(shè)備提供。在靜基座下載體無線運(yùn)動(dòng)，因此

同理對(duì)SINS的緯度輸出和經(jīng)度輸出做如下分解：

式中，L和λ為載體的理想緯度和經(jīng)度，該位置信息可由 GPS等設(shè)備提供。在靜基座下，載體無位置移動(dòng)且載體所在的地理位置精確已知，即L和λ已知。所以量測(cè)方程為

式中，V為噪聲陣，量測(cè)矩陣為

同樣以T作為濾波周期，并以T作為步長(zhǎng)進(jìn)行簡(jiǎn)單離散化，得離散化后的量測(cè)方程為

綜上，由狀態(tài)方程和量測(cè)方程組成如下非線性濾波方程為

4 半物理實(shí)驗(yàn)結(jié)果與分析

SINS解算模塊采集到慣性測(cè)量單元（IMU）模塊輸出的陀螺輸出值和加速度計(jì)輸出值進(jìn)行捷聯(lián)解算，得到姿態(tài)角、姿態(tài)矩陣、速度、位置等信息；靜基座下載體真實(shí)速度為0，且真實(shí)位置信息已知。將SINS輸出的信息輸入到RUPF濾波器中，進(jìn)行信息的濾波處理，系統(tǒng)方案如圖1所示。

為了進(jìn)一步驗(yàn)證RUPF算法的有效性，利用實(shí)驗(yàn)室的光纖陀螺捷聯(lián)慣性系統(tǒng)設(shè)備（FOSN）在三軸高精度轉(zhuǎn)臺(tái)上進(jìn)行實(shí)際環(huán)境下的工程驗(yàn)證。三軸轉(zhuǎn)臺(tái)所在實(shí)驗(yàn)室的地理位置為北緯32.057 305°N，東經(jīng)118.786 389 °E。分別采用UPF算法和RUPF算法進(jìn)行SINS靜基座大失準(zhǔn)角初始對(duì)準(zhǔn)的半物理實(shí)驗(yàn)，觀察實(shí)際情況下RUPF算法的效果。實(shí)驗(yàn)時(shí)，將 FOSN固聯(lián)在三軸轉(zhuǎn)臺(tái)上，標(biāo)定出FOSN與轉(zhuǎn)臺(tái)之間的安裝誤差角，采集FOSN捷聯(lián)慣性儀表敏感的角速度信息和加速度信息，利用所設(shè)計(jì)的算法完成導(dǎo)航運(yùn)算。有以下四種情況：

圖1 系統(tǒng)方案圖Fig.1 System schematic diagram

當(dāng)真實(shí)的初始姿態(tài)角為θ=0°、γ=0°、Ψ=0°時(shí)，初始姿態(tài)角為θ=? 5°、γ=? 5°、Ψ=? 20°和θ=? 15°、γ=? 15°、Ψ=? 30°。

當(dāng)真實(shí)的初始姿態(tài)角為θ=0°、γ=0°、Ψ=90°時(shí)，初始姿態(tài)角為θ=? 5°、γ=? 5°、Ψ=? 115°和θ=? 10°、γ=? 10°、Ψ=? 130°。

利用UPF、RUPF算法進(jìn)行SINS靜基座大失準(zhǔn)角初始對(duì)準(zhǔn)，對(duì)準(zhǔn)時(shí)間為600 s。兩種算法在上述兩種大失準(zhǔn)角情況下的失準(zhǔn)角估計(jì)誤差統(tǒng)計(jì)表如表1和表2所示，其中均值與標(biāo)準(zhǔn)差是用在對(duì)準(zhǔn)結(jié)束后100 s內(nèi)，即600 s至700 s時(shí)間段內(nèi)數(shù)據(jù)計(jì)算出的。四種情況下失準(zhǔn)角估計(jì)誤差曲線分別如圖2(a)、2(b) 和圖3(a)、3(b)所示。圖 2和表 1真實(shí)的初始姿態(tài)角為θ=0°、γ=0°、Ψ=0°；圖 3和表 2真實(shí)的初始姿態(tài)角為θ=0°、γ=0°、Ψ=90°。

由圖2可知，在靜基座大失準(zhǔn)角情況下，當(dāng)真實(shí)的初始姿態(tài)角為θ=0°、γ=0°、Ψ=0°，設(shè)置初始姿態(tài)角為θ=?5°、γ=?5°、Ψ=? 20°時(shí)，這兩種方法水平失準(zhǔn)角誤差曲線較為接近，對(duì)準(zhǔn)精度高，時(shí)間短，水平對(duì)準(zhǔn)精度在 0.012°以內(nèi)，水平失準(zhǔn)角大約在 200 s后基本收斂。但在航向失準(zhǔn)角方面RUPF算法精度明顯優(yōu)于UPF算法，RUPF算法誤差在0.027°而 UPF算法誤差在0.055°，精度提高了約50%。RUPF在對(duì)準(zhǔn)時(shí)間約為350 s時(shí)航向失準(zhǔn)角基本收斂，而UPF在400 s時(shí)收斂；當(dāng)設(shè)置初始失準(zhǔn)角為θ=?15°、γ=?15°、Ψ=? 30°時(shí)，這兩種方法水平失準(zhǔn)角誤差曲線也較為接近，但航向失準(zhǔn)角RUPF算法精度明顯優(yōu)于UPF算法，

RUPF算法誤差在0.042°而 UPF算法誤差在0.083°，精度提高了約50%。RUPF在對(duì)準(zhǔn)時(shí)間約為400 s時(shí)航向失準(zhǔn)角基本收斂，而UPF在450 s時(shí)收斂。

表1 失準(zhǔn)角估計(jì)誤差統(tǒng)計(jì)Tab.1 Statistics on misalignment estimation errors of UPF and RUPF

表2 失準(zhǔn)角估計(jì)誤差統(tǒng)計(jì)Tab.2 Statistics on misalignment estimation errors of UPF and RUPF

圖2(a) 失準(zhǔn)角估計(jì)誤差Fig.2(a) Estimation errors of misalignment angle

圖3(a) 失準(zhǔn)角估計(jì)誤差Fig.3(a) Estimation errors of misalignment angle

圖2(b) 失準(zhǔn)角估計(jì)誤差Fig.2(b) Estimation errors of misalignment angle

圖3(b) 失準(zhǔn)角估計(jì)誤差Fig.3(b) Estimation errors of misalignment angle

與上述結(jié)論一樣，由圖3可知，當(dāng)真實(shí)的初始姿態(tài)角為θ=0°、γ=0°、Ψ=90°，設(shè)置初始姿態(tài)角為θ=?5°、γ=?5°、Ψ=? 115°時(shí)，這兩種方法水平失準(zhǔn)角誤差曲線較為接近，但在航向失準(zhǔn)角方面RUPF算法精度明顯優(yōu)于UPF算法，RUPF算法誤差在0.0328°，而 UPF算法誤差在0.0571°，精度提高了約42.5%。RUPF在對(duì)準(zhǔn)時(shí)間約為400 s時(shí)，航向失準(zhǔn)角基本收斂，而UPF在450 s時(shí)收斂；當(dāng)設(shè)置初始姿態(tài)角為θ=?5°、γ=?5°、Ψ=? 115°時(shí)，這兩種方法水平失準(zhǔn)角誤差曲線較為接近，但在航向失準(zhǔn)角方面RUPF算法精度明顯優(yōu)于UPF算法，RUPF算法誤差在0.0499°，而 UPF算法誤差在0.0909°，精度提高了約40%。RUPF在對(duì)準(zhǔn)時(shí)間約為450 s時(shí)航向失準(zhǔn)角基本收斂，而UPF在500 s時(shí)收斂。當(dāng)失準(zhǔn)角增大后，兩種算法的對(duì)準(zhǔn)收斂速度都在變慢，精度也在下降，但RUPF算法下降較小。

通過以上分析可以發(fā)現(xiàn)，在實(shí)際工程環(huán)境中，當(dāng)失準(zhǔn)角較大的情況下，RUPF算法可以以較高的精度和較快的速度完成初始對(duì)準(zhǔn)，且對(duì)準(zhǔn)精度特別是航向角對(duì)準(zhǔn)精度高于UPF算法，對(duì)準(zhǔn)速度也更快。

5 結(jié) 論

本文采用魯棒無跡粒子濾波(RUPF)算法對(duì)SINS靜基座大失準(zhǔn)角初始對(duì)準(zhǔn)進(jìn)行了研究。魯棒環(huán)節(jié)的引入使得RUPF算法具有很強(qiáng)的魯棒性。在半物理實(shí)驗(yàn)中將RUPF算法與UPF算法進(jìn)行了對(duì)比，實(shí)驗(yàn)結(jié)果表明，在實(shí)際工程環(huán)境中，當(dāng)系統(tǒng)噪聲陣和觀測(cè)噪聲陣無法準(zhǔn)確獲知并且失準(zhǔn)角較大時(shí)，RUPF算法的初始對(duì)準(zhǔn)精度較高，對(duì)準(zhǔn)速度較快，并且該算法對(duì)準(zhǔn)誤差，特別是航向角對(duì)準(zhǔn)誤差，遠(yuǎn)小于UPF算法。因此該算法具有很強(qiáng)的工程應(yīng)用價(jià)值。

（Reference）：

[1] Sebesta K D, Boizot N. A real-time adaptive high-gain EKF, applied to a quadcopter inertial navigation system [J]. IEEE Transactions on Industrial Electronics, 2014, 61(1): 495-503.

[2] Li W, Wang J, Lu L, et al. A novel scheme for DVL-aided SINS in-motion alignment using UKF techniques[J]. Sensors, 2013, 13(1): 1046-1063.

[3] Chen Z, Qu Y, Zhang T, et al. Hybrid adaptive particle swarm optimized particle filter for integrated navigation system[J]. Computer Modeling in Engineering & Sciences, 2015, 106(6): 379-393.

[4] Chen X, Shen C, Zhao Y. Study on GPS/INS system using novel filtering methods for vessel attitude determination [J]. Mathematical Problems in Engineering, 2013(1): 289-325.

[5] Jwo D J, Yang C F, Chuang C H, et al. A novel design for the ultra-tightly coupled GPS/INS navigation system[J]. Journal of Navigation, 2012, 65(4): 717-747.

[6] 秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 2011. Qin Yong-yuan, Zhang Hong-yue,Wang Shu-hua. Kalman filter and the principle of integrated navigation[M]. Xi’an, China: Northwestern Polytechnical University Press, 2011.

[7] 嚴(yán)恭敏, 嚴(yán)衛(wèi)生, 徐德民. 簡(jiǎn)化UKF濾波在SINS大失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2008, 16(3): 253-264. Yan Gong-min, Yan Wei-sheng, Xu De-min. Application of simplified UKF in SINS initial alignment for large misalignment angles[J]. Journal of Chinese Inertial Technology, 2008, 16(3): 253-264.

[8] Wan-xin S. Application of H∞ filtering algorithm in SINS/GPS integrated navigation system[C]//2014 2nd International Conference on Information Technology and Electronic Commerce. IEEE, 2014: 72-76.

[9] Yu F, Lv C, Dong Q. A novel robust H∞ filter based on Krein space theory in the SINS/CNS attitude reference system[J]. Sensors, 2016, 16(3): 396.

[10] Liu X, Xu X, Wang L, et al. H∞filter for flexure deformation and lever arm effect compensation in M/S INS integration[J]. International Journal of Naval Architecture and Ocean Engineering, 2014, 6(3): 626-637.

[11] Einicke G A, White L B. Robust extended Kalman filtering[J]. IEEE Transactions on Signal Processing, 1999, 47(9): 2596-2599.

[12] 孫進(jìn), 徐曉蘇, 劉義亭, 等. 基于自適應(yīng)無跡粒子濾波的SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2016, 24(2): 154-159. Sun Jin, Xu Xiao-su, Liu Yi-ting, et al. Initial alignment of large azimuth misalignment in SINS based on adaptive unscented particle filter[J]. Journal of Northwestern Polytechnical University, 2016, 24(2): 154-159.

Improved particle filter algorithm in SINS initial alignment

XU Xiao-su1,2, LIU Xin-yu1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China; 2. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China)

In real engineering environments, the noise statistical characteristics are unknown in the initial alignment of SINS with large misalignment angle. To solve this problem, a RUPF algorithm is designed based on H∞filtering algorithm. By combining UKF algorithm and robust link into importance density function in PF, the RUPF algorithm is obtained to improve the robustness of this algorithm. By means of emi-physical experiment, the filter performance of RUPF and UPF in SINS initial alignment on a static base is compared with that of large misalignment angles under various experimental conditions, which show that the accuracy of heading misalignment is increased by at least 40%, the alignment accuracy is better than 0.05°, and the alignment time is reduced about 50 s. These results show that the RUPF can realize the initial alignment of SINS with large misalignment angles, whose alignment accuracy and alignment speed are higher than those of UPF.

robust unscented particle filter; SINS; initial alignment; large misalignment angle

U666.1

：A

2016-03-30；

：2016-04-12

國(guó)家自然科學(xué)基金項(xiàng)目（51175082，61473085）資助

徐曉蘇（1961—），男，博士生導(dǎo)師，從事測(cè)控技術(shù)與導(dǎo)航定位領(lǐng)域的研究。E-mail: xxs@seu.edu.cn

1005-6734(2016)03-0299-07

10.13695/j.cnki.12-1222/o3.2016.03.005