基于四元數(shù)和擴(kuò)展卡爾曼濾波器的姿態(tài)解算與外力加速度同步估計(jì)算法

　　針對(duì)慣性導(dǎo)航應(yīng)用中，姿態(tài)解算與外力加速度估計(jì)互相干擾的問(wèn)題，提出一種基于四元數(shù)和擴(kuò)展卡爾曼濾波器的姿態(tài)解算與外力加速度同步估計(jì)算法。首先，利用估計(jì)的外力加速度修正傳感器加速度數(shù)據(jù)得到準(zhǔn)確的反向重力加速度，再結(jié)合地磁場(chǎng)向量通過(guò)梯度下降算法解算得到旋轉(zhuǎn)四元數(shù)的測(cè)量值；其次，構(gòu)建擴(kuò)展卡爾曼濾波模型，對(duì)旋轉(zhuǎn)四元數(shù)和外力加速度進(jìn)行更新，得到旋轉(zhuǎn)四元數(shù)的預(yù)測(cè)值和外力加速度的預(yù)測(cè)值；最后，用旋轉(zhuǎn)四元數(shù)的測(cè)量值和測(cè)量得到的加速度數(shù)據(jù)對(duì)預(yù)測(cè)值通過(guò)擴(kuò)展卡爾曼濾波的方法進(jìn)行校正，最終得到準(zhǔn)確的旋轉(zhuǎn)四元數(shù)和參考坐標(biāo)系下三軸方向上的外力加速度。實(shí)驗(yàn)表明，通過(guò)擴(kuò)展卡爾曼濾波同時(shí)對(duì)姿態(tài)和外力加速度估計(jì)的方法，能夠迅速收斂并準(zhǔn)確得機(jī)體姿態(tài)信息以及外力加速度信息，歐拉角誤差為±1. 95。，加速度誤差為±0.12 m／s2，并且該算法能有效抑制外力加速度對(duì)姿態(tài)解算的影響，準(zhǔn)確估計(jì)外力加速度。