姿態(tài)誤差方程為： 
　　速度誤差方程為： 
　　位置誤差方程為：  

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

　　卡爾曼濾波器系統(tǒng)狀態(tài)向量由15項(xiàng)構(gòu)成，其中， 為姿態(tài)誤差角;分別為載體的東向、北向和天向速度誤差;分別為緯度誤差、經(jīng)度誤差和高度誤差;分別為陀螺隨機(jī)常值漂移和加速度計(jì)隨機(jī)常值零偏。具體卡爾曼濾波算法功能通過軟件代碼實(shí)現(xiàn)。

　　通過軟件實(shí)現(xiàn)所設(shè)計(jì)的捷聯(lián)算法與組合濾波算法。重點(diǎn)研究了采用EKF組合算法之后的系統(tǒng)誤差狀態(tài)向量的估計(jì)情況。并通過matlab軟件對衛(wèi)星/MEMS組合中的各種誤差向量估計(jì)做仿真觀測。并在實(shí)際路測環(huán)境下，獲得導(dǎo)航全參數(shù)。如圖6所示為路測試驗(yàn)專用車。

　　如圖7所示，紅色為衛(wèi)星導(dǎo)航，藍(lán)色為組合導(dǎo)航，當(dāng)試驗(yàn)車路過城市峽谷時，由于遮擋和多路徑效應(yīng)等，衛(wèi)星導(dǎo)航出現(xiàn)較大偏差，而組合導(dǎo)航表現(xiàn)良好。

　　如圖8所示，當(dāng)路測試驗(yàn)車進(jìn)入地下車庫時，衛(wèi)星導(dǎo)航立刻中斷，而組合導(dǎo)航依然能夠維持導(dǎo)航能力。　　

 

　　當(dāng)衛(wèi)星導(dǎo)航信號中斷，組合導(dǎo)航模塊的定位、定速性能將逐漸惡化，而姿態(tài)角受影響較小。表1給出了組合濾波算法在不同的衛(wèi)星導(dǎo)航中斷間隔內(nèi)的實(shí)測結(jié)果。在靜態(tài)情況下，姿態(tài)角誤差小于0.2°，航向角誤差小于0.5°。在動態(tài)情況下，姿態(tài)角與航向角誤差小于1°，如果衛(wèi)星導(dǎo)航信號突然中斷，將能持續(xù)30秒，定位精度維持在20米內(nèi)?！　?/p>

 

　　硬件平臺

　　本文以GPS/BD多模導(dǎo)航接收機(jī)、MEMS-IMU、磁強(qiáng)計(jì)構(gòu)成的超低成本微小型組合導(dǎo)航模塊為研究對象，結(jié)合實(shí)際需求，設(shè)計(jì)組合導(dǎo)航架構(gòu);通過比較傳統(tǒng)SINS與現(xiàn)代MEMS-SINS，提出實(shí)用的MEMS-SINS的初始化方案和捷聯(lián)解算算法;基于MEMS-IMU，給出衛(wèi)星/MEMS組合導(dǎo)航模塊工程化的濾波算法，實(shí)現(xiàn)非線性連續(xù)系統(tǒng)的線性離散化。仿真與實(shí)測試驗(yàn)結(jié)果表明：在MEMS-IMU精度較低的情況下，通過相關(guān)算法的合理設(shè)計(jì)，仍可以實(shí)現(xiàn)較高精度導(dǎo)航。圖9展示了自主設(shè)計(jì)的衛(wèi)星/MEMS組合導(dǎo)航模塊和集成開發(fā)軟件平臺。

　　參考文獻(xiàn)

　　[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]秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 1998.
　　[5]Di Li. Based Low-cost Inertial/GPS Integrated Navigation Platform: Design and Experiments [J]. Journal of GPS, 2007
　　[6]杜春雷. ARM體系結(jié)構(gòu)與編程[M]. 清華大學(xué)出版社，2003.