姿態誤差方程為： 
　　速度誤差方程為： 
　　位置誤差方程為：  

　　其中，為姿態誤差， 為速度誤差， 為位置誤差。為輸出角速度誤差矢量， 為所測得的比力誤差矢量。 

　　卡爾曼濾波器系統狀態向量由15項構成，其中， 為姿態誤差角;分別為載體的東向、北向和天向速度誤差;分別為緯度誤差、經度誤差和高度誤差;分別為陀螺隨機常值漂移和加速度計隨機常值零偏。具體卡爾曼濾波算法功能通過軟件代碼實現。

　　通過軟件實現所設計的捷聯算法與組合濾波算法。重點研究了采用EKF組合算法之后的系統誤差狀態向量的估計情況。并通過matlab軟件對衛星/MEMS組合中的各種誤差向量估計做仿真觀測。并在實際路測環境下，獲得導航全參數。如圖6所示為路測試驗專用車。

　　如圖7所示，紅色為衛星導航，藍色為組合導航，當試驗車路過城市峽谷時，由于遮擋和多路徑效應等，衛星導航出現較大偏差，而組合導航表現良好。

　　如圖8所示，當路測試驗車進入地下車庫時，衛星導航立刻中斷，而組合導航依然能夠維持導航能力。　　

 

　　當衛星導航信號中斷，組合導航模塊的定位、定速性能將逐漸惡化，而姿態角受影響較小。表1給出了組合濾波算法在不同的衛星導航中斷間隔內的實測結果。在靜態情況下，姿態角誤差小于0.2°，航向角誤差小于0.5°。在動態情況下，姿態角與航向角誤差小于1°，如果衛星導航信號突然中斷，將能持續30秒，定位精度維持在20米內。　　

 

　　硬件平臺

　　本文以GPS/BD多模導航接收機、MEMS-IMU、磁強計構成的超低成本微小型組合導航模塊為研究對象，結合實際需求，設計組合導航架構;通過比較傳統SINS與現代MEMS-SINS，提出實用的MEMS-SINS的初始化方案和捷聯解算算法;基于MEMS-IMU，給出衛星/MEMS組合導航模塊工程化的濾波算法，實現非線性連續系統的線性離散化。仿真與實測試驗結果表明：在MEMS-IMU精度較低的情況下，通過相關算法的合理設計，仍可以實現較高精度導航。圖9展示了自主設計的衛星/MEMS組合導航模塊和集成開發軟件平臺。

　　參考文獻

　　[1]G H Elkaim. Comparison of Low-Cost GPS/INS Sensors for Autonomous Vehicle Applications [J]. 2008, 1133: 1144.
　　[2]Michael J. Caruso Applications of Magnetic Sensors for Low Cost Compass Systems [R]. Honeywell Inc, 2000.
　　[3]David H Titterton, John L Weston. Strapdown Inertial Navigation Technology 2nd Edition [M]. The IEE, 2004.
　　[4]秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導航原理[M]. 西安: 西北工業大學出版社, 1998.
　　[5]Di Li. Based Low-cost Inertial/GPS Integrated Navigation Platform: Design and Experiments [J]. Journal of GPS, 2007
　　[6]杜春雷. ARM體系結構與編程[M]. 清華大學出版社，2003.