卡爾曼濾波算法

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

???????

　　試驗結果

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

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

　　參考文獻：

　　[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濾波理論及其在導航系統(tǒng)中的應用[M].北京:科學出版社,2003:104-115,132-135

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

　　[4] 王惠南.GPS導航原理與應用[M].北京:科學出版社,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