傳遞對準(zhǔn)數(shù)字仿真系統(tǒng)Kalman濾波池設(shè)計

解春明，趙剡，王紀(jì)南，3

(1.北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院，北京100191;2.桂林空軍學(xué)院，廣西 桂林541003;3.中國空導(dǎo)彈研究院，河南洛陽471009)

傳遞對準(zhǔn)是用于解決捷聯(lián)慣導(dǎo)系統(tǒng)空中動基座條件下初始對準(zhǔn)問題的一項關(guān)鍵技術(shù).特別是由于戰(zhàn)術(shù)武器在現(xiàn)代局部戰(zhàn)爭中的作用日益突出，對捷聯(lián)慣導(dǎo)系統(tǒng)對準(zhǔn)的快速性和準(zhǔn)確性提出了更高的要求［1］.

國內(nèi)外對此問題的大量研究集中在對準(zhǔn)匹配方案、濾波器的設(shè)計上.傳遞對準(zhǔn)可采用的對準(zhǔn)匹配方案很多，其中典型方案包括速度匹配、姿態(tài)匹配、速度+姿態(tài)匹配、位置匹配、速度+位置匹配、角速率匹配、速度+角速率、比力匹配等等，而且即使是同一種匹配方案，其狀態(tài)方程和量測方程也不盡相同［2-12］.另外，對準(zhǔn)所采用濾波器的配置參數(shù)及算法也很多［13-15］.因此，對準(zhǔn)匹配及濾波的研究內(nèi)容十分繁雜.

現(xiàn)有研究往往僅對特定的傳遞對準(zhǔn)匹配方案、狀態(tài)方程、量測方程、濾波參數(shù)以及濾波算法考慮，進(jìn)而編制對應(yīng)的程序.值得注意的是，當(dāng)匹配方案、狀態(tài)方程、量測方程、濾波參數(shù)以及濾波算法的任何部分發(fā)生變化時，都需首先做代碼級修改，包括增刪變量、修改賦值、模型修改、算法修改、輸入輸出修改等，然后再進(jìn)行調(diào)試、發(fā)行，既浪費(fèi)了開發(fā)時間，又不利于系統(tǒng)集成.

本文旨在為傳遞對準(zhǔn)數(shù)字仿真系統(tǒng)設(shè)計通用的濾波機(jī)制，使仿真系統(tǒng)能夠靈活方便地進(jìn)行任意匹配方案、狀態(tài)方程、量測方程、濾波參數(shù)、濾波算法的研究.由于傳遞對準(zhǔn)通常采用Kalman濾波，因此本文設(shè)計的通用濾波機(jī)制稱為Kalman濾波池.Kalman濾波池基于VC++6.0開發(fā)環(huán)境，結(jié)合數(shù)據(jù)庫技術(shù)、動態(tài)鏈接庫技術(shù)實(shí)現(xiàn).用戶可以擺脫代碼編寫的束縛，直接通過濾波池操作面板，進(jìn)行傳遞對準(zhǔn)匹配方案、濾波參數(shù)、矩陣等信息的填寫、保存，使得現(xiàn)有文獻(xiàn)提出的某種對準(zhǔn)匹配模型壓縮為濾波池的一條記錄.濾波池根據(jù)用戶需求提取記錄信息，啟動濾波過程，解析數(shù)據(jù)，調(diào)用導(dǎo)航、濾波算法，輸出濾波結(jié)果.

1 傳遞對準(zhǔn)Kalman濾波池設(shè)計

傳遞對準(zhǔn)線性離散方程表示為［16］

式中:Xk∈Rn為系統(tǒng)狀態(tài)向量，Φk，k-1∈Rn×n為狀態(tài)轉(zhuǎn)移矩陣，Zk∈Rm為系統(tǒng)量測向量，Γk，k-1∈Rn×r為噪聲驅(qū)動陣，wk∈Rr為系統(tǒng)噪聲向量，Hk∈Rm×n為量測矩陣，vk∈Rm為量測噪聲向量.

系統(tǒng)噪聲和量測噪聲符合:

式中:Qk∈Rr×r為系統(tǒng)噪聲陣，Rk∈Rm×m為觀測噪聲陣，δkj為 Kronecker-δ函數(shù).

傳遞對準(zhǔn)采用標(biāo)準(zhǔn)卡爾曼濾波方程［16］:

1.1 濾波池符號集構(gòu)建

依據(jù)各種慣導(dǎo)誤差方程，傳感器誤差方程、機(jī)翼模型、量測匹配方案，考慮空中、海上或是陸基對準(zhǔn)情況，式(1)可以寫成許多具體形式，即 Xk、Zk、wk、vk、Φk，k-1、Γk-1、Hk對應(yīng)許多具體形式.

以符號來表示以上矢量和矩陣，則它們所有具體形式的并集為一個固定的符號集，而每種對準(zhǔn)匹配方案即為從此符號集抽取的一個特定子集.根據(jù)符號類型不同，分成如下幾類:

1)濾波狀態(tài)符號說明.

包括:x、y、z向速度差，x、y、z向失準(zhǔn)角，x、y、z向量測失準(zhǔn)角，經(jīng)度差、緯度差、高度差，x、y、z向角速率差，x、y、z向比力差，陀螺 向常值漂移，陀螺x、y、z向一階馬氏漂移，陀螺 x、y、z向刻度系數(shù)差，加計 向常值偏置，加計x、y、z向一階馬氏偏置，加計x、y、z向刻度系數(shù)差，x、y、z向機(jī)翼變形角，向機(jī)翼變形角速度.

2)濾波噪聲符號說明.

包括:陀螺 x、y、z向常值漂移噪聲，陀螺 x、y、z向一階馬氏漂移噪聲，陀螺x、y、z向刻度系數(shù)噪聲，加計 x、y、z向常值偏置噪聲，加計 x、y、z向一階馬氏偏置噪聲，加計x、y、z向刻度系數(shù)噪聲，向機(jī)翼變形角噪聲x、y、z向機(jī)翼變形角速度噪聲.

3)濾波觀測符號說明.

包括:x、y、z向速度，x、y、z向位置、經(jīng)度、緯度、高度，俯仰角、橫滾角、航向角，x、y、z向角速率，x、y、z向比力.

4)濾波矩陣元素符號說明.

矩陣 Φk，k-1、Γk-1、Hk由許多元素組成，其中每個元素為常數(shù)或多個符號運(yùn)算的表達(dá)式.構(gòu)成矩陣元素的符號分為如下幾類:

①基本數(shù)學(xué)符號

包括:數(shù)字0～9、小數(shù)點(diǎn)、負(fù)號、四則運(yùn)算符、冪運(yùn)算符、左右括號.

②常用函數(shù)符號

③常量符號

橢球度e、基準(zhǔn)重力加速度g0、圓周率PI、地球自轉(zhuǎn)角速率、克拉索夫斯基規(guī)定地球橢球長半軸長Re.

④常用變量符號

變量符號包括:經(jīng)度、緯度、高度，x、y、z向速度，俯仰、橫滾、航向角，游移角，重力加速度，地球子午、卯酉圈主半徑，火控 x、y、z向位置、速度，x、y、z向桿臂長度，x、y、z向機(jī)翼結(jié)構(gòu)撓曲變形系數(shù)，導(dǎo)航系、載體系下x、y、z向角速率，導(dǎo)航系、載體系下x、y、z向比力，坐標(biāo)變換矩陣，傳感器相關(guān)時間等.

以上符號的名稱、含義和初始值由數(shù)據(jù)庫管理，可根據(jù)需要進(jìn)行增刪.

1.2 濾波池輸入面板設(shè)計

具備了式(1)的各矢量及矩陣，再給出濾波式(2)的初始條件 X0、P0、Q0、R0，就可以啟動實(shí)施對準(zhǔn)濾波.因此，這里設(shè)計了濾波池輸入面板，用于Xk、Zk、wk、vk、Φk，k-1、Γk-1、Hk、X0、P0、Q0、R0的輸入、修改、保存、導(dǎo)入等操作.面板如圖1所示.

圖1 濾波池輸入面板Fig.1 Input panel of filtering pool

首先，輸入系統(tǒng)狀態(tài)維數(shù)、噪聲維數(shù)、觀測狀態(tài)維數(shù);然后根據(jù)矢量維數(shù)來給出 Xk、Zk、wk、vk、X0、P0、Q0、R0逐項添加其參數(shù)，包括初始值、方差等，如圖2所示.

圖2 濾波初始參數(shù)配置面板Fig.2 Initial filter parameter configuration panel

接著，依次利用矩陣輸入面板給出 Φk，k-1、Γk-1、Hk的各元素.為了方便用戶使用，設(shè)計了鼠標(biāo)滑到某個符號上時給出提示的功能，如圖1中下部分所示.

最后，將濾波匹配模型保存入數(shù)據(jù)庫，例如，可命名“九維狀態(tài)九維噪聲速度+姿態(tài)對準(zhǔn)匹配模型”濾波匹配模型庫，當(dāng)名稱與數(shù)據(jù)庫某一名稱相同時，可選擇是否覆蓋之.另外，可用面板導(dǎo)入某一模型進(jìn)行修改.

1.3 濾波池模型庫設(shè)計

濾波池模型庫用于保存2.2面板輸入的濾波匹配模型，采用SQL Server2000數(shù)據(jù)庫設(shè)計，模型庫構(gòu)成如表1所示.

表1 濾波池模型庫Table 1 Model database of filtering pool

1.4 濾波池算法庫設(shè)計

濾波池采用的濾波基本算法如式(2)，但在實(shí)際中式(2)的前提很難滿足，有時需要增大系統(tǒng)噪聲來近似處理.進(jìn)一步的研究是設(shè)計更多的自適應(yīng)濾波算法來擴(kuò)展系統(tǒng)濾波功能.

常用的算法有Sage-Husa自適應(yīng)濾波及其簡化算法［14］，在式(2)中加入:

式中:

還可采用強(qiáng)跟蹤自適應(yīng)濾波算法［15］，即對狀態(tài)方差陣Pk進(jìn)行處理:

其中，sk+1=diag(s1(k+1)，s2(k+1)，…，sn(k+1))為狀態(tài)方差預(yù)測陣的對角權(quán)重陣，由如下公式計算:

其中:Tr()為矩陣跡運(yùn)算符，0≤ρ≤1為調(diào)節(jié)因子.

實(shí)際中，還有許多新近提出的自適應(yīng)濾波算法.這里，采用動態(tài)鏈接庫進(jìn)行算法的編寫，使得算法與濾波過程獨(dú)立，且便于擴(kuò)充.另外，濾波仿真前，需要模擬導(dǎo)航解算，其中也包含許多算法，也采用動態(tài)鏈接庫設(shè)計.

1)算法動態(tài)鏈接庫

算法動態(tài)鏈接庫由頭文件、實(shí)現(xiàn)文件和定義文件組成.

①頭文件

用于給出導(dǎo)出函數(shù)命名，形式如下:

③定義文件

用于給出函數(shù)的導(dǎo)出列表，形式如下:

“LIBRARY(Navigation、Filter)

EXPORTS

(導(dǎo)航、濾波)函數(shù)1

(導(dǎo)航、濾波)函數(shù)2

……………………….

(導(dǎo)航、濾波)函數(shù) n”.

2)算法數(shù)據(jù)庫

算法數(shù)據(jù)庫用于保存算法對應(yīng)的函數(shù)信息，構(gòu)成如表2所示.

表2 算法數(shù)據(jù)庫Table 2 Algorithm database

1.5 濾波池功能類設(shè)計

1)文件讀取類CGetFileData.

能夠從數(shù)據(jù)文件中讀出指定行、指定列數(shù)據(jù)，用于濾波計算和量測匹配等.

主要包括:函數(shù)StrToArray，用于將文件中一行數(shù)據(jù)按分割符拆分并存入字符串?dāng)?shù)組中;函數(shù)GetL-ineCount用于獲取文件按總行數(shù);函數(shù)GetColCount用于獲取文件總列數(shù);函數(shù)IsDataFile用于判斷文件是否為數(shù)據(jù)文件;函數(shù)GetFile用于從指定文件中提取一定行列數(shù)據(jù)到二維字符串?dāng)?shù)組中.

2)矩陣元素提取類CGetStrData能夠按照矩陣維數(shù)設(shè)定值將匹配模型中的矩陣字段按分割符拆分成單個矩陣元素.

主要包括:函數(shù)StrToArray，用于將矩陣字段值分成多行，將每行數(shù)據(jù)按分割符拆分并存入字符串?dāng)?shù)組中;函數(shù)GetStrData，利用字符串?dāng)?shù)組為對應(yīng)矩陣元素賦表達(dá)式類型值.

3)表達(dá)式計算類CExpCal根據(jù)當(dāng)前濾波時刻符號集中各符號數(shù)值對矩陣元素表達(dá)式進(jìn)行計算.

主要包括:符號值提取函數(shù)GetSignValue，用于根據(jù)濾波迭代和導(dǎo)航解算的更新結(jié)果提取符號集分量值;函數(shù)CalFun，用于計算符號函數(shù)值;函數(shù)GetUnit，用于將運(yùn)算單元保存到雙向鏈表;函數(shù)Get-Node，用于提取表達(dá)式樹狀結(jié)點(diǎn);函數(shù)GetNodeValue，用于計算節(jié)點(diǎn)值;函數(shù)AnalyzeExp，用于分析并表達(dá)式生成鏈表;函數(shù)CalExp，用于計算表達(dá)式值.1.6 濾波池執(zhí)行流程設(shè)計

設(shè)計如圖3所示的導(dǎo)航算法、對準(zhǔn)匹配模型、濾波算法設(shè)置面板，從數(shù)據(jù)庫中提取對應(yīng)的函數(shù)和模型數(shù)據(jù).

圖3 濾波執(zhí)行配置Fig.3 Executing configurtion of filtering

點(diǎn)擊“確定”，進(jìn)入如圖4所示的濾波池執(zhí)行流程.1)濾波仿真參數(shù)提取

濾波仿真參數(shù)包含子慣導(dǎo)開機(jī)時間、對準(zhǔn)開始時間、主、子陀螺和加計的采樣頻率、主、子慣導(dǎo)的姿態(tài)、速度、位置解算頻率、對準(zhǔn)濾波頻率.

2)慣導(dǎo)模擬導(dǎo)航

利用類CGetFileData提取主、子慣導(dǎo)導(dǎo)航參數(shù)，利用導(dǎo)航算法庫首先對主慣導(dǎo)進(jìn)行導(dǎo)航解算，然后在主慣導(dǎo)開機(jī)時刻，利用主慣導(dǎo)數(shù)據(jù)對其進(jìn)行粗對準(zhǔn)，進(jìn)行子慣導(dǎo)模擬導(dǎo)航.

圖4 濾波執(zhí)行流程Fig.4 Executing procedure of filtering

導(dǎo)航解算動態(tài)鏈接庫的使用步驟為:

①按算法庫的函數(shù)參數(shù)定義導(dǎo)航算法函數(shù)

typedef void(_stdcall*NavFun)(參數(shù)列表).

②顯示導(dǎo)入動態(tài)鏈接庫

HMODULE hModule=::LoadLibrary(＂Navigation.dll＂).

③根據(jù)定義文件的函數(shù)索引提取導(dǎo)航函數(shù)

NavFun Nav=(NavFun)GetProcAddress(hModule，函數(shù)索引).

3)濾波器配置.

從模型庫中提取濾波狀態(tài)、噪聲、觀測量的符號、初始值、方差值、矩陣信息，利用類CGetStrData為各矩陣元素分配表達(dá)式.

4)匹配濾波.

在對準(zhǔn)時間內(nèi)，進(jìn)行主子慣導(dǎo)匹配濾波.

①更新符號集數(shù)值，

②利用類CExpCal計算矩陣元素表達(dá)式數(shù)值，

③考慮主子慣導(dǎo)周期、延遲進(jìn)行量測匹配，

④系統(tǒng)離散化，

⑤按照導(dǎo)航函數(shù)導(dǎo)入方法從濾波算法庫導(dǎo)入濾波函數(shù)，執(zhí)行當(dāng)前濾波運(yùn)算，

⑥輸出當(dāng)前濾波結(jié)果，重復(fù)步驟①～⑥.

2 Kalman濾波池性能測試

為了驗(yàn)證Kalman濾波池的工作性能，進(jìn)行了6類30種對準(zhǔn)匹配方案的濾波仿真試驗(yàn)，測試平臺如表3.

表3 測試平臺配置Table 3 Testing environment configuration

其中，6類對準(zhǔn)匹配方案包括［2-12］:速度匹配、速度+姿態(tài)匹配、速度+位置匹配、比力匹配、角速率匹配、速度+角速率匹配.每類匹配方案包括幾種具體的系統(tǒng)狀態(tài)維數(shù)、噪聲維數(shù)、濾波初始設(shè)置不同的對準(zhǔn)匹配方案，例如速度匹配方案測試了:六維狀態(tài)三維噪聲小、中、大狀態(tài)方差速度匹配方案、十二維狀態(tài)三維噪聲小、中、大狀態(tài)方差速度匹配方案、六維狀態(tài)六維噪聲小、中、大狀態(tài)方差速度匹配方案9種具體方案.實(shí)際中，狀態(tài)初始值、系統(tǒng)噪聲方差、量測噪聲方差也可根據(jù)用戶需要設(shè)置.仿真試驗(yàn)的條件為:初始經(jīng)度 116°，緯度 40°，高度 3 000 m，初始軌跡速度500 m/s，初始姿態(tài)角為0;假設(shè)主慣導(dǎo)無誤差;子慣導(dǎo)陀螺誤差參數(shù)εbsb量級為0.1°/h，εbsw量級為0.01°/h;加速度計誤差參數(shù)?bsb量級為10-3g，?bsw量級為10-4g;τg=τa=7 200 s;安裝誤差角 μbf各向均取為 1°;桿臂矢量為 LA=(5 m，-0.5 m，-1 m).

機(jī)翼撓曲變形采用2階馬爾科夫［7］過程模擬，三向結(jié)構(gòu)撓曲變形系數(shù)為 βx=4，βy=5，β=6，方差為 E［ηηT］=0.1δkj;機(jī)翼顫振變形幅度為 Ay=Az=0.001 m，顫振頻率為 fy=fz=40 Hz.

仿真時間20 s，濾波周期0.5 s，主子慣導(dǎo)姿態(tài)、速度、位置解算周期0.01 s.

通過記錄測試過程及結(jié)果信息，得具體參數(shù)如表4.

表4 傳遞對準(zhǔn)Kalman濾波池性能測試結(jié)果Table 4 Performance testing result of Kalman filtering pool

測試結(jié)果表明:傳遞對準(zhǔn)Kalman濾波池運(yùn)行穩(wěn)定、占用系統(tǒng)資源較小、存儲容量小、修改方便，且具有較高的計算精度和運(yùn)行速度.

3 結(jié)束語

本文設(shè)計了一種應(yīng)用于傳遞對準(zhǔn)數(shù)字仿真系統(tǒng)的通用Kalman濾波池，進(jìn)行匹配方案、濾波算法研究.濾波池采用VC++6.0平臺和SQL Server 2000數(shù)據(jù)庫系統(tǒng)開發(fā)，通過符號集、操作面板、C++功能類、模型算法數(shù)據(jù)庫、算法動態(tài)鏈接庫的設(shè)計，使得用戶能夠靈活方便地進(jìn)行任意匹配方案、狀態(tài)方程、量測方程、濾波參數(shù)、濾波算法的研究.仿真測試驗(yàn)證了濾波池穩(wěn)定性、靈活性，表明其占用系統(tǒng)資源少、存儲容量小，具有較高的計算精度和運(yùn)行速度，有效提高了傳遞對準(zhǔn)匹配方案及濾波算法的研究效率，為機(jī)載導(dǎo)彈捷聯(lián)慣導(dǎo)系統(tǒng)設(shè)計和傳遞對準(zhǔn)的有效實(shí)施提供理論和技術(shù)參考.另外，此濾波池可以很容易地修改應(yīng)用于其他濾波領(lǐng)域.

［1］王司，鄧正隆.慣導(dǎo)系統(tǒng)動基座傳遞對準(zhǔn)技術(shù)綜述［J］.中國慣性技術(shù)學(xué)報，2003，11(2):61-67.

WANG Si，DENG Zhenglong.Technique review of transfer alignmentfor inertial navigation systems on moving base［J］.Journal of Chinese Inertial Technology，2003，11(2):61-67.

［2］KAIN J E，CLOUTIER J R.Rapid transfer alignment for tactical weapon applications［C］//Proceedings of the AIAA Guidance，Navigation and Control Conference. Boston:MA，1989:1290-1300.

［3］ROGERS R M.Velocity-plus-rate matching for improved tactical weapons rapid transfer alignment［C］//AIAA Guidance，Navigation and Control Conference.UNJ SA，1991:1580-1588.

［4］SHORTELLE K J，GRAHAM W R.F-16 flight test of a rapid transfer alignment procedure［C］//IEEE PLANS.Ft.Lauderdale:FL，1998:379-386.

［5］LYOU J，LIM Y C.Transfer alignment error compensator design based on robust state estimation［J］.Transactions of the Japan Society for Aeronautical and Space Sciences，2005，48(61):143-151.

［6］LYOU J，LIM Y C.Transfer alignment considering measurement time delay and ship body flexure［J］.Journal of Mechanical Science and Technology，2009，23(1):195-203.

［7］俞濟(jì)祥.慣性導(dǎo)航系統(tǒng)各種傳遞對準(zhǔn)方法討論［J］.航空學(xué)報，1988，9(5):211-217.

YU Jixiang.Discussion on several transfer alignment methods of inertial navigation system［J］.Acta Aeronautica et Astronautica Sinica，1988，9(5):211-217.

［8］黃昆，單福林，楊功流，等.艦載角速度匹配傳遞對準(zhǔn)方法研究［J］.中國慣性技術(shù)學(xué)報，2005，13(4):1-5.HUANG Kun，SHAN Fulin，YANG Gongliu，et al.Angular rate matching method for shipboard transfer alignment［J］.Journal of Chinese Inertial Technology，2005，13(4):1-5.

［9］盧松華，羅建軍，岳曉奎.速度+方位角匹配傳遞對準(zhǔn)算法的簡化與仿真［J］.彈箭與制導(dǎo)學(xué)報，2005，25(3):292-295.

LU Songhua，LUO Jianjun，YUE Xiaokui.Simplification and simulation of velocity-plus-azimuth matching T/A［J］.Journal of Projectiles，Rockets，Missiles and Guidance，2005，25(3):292-295.

［10］陳凱，魯浩，閆杰.傳遞對準(zhǔn)中一種新的姿態(tài)匹配算法［J］.西北工業(yè)大學(xué)學(xué)報，2007，25(5):691-694.

CHEN Kai，LU Hao，YAN Jie.A different and better algorithm for attitude match of transfer alignment［J］.Journal of Northwestern Polytechnical University，2007，25(5):691-694.

［11］于飛，翟國富，李倩，等.速度加角速度匹配傳遞對準(zhǔn)方法研究［J］.傳感器與微系統(tǒng)，2009，28(6):69-72.

YU Fei，ZHAI Guofu，LI Qian，et al.Research on velocity plus angular rate match transfer alignment［J］.Transducer and Microsystem Technologies，2009，28(6):69-72.

［12］李群生，董景新，魯浩.角速度匹配在機(jī)載武器傳遞對準(zhǔn)中的應(yīng)用［J］.彈箭與制導(dǎo)學(xué)報，2009，29(3):62-64.

LI Qunsheng，DONG Jingxin，LU Hao.The application of angular rate matching to transfer alignment for airborne weapons［J］.Journal of Projectiles，Rockets，Missiles and Guidance，2009，29(3):62-64.

［13］SPALDING K.An efficient rapid transfer alignment filter［C］//Proc of the AIAA GN＆C Conference.Montreal，Canada，1992:1276-1286.

［14］魯平，趙龍，陳哲.改進(jìn)的Sage-Husa自適應(yīng)濾波及其應(yīng)用［J］.系統(tǒng)仿真學(xué)報，2007，19(15):3503-3505.

LU Ping，ZHAO Long，CHEN Zhe.Improved Sage-Husa adaptive filtering and its application［J］.Journal of System Simulation，2007，19(15):3503-3505.

［15］張詩桂，朱立新，趙義正.粒子濾波算法研究進(jìn)展與展望［J］.自動化技術(shù)與應(yīng)用，2010，29(6):1-16.

ZHANG Shigui，ZHU Lixin，ZHAO Yizheng.The development and prospect of particle filtering algorithm［J］.Automation Technology and Application，2010，29(6):1-16.

［15］付夢印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用［M］.北京:科學(xué)出版社，2003:25-28.

FU Mengyin，DENG Zhihong，ZHANG Jiwei.Kalman filtering theory and its application in navigation system［M］.Beijing:Science Press，2003:25-28.