卡爾曼濾波算法

　　本文采用卡爾曼濾波算法對GPS，IMU數(shù)據(jù)進(jìn)行融合。將式(1)(8)離散化后方程為：

???????

　　試驗結(jié)果

　　為了驗證本文提出的雙天線GPS/SINS組合導(dǎo)航系統(tǒng)的可行性，在南京某地進(jìn)行了跑車實驗。其中姿態(tài)GPS模塊定位精度小于5m，速度精度0.1m/s，航向精度0.1°。IMU常值漂移小于0.1°/h。跑車過程中以高精度GPS作為真實值，將組合導(dǎo)航系統(tǒng)輸出值與高精度GPS數(shù)據(jù)進(jìn)行比較。整個系統(tǒng)中每5ms進(jìn)行一次慣導(dǎo)數(shù)據(jù)解算，通過卡爾曼濾波算法進(jìn)行數(shù)據(jù)修正的頻率為1次/s。將跑車過程中導(dǎo)航計算機(jī)輸出數(shù)據(jù)存入文件，通過MATLAB進(jìn)行數(shù)據(jù)處理。圖3中給出一次跑車中組合導(dǎo)航系統(tǒng)與高精度GPS同一時刻輸出數(shù)據(jù)的差值，結(jié)果如圖3所示。

　　從圖3可以看出，在動態(tài)情況下本文提出的組合導(dǎo)航系統(tǒng)具有較高的定位精度，從與高精度GPS數(shù)據(jù)對比中可以看，此系統(tǒng)定位誤差在4m以內(nèi)，而速度誤差在0.1m/s左右，達(dá)到了系統(tǒng)的要求，充分滿足了實際應(yīng)用需求。

　　參考文獻(xiàn)：

　　[1] Li Y, Zhang K, Roberts C , Murata M. On-the-fly GPS-based Attitude Determination Using Single-and Double-differenced Carrier Phase Measurements[J]. Springer_GPS Solutions,,2004,Volume 8, Number 2,(10):93-102

　　[2] 付夢印.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京:科學(xué)出版社,2003:104-115,132-135

　　[3] 何曉峰,胡小平.基于SINS/GPS組合的低成本船用姿態(tài)確定系統(tǒng)[J].中國慣性技術(shù)學(xué)報.2004,12,(05):21-25

　　[4] 王惠南.GPS導(dǎo)航原理與應(yīng)用[M].北京:科學(xué)出版社,2003:33-41

　　[5] Yanjuan Yang, Weifeng Tian, Zhihua Jin. Study on a Novel Marine INS/GPS Integrated Navigation Technology. IEEE Seventh International Conference on Control, Automation , Robotics and Vision 2002, Volume: 3 [C]:1563-1567