基于強(qiáng)跟蹤濾波器的SINS大失準(zhǔn)角初始對(duì)準(zhǔn)新方法

彭 卓，張 嶸，郭美鳳，劉 剛，羅壽紅

（1. 清華大學(xué) 精密儀器系，北京 100084；2. 中國(guó)航天員科研訓(xùn)練中心，北京 100094）

基于強(qiáng)跟蹤濾波器的SINS大失準(zhǔn)角初始對(duì)準(zhǔn)新方法

彭 卓1,2，張 嶸1，郭美鳳1，劉 剛1，羅壽紅1

（1. 清華大學(xué) 精密儀器系，北京 100084；2. 中國(guó)航天員科研訓(xùn)練中心，北京 100094）

捷聯(lián)慣性導(dǎo)航系統(tǒng)靜基座初始對(duì)準(zhǔn)時(shí)一般先進(jìn)行粗對(duì)準(zhǔn)，使失準(zhǔn)角縮小到一定范圍內(nèi)從而滿足小失準(zhǔn)角假設(shè)下的線性誤差模型，然后再進(jìn)行精對(duì)準(zhǔn)。在不進(jìn)行粗對(duì)準(zhǔn)時(shí)失準(zhǔn)角一般為大角度，需要采用復(fù)雜的非線性誤差模型和非線性濾波方法。研究發(fā)現(xiàn)通過(guò)設(shè)置合理的誤差協(xié)方差矩陣初值，采用反饋校正濾波結(jié)構(gòu)，并引入強(qiáng)跟蹤濾波算法可以在大失準(zhǔn)角情況下既無(wú)需粗對(duì)準(zhǔn)，又無(wú)需采用非線性模型來(lái)實(shí)現(xiàn)精對(duì)準(zhǔn)。仿真結(jié)果表明，該方法可以實(shí)現(xiàn)大失準(zhǔn)角初始對(duì)準(zhǔn)，魯棒性好，在任意姿態(tài)初值下都可以使航向角在300 s內(nèi)收斂到0.05°的理論極限精度，與小失準(zhǔn)角精對(duì)準(zhǔn)方法的速度和精度相當(dāng)?shù)∪チ舜謱?duì)準(zhǔn)因而耗時(shí)更短，與無(wú)跡卡爾曼濾波在600 s時(shí)才收斂到0.5°的速度相比大為改善。

捷聯(lián)慣性導(dǎo)航；初始對(duì)準(zhǔn)；大失準(zhǔn)角；強(qiáng)跟蹤濾波器

捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System，SINS）本身是一個(gè)積分系統(tǒng)，積分初值對(duì)導(dǎo)航結(jié)果的影響巨大。初始對(duì)準(zhǔn)就是在導(dǎo)航開始前確定載體初始姿態(tài)的過(guò)程，對(duì)導(dǎo)航精度至關(guān)重要。SINS靜基座初始對(duì)準(zhǔn)通常采用標(biāo)準(zhǔn)卡爾曼濾波器（Kalman Filter，KF），濾波模型采用小失準(zhǔn)角假設(shè)下的線性誤差模型。在實(shí)際使用中，為了滿足小失準(zhǔn)角假設(shè)，必須先進(jìn)行粗對(duì)準(zhǔn)，使姿態(tài)誤差縮小到一定范圍內(nèi)，然后才能采用KF進(jìn)行精對(duì)準(zhǔn)。常用的粗對(duì)準(zhǔn)方法為解析式粗對(duì)準(zhǔn)[1]，為了提高精度通常需要取一段時(shí)間的平均值，這不僅增加了步驟，還增加了時(shí)間。如果不進(jìn)行粗對(duì)準(zhǔn)，失準(zhǔn)角一般為大角度，此時(shí)線性誤差模型已經(jīng)失效，需要采用非線性誤差模型和非線性濾波方法。例如劉海鵬采用擴(kuò)展卡爾曼濾波（ExtendedKalman Filter，EKF）和無(wú)跡卡爾曼濾波（Unscented Kalman Filter，UKF）進(jìn)行大失準(zhǔn)角初始對(duì)準(zhǔn)并仿真驗(yàn)證了隨著失準(zhǔn)角的增大UKF比EKF估計(jì)精度更高[2]；龍瑞采用中心差分卡爾曼濾波器（Central Difference Kalman Filter，CDKF）并仿真驗(yàn)證了在大失準(zhǔn)角條件下CDKF比EKF和UKF具有更高的精度[3]；Jamshaid提出二階分離插值濾波器（Second-order Divided Difference filter，DDF2）在航向大失準(zhǔn)角初始對(duì)準(zhǔn)中能夠獲得比EKF和UKF更高的姿態(tài)精度[4]；Wu采用基于奇異值分解的容積卡爾曼濾波器（Cubature Kalman Filter，CKF）進(jìn)行大失準(zhǔn)角初始對(duì)準(zhǔn)獲得了比UKF更高的精度和更快的速度[5]。但是，以上這些非線性濾波方法都存在兩個(gè)問(wèn)題：一是算法復(fù)雜，計(jì)算量比標(biāo)準(zhǔn)卡爾曼濾波大的多；二是非線性濾波都存在近似系統(tǒng)模型或者近似概率分布的問(wèn)題，不是最優(yōu)濾波器，特別是在失準(zhǔn)角已經(jīng)滿足小角度假設(shè)時(shí)非線性濾波不如標(biāo)準(zhǔn)卡爾曼濾波精度高。針對(duì)以上問(wèn)題，本文提出一種大失準(zhǔn)角初始對(duì)準(zhǔn)新方法，該方法仍然采用線性模型，通過(guò)設(shè)置合理的濾波初值，采用反饋校正并創(chuàng)新性的引入強(qiáng)跟蹤濾波器來(lái)實(shí)現(xiàn)大失準(zhǔn)角情況下的直接精對(duì)準(zhǔn)。該方法與傳統(tǒng)粗對(duì)準(zhǔn)+精對(duì)準(zhǔn)方法相比不僅省去了粗對(duì)準(zhǔn)，節(jié)省了時(shí)間，還保持了標(biāo)準(zhǔn)卡爾曼濾波器能達(dá)到的精度，與一般非線性濾波方法相比不僅不需要進(jìn)行復(fù)雜的非線性濾波計(jì)算，還具有更高的精度、更快的速度和更小的計(jì)算量，是一種非常實(shí)用的大失準(zhǔn)角初始對(duì)準(zhǔn)解決方案。

1 系統(tǒng)模型

一般所謂的失準(zhǔn)角指的是n系到n′系的小角度旋轉(zhuǎn)矢量，與轉(zhuǎn)動(dòng)次序無(wú)關(guān)。而在大失準(zhǔn)角情況下，根據(jù)有限轉(zhuǎn)動(dòng)的不可交換性，需要定義大失準(zhǔn)角的轉(zhuǎn)動(dòng)次序。本文定義大失準(zhǔn)角的轉(zhuǎn)動(dòng)次序?yàn)椋簄系繞z軸轉(zhuǎn)?z角到n1系，n1系再繞x1軸轉(zhuǎn)?x角到n2系，n2系再繞y2軸轉(zhuǎn)?y角到n′系。記大失準(zhǔn)角可以推出大失準(zhǔn)角情況下 SINS姿態(tài)誤差和速度誤差方程分別如式(1)和式(2)所示[6]：

2 算法原理

2.1 反饋校正技術(shù)

以上卡爾曼濾波模型是在小失準(zhǔn)角假設(shè)下推導(dǎo)出來(lái)的，要用它來(lái)解決大失準(zhǔn)角非線性模型的初始對(duì)準(zhǔn)問(wèn)題，首要解決的問(wèn)題是如何讓系統(tǒng)迅速收斂到小失準(zhǔn)角范圍內(nèi)。聯(lián)想到在羅經(jīng)法對(duì)準(zhǔn)中，雖然羅經(jīng)對(duì)準(zhǔn)算法是在小失準(zhǔn)角假設(shè)下推導(dǎo)出來(lái)的，但是在大失準(zhǔn)角時(shí)依然適用[7]，究其原因在于在大失準(zhǔn)角情況下，雖然線性模型中的羅經(jīng)項(xiàng)與真實(shí)的非線性羅經(jīng)項(xiàng)不同，但是它們反饋之后對(duì)系統(tǒng)的校正方向是相同的，因此通過(guò)反饋可以逐漸將系統(tǒng)“拉回”到小失準(zhǔn)角。受此啟發(fā)，我們采用帶反饋校正的卡爾曼濾波器來(lái)輔助大失準(zhǔn)角初始對(duì)準(zhǔn)。反饋校正是一種在動(dòng)基座初始對(duì)準(zhǔn)和組合導(dǎo)航中經(jīng)常被采用的系統(tǒng)編排方式[8]，它是指在由卡爾曼濾波器得到系統(tǒng)誤差的估計(jì)值后，將估計(jì)結(jié)果反饋回系統(tǒng)中，校正系統(tǒng)的力學(xué)編排方程。

在反饋校正時(shí)如果不加區(qū)分的反饋所有狀態(tài)會(huì)由于某些狀態(tài)的估計(jì)效果不佳而影響對(duì)準(zhǔn)的精度。有學(xué)者提出通過(guò)可觀測(cè)度給反饋狀態(tài)加權(quán)決定其反饋程度可以改善反饋校正的效果[9]。因此，在大失準(zhǔn)角初始對(duì)準(zhǔn)中，我們只反饋校正我們希望迅速收斂的失準(zhǔn)角?和觀測(cè)量δVn，而不反饋可觀測(cè)性較差且對(duì)模型不確定性影響較小的狀態(tài)εb和▽b。此外，反饋系數(shù)對(duì)系統(tǒng)輸出也有一定的影響，反饋系數(shù)小系統(tǒng)輸出會(huì)比較平滑，但是收斂時(shí)間較長(zhǎng)，反饋系數(shù)大系統(tǒng)在初始階段波動(dòng)較劇烈，但是收斂較快，為了使大失準(zhǔn)角迅速收斂這里選取反饋系數(shù)為最大值1。

2.2 P0陣的設(shè)置

標(biāo)準(zhǔn)卡爾曼濾波器的公式如式(7)所示：

當(dāng)卡爾曼濾波器運(yùn)行一段時(shí)間后，Pk-1矩陣將趨于一個(gè)小量，一般情況下R＞＞Q，由式(7)可以看出此時(shí)Pk/k-1＜＜R，Kk也趨近于一個(gè)小量，這將導(dǎo)致新息很難進(jìn)入到估計(jì)結(jié)果中去，濾波估計(jì)對(duì)系統(tǒng)的修正變得微乎其微，如果這時(shí)系統(tǒng)還未收斂到小失準(zhǔn)角，那么就很難再繼續(xù)收斂了，將導(dǎo)致反饋校正失敗。從這個(gè)角度來(lái)講，P矩陣反映了反饋校正的動(dòng)力，P越大反饋校正越充分。為了使反饋校正有效的將系統(tǒng)由大失準(zhǔn)角“拉回”到小失準(zhǔn)角，就必須提供足夠大的P矩陣，一個(gè)最直接的辦法是選用足夠大的P0。考慮到系統(tǒng)12維狀態(tài)的可觀測(cè)性以及對(duì)反饋校正的貢獻(xiàn)各不相同，需要進(jìn)一步分析P0中各分量的設(shè)置。

δV是觀測(cè)量，在濾波開始階段主要依靠觀測(cè)量來(lái)提供信息，所以P0中的δV應(yīng)該設(shè)置的盡量大一些，當(dāng)δV足夠大時(shí)有即P1會(huì)迅速收斂到與P0無(wú)關(guān)的R水平，所以δV大了是沒(méi)有壞處的。在濾波初始階段?的誤差很大，主要依靠觀測(cè)量來(lái)估計(jì)?，所以P0中的?也應(yīng)設(shè)置為較大的值，同時(shí)為了避免隨著濾波進(jìn)行?迅速變小，從而失去對(duì)大失準(zhǔn)角反饋校正的“動(dòng)力”，P0中的?應(yīng)設(shè)置的足夠大。P0中的ε和▽不宜設(shè)置過(guò)大，過(guò)大會(huì)導(dǎo)致過(guò)分依賴量測(cè)來(lái)估計(jì)ε和▽，但是因?yàn)棣藕通尣煌耆捎^測(cè)，依靠量測(cè)估計(jì)出來(lái)的ε和▽是發(fā)散的，可能引起很大誤差，同時(shí)ε和▽也不宜太小，太小會(huì)影響ε和▽中可觀測(cè)分量的收斂速度。高精度陀螺和加表的常值零偏通常不大，ε和▽的初值設(shè)為零已經(jīng)很接近真實(shí)值了，所以P0中的ε和▽可根據(jù)先驗(yàn)知識(shí)適當(dāng)設(shè)置的小一些，例如（0.1～1）倍的經(jīng)驗(yàn)值，▽的可觀測(cè)性更弱一些所以可以設(shè)置的更小一些。綜合考慮，推薦P0中的δV設(shè)置為10 m/s，?設(shè)置為1000o，ε設(shè)置為先驗(yàn)知識(shí)的0.5倍，▽設(shè)置為先驗(yàn)知識(shí)的0.1倍。

2.3 強(qiáng)跟蹤濾波器

僅僅依靠調(diào)節(jié) P0陣來(lái)解決大失準(zhǔn)角初始對(duì)準(zhǔn)問(wèn)題是不可靠的，有時(shí)候并不能將系統(tǒng)“拉回”到小失準(zhǔn)角，這一點(diǎn)在后面的仿真中可以看到，因此需要一種能夠在線調(diào)節(jié)P矩陣的方法，強(qiáng)跟蹤濾波器（Strong Tracking Filter，STF）是一個(gè)很好的解決方案。強(qiáng)跟蹤濾波器最早由周東華教授提出，強(qiáng)跟蹤只是指它的性能，它實(shí)際上是一種自適應(yīng)的指數(shù)漸消記憶卡爾曼濾波器。指數(shù)漸消是指通過(guò)引入漸消因子來(lái)使濾波器的記憶長(zhǎng)度按照指數(shù)速度衰減，以提高對(duì)新近量測(cè)的利用程度。強(qiáng)跟蹤濾波器通過(guò)新息正交原理來(lái)自適應(yīng)的調(diào)節(jié)漸消因子，使得濾波器具有更強(qiáng)的關(guān)于模型不確定性的魯棒性，更強(qiáng)的關(guān)于突變狀態(tài)的跟蹤能力，在故障檢測(cè)、自適應(yīng)控制等領(lǐng)域被廣泛應(yīng)用[10-11]。

早期使用的強(qiáng)跟蹤濾波器中對(duì)所有的狀態(tài)采用相同的漸消因子[12]，稱之為單一漸消因子強(qiáng)跟蹤濾波器（Single Fading Strong Tracking Filter，SFSTF）。仿真表明在初始對(duì)準(zhǔn)中采用SFSTF會(huì)帶來(lái)嚴(yán)重錯(cuò)誤，究其原因在于并不是12個(gè)狀態(tài)都存在嚴(yán)重“突變”，不同狀態(tài)對(duì)模型不確定性的貢獻(xiàn)差異巨大，因此必須對(duì)不同狀態(tài)施加不同的漸消因子，這也是復(fù)雜多變量系統(tǒng)中經(jīng)常采用的手段[13-14]。對(duì)不同變量采用不同漸消因子的強(qiáng)跟蹤濾波器稱為多重漸消因子強(qiáng)跟蹤濾波器（Multiple Fading Strong Tracking Filter，MFSTF），MFSTF的公式與標(biāo)準(zhǔn)卡爾曼濾波器基本相同，只是在計(jì)算Pk/k-1時(shí)引入漸消因子矩陣，如式(8)所示：

Dk為漸消因子矩陣，是一個(gè)對(duì)角陣，對(duì)角元素是對(duì)不同狀態(tài)分量施加的漸消因子。多重漸消因子的求解方法可以參考文獻(xiàn)[14]，計(jì)算量并不大，而且當(dāng)漸消因子取1時(shí)退化為標(biāo)準(zhǔn)卡爾曼濾波。此處，為了保證P矩陣的對(duì)稱性，創(chuàng)新性的將式(8)改進(jìn)為式(9)的形式，仿真顯示這樣做可以提高對(duì)準(zhǔn)精度。

在 MFSTF中需要事先根據(jù)先驗(yàn)知識(shí)確定多重漸消因子的比例，考慮到大失準(zhǔn)角初始對(duì)準(zhǔn)中主要模型誤差是失準(zhǔn)角，因此確定漸消因子比例時(shí)突出對(duì)失準(zhǔn)角的跟蹤，選用

3 仿真驗(yàn)證

為了驗(yàn)證以上結(jié)論，進(jìn)行了一系列仿真驗(yàn)證。仿真參數(shù)為：陀螺常值零偏0.01 (°)/h，角度隨機(jī)游走系數(shù)0.001 (°)/√h，加表常值零偏100 μg，速度隨機(jī)游走系數(shù)10 μg/√Hz；載體真實(shí)姿態(tài)[0°, 0°, 10°]，真實(shí)位置[40°, 116°, 50m]，真實(shí)速度0 m/s；卡爾曼濾波器狀態(tài)初值X0=0，R=diag([0.01,0.01,0.01] m/s)2，Q根據(jù)陀螺角度隨機(jī)游走系數(shù)和加表速度隨機(jī)游走系數(shù)設(shè)置，P0= diag(k×[1°, 1°, 1°], [10, 10, 10] m/s, 0.5×[0.01, 0.01, 0.01] (°)/h, 0.1×[100, 100, 100] μg)2，其中k用于調(diào)節(jié)P0陣中?的大小。

首先，驗(yàn)證了P0對(duì)大失準(zhǔn)角初始對(duì)準(zhǔn)的影響。仿真中采用帶反饋校正的標(biāo)準(zhǔn)卡爾曼濾波器，初始姿態(tài)設(shè)為[1°, 1°, -170°]（對(duì)應(yīng)航向角誤差180°，是大失準(zhǔn)角中最惡劣的情況），調(diào)節(jié)P0中的k從10-2～106變化，每次仿真時(shí)長(zhǎng)3600 s，得到的航向角估計(jì)值如圖1所示，圖中紅色虛線是真實(shí)航向角，黑色圓圈是仿真結(jié)果，右下角小圖是局部放大圖。由圖 1可以看出 P0很小時(shí)得不到正確的航向估計(jì)，隨著P0的增大，逐漸可以得到正確的航向估計(jì)，說(shuō)明調(diào)節(jié)P0確實(shí)可以解決大失準(zhǔn)角情況下的初始對(duì)準(zhǔn)問(wèn)題；當(dāng) P0設(shè)置為 100°以上時(shí)大多數(shù)情況下都可以得到正確的航向角，但是在約 390°時(shí)出現(xiàn)了一個(gè)“突變”，這說(shuō)明并不是 P0越大，系統(tǒng)就一定能夠收斂到正確的航向角，單靠調(diào)節(jié)P0的這種方法確實(shí)可靠性較差。

圖1 P0對(duì)航向角估計(jì)的影響（KF）Fig.1 Effect of P0on the estimate of heading in KF

其次，驗(yàn)證了采用強(qiáng)跟蹤濾波器對(duì)系統(tǒng)可靠性的提高。采用與前次仿真相同的參數(shù)，只是濾波算法由KF變?yōu)镸FSTF，得到的航向角估計(jì)值如圖2所示。對(duì)比圖2和圖1可以看出：采用MFSTF后，P0選用范圍大大放寬了，選用任意值時(shí)都可以得到足夠精確的航向估計(jì)，經(jīng)過(guò)多次仿真也沒(méi)有發(fā)現(xiàn)“突變”現(xiàn)象，說(shuō)明采用強(qiáng)跟蹤跟濾波器后大大提高了系統(tǒng)的可靠性，完美解決了大失準(zhǔn)角初始對(duì)準(zhǔn)問(wèn)題。

圖2 P0對(duì)航向角估計(jì)的影響（MFSTF）Fig.2 Effect of P0on the estimate of heading in MFSTF

最后，綜合比較了小失準(zhǔn)角KF精對(duì)準(zhǔn)方法、本文提出的 MFSTF方法和基于非線性模型的 UKF方法。仿真數(shù)據(jù)同前，KF方法中沒(méi)有進(jìn)行粗對(duì)準(zhǔn)而直接將初始失準(zhǔn)角設(shè)置為小角度（初始姿態(tài)[0.1°, 0.1°, 5°]），MFSTF和UKF的初始姿態(tài)都選為[1°, 1°, -170°]。三種方法的仿真結(jié)果如圖3所示，圖中的兩幅小圖是局部放大圖。由圖3可以看出MFSTF的收斂速度要明顯快于 UKF，而且精度也比UKF高；MFSTF和KF幾乎都在300 s左右收斂，速度和精度不相上下。

為了進(jìn)一步比較三種算法的精度、速度和計(jì)算量，將一些有代表性的仿真結(jié)果列在表1中。從精度上看，零偏 0.01 (°)/h的陀螺對(duì)應(yīng)的航向誤差極限精度約為0.05°，比較300 s時(shí)的航向誤差可以看出MFSTF和KF的都可以達(dá)到這個(gè)精度，而UKF由于尚未收斂而精度較差；從速度上看，KF和MFSTF都在300 s左右收斂到極限精度10.05°附近，收斂速度相差無(wú)幾，而UKF在600 s時(shí)才收斂到10.5°附近，收斂速度遠(yuǎn)遠(yuǎn)慢于前兩者；從計(jì)算量上看，比較各種算法處理3600 s數(shù)據(jù)的耗時(shí)可以看出 KF最短，MFSTF約是KF的1.1倍，而UKF約是KF的3.8倍，說(shuō)明MFSTF的計(jì)算量與KF在同一水平，遠(yuǎn)小于UKF。通過(guò)以上比較可以看出，MFSTF可以很好地處理大失準(zhǔn)角初始對(duì)準(zhǔn)問(wèn)題，與UKF相比精度、速度和計(jì)算量都有明顯優(yōu)勢(shì)，與KF相比精度、速度和計(jì)算量均在同一水平，但省去了粗對(duì)準(zhǔn)步驟，因而總耗時(shí)更少。

表1 KF、MFSTF和UKF比較Tab.1 Comparison among KF, MFSTF and UKF

圖3 KF、MFSTF和UKF航向角估計(jì)結(jié)果對(duì)比Fig.3 Comparison on heading estimates among KF, MFSTF and UKF

4 結(jié) 論

本文提出一種適用于靜基座大失準(zhǔn)角初始對(duì)準(zhǔn)的新方法，無(wú)需進(jìn)行粗對(duì)準(zhǔn)，也無(wú)需采用非線性模型和非線性濾波，只須在傳統(tǒng)的線性模型基礎(chǔ)上選擇合適的P0陣，并采用反饋校正濾波結(jié)構(gòu)和強(qiáng)跟蹤濾波器即可。仿真結(jié)果表明這種方法能夠很好的解決大失準(zhǔn)角初始對(duì)準(zhǔn)問(wèn)題，魯棒性好，對(duì)準(zhǔn)精度、收斂速度和計(jì)算量都明顯優(yōu)于基于非線性模型的UKF方法，與只適用于小失準(zhǔn)角情況的標(biāo)準(zhǔn)卡爾曼濾波精對(duì)準(zhǔn)方法在同一水平，但因無(wú)需粗對(duì)準(zhǔn)而總耗時(shí)更短。這種新方法在大失準(zhǔn)角初始對(duì)準(zhǔn)領(lǐng)域非常實(shí)用。

（References）：

[1] Jiang Y F. Error analysis of analytic coarse alignment methods[J]. IEEE Transactions on Aerospace and Electronic Systems, 1998, 34(1): 334-337.

[2] 劉海鵬, 張科, 李恒年, 等. 捷聯(lián)慣導(dǎo)系統(tǒng)大失準(zhǔn)角下的初始對(duì)準(zhǔn)研究[J]. 航空計(jì)算技術(shù), 2012, 42(04): 39-42. Liu Hai-peng, Zhang Ke, Li Heng-nian, et al. Research on initial alignment of SINS with large misalignment angles[J]. Aeronautical Computing Technique, 2012, 42(4): 39-42.

[3] 龍瑞, 秦永元, 夏家和. CDKF在捷聯(lián)慣導(dǎo)系統(tǒng)大失準(zhǔn)角初始對(duì)準(zhǔn)的應(yīng)用[J]. 西北工業(yè)大學(xué)學(xué)報(bào), 2010, 28(03): 364-368. Long Rui, Qin Yong-yuan, Xia Jia-he. Applying CDKF (Central Difference Kalman Filter) to initial alignment of sins for large misalignment angles[J]. Journal of Northwestern Polytechnical University, 2010, 28(3): 364-368.

[4] Jamshaid A, Rasheeq M. Initial orientation of inertial navigation system realized through nonlinear modeling and filtering[J]. Measurement, 2011, 44(5): 793-801.

[5] Wu Xu, Sun Feng. Simulation study for FOG strapdown inertial navigation nonlinear alignment based on SVD-cubature Kalman filter[C]//Symposium on Photonics and Optoelectronics. Shanghai, 2012: 1-4.

[6] 嚴(yán)恭敏, 嚴(yán)衛(wèi)生, 徐德民. 基于歐拉平臺(tái)誤差角的SINS非線性誤差模型研究[J]. 西北工業(yè)大學(xué)學(xué)報(bào), 2009, 27(4): 511-516. Yan Gong-min, Yan Wei-sheng, Xu De-min. A SINS nonlinear error model reflecting better characteristics of SINS errors[J]. Journal of Northwestern Polytechnical University, 2009, 27(4): 511-516.

[7] 李漢舟, 潘泉, 鄧麟, 等. 極點(diǎn)配置對(duì)SINS羅經(jīng)對(duì)準(zhǔn)性能影響[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2014, 22(06): 711-718. Li Han-zhou, Pan Quan, Deng Lin, et al. Pole assignment’s influence on alignment performance of SINS gyrocompass[J]. Journal of Chinese Inertial Technology, 2014, 22(6): 711-718.

[8] Hu Jie, Cheng Xiang-hong. A new in-motion initial alignment for land-vehicle SINS/OD integrated system[C] //2014 Position, Location and Navigation Symposium. Monterey, CA, 2014: 407-412.

[9] 孫鈺琛, 段鳳陽(yáng), 陳鵬, 等. 基于可觀測(cè)度分析的捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)方法[J]. 壓電與聲光, 2014, 36(03): 380-383. Sun Yu-chen, Duan Feng-yang, Chen Peng, et al. Initial alignment method for SINS based on the analysis of the degree of observability[J]. Piezoelectrics & Acoustooptics, 2014, 36(03): 380-383.

[10] Wang L, Wu L, Guan Y, et al. Online sensor fault detection based on an improved strong tracking filter[J]. Sensors, 2015, 15(2): 4578-4591.

[11] Wang D, Zhou D H, Jin Y H, et al. Adaptive generic model control for a class of nonlinear time-varying processes with input time delay[J]. Journal of Process Control, 2004, 14(5): 517-531.

[12] Xia Qi-jun, Rao Ming, Ying Yi-qun, et al. Adaptive fading Kalman filter with an application[J]. Automatica, 1994, 30(8): 1333-1338.

[13] Gao Wei-xi, Miao Ling-juan, Ni Mao-lin. Multiple fading factors Kalman filter for SINS static alignment application[J]. Chinese Journal of Aeronautics, 2011, 24(4): 476-483.

[14] 趙小明, 趙帥, 郭永剛, 等. 基于強(qiáng)跟蹤濾波的車載行進(jìn)間對(duì)準(zhǔn)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2015, 23(2): 141-144. Zhao Xiao-ming, Zhao Shuai, Guo Yong-gang, et al. In-motion alignment based on strong tracking filter[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 141-144.

Strong tracking filter based SINS initial alignment for large misalignment angles

PENG Zhuo1,2, ZHANG Rong1, GUO Mei-feng1, LIU Gang1, LUO Shou-hong1
(1. Department of Precision Instrument, Tsinghua University, Beijing 100084, China; 2. Astronaut Center of China, Beijing 100094, China)

Initial alignment is important for strapdown inertial navigation system. Coarse alignment is usually implemented before fine alignment to make the misalignment angles sufficiently small to meet the linear error model. Without coarse alignment, the misalignment angles will be large enough to need non-linear model and non-linear filter, which are complicated and inaccurate. For this reason, a new method is presented which don’t need coarse alignment or non-linear model. This is achieved by choosing appropriate initial error covariance matrix and using feedback compensation structure and strong tracking filter. Simulation shows the new method is effective and robust for large misalignment angles. The heading error can ultimately converge to 0.05° after 300 s under arbitrary initial attitude, while the traditional fine alignment has almost the same accuracy and speed but needs additional time for coarse alignment, and the UKF only converges to 0.5° after 600 s which is much slower.

SINS; initial alignment; large misalignment angles; strong tracking filter

U666.1

：A

2015-05-07；

：2015-08-28

總裝備部預(yù)研項(xiàng)目（20114113019）

彭卓（1986—），男，博士研究生，從事高精度慣性導(dǎo)航系統(tǒng)研究。E-mail：z-peng12@mails.tsinghua.edu.cn

聯(lián) 系 人：張嶸（1969—），男，研究員，博士生導(dǎo)師。E-mail：rongzh@mail.tsinghua.edu.cn

1005-6734(2015)05-0565-05

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