基于四元数EKF的低成本MEMS姿态估计算法

Attitude Estimation Algorithm for Low Cost MEMS Based on Quaternion EKF

低成本MEMS器件具有低精度高噪声特点,其误差随时间不断累加,无法满足长时间载体姿态测量要求,针对此问题,设计了基于四元数扩展卡尔曼滤波器EKF(Extended Kalman Filter)的姿态估计算法。该算法将姿态四元数作为EKF滤波器状态,应用测量的角速率完成滤波器时间更新;应用载体所处位置的重力场与地磁场数据,完成滤波器量测更新,抑制了姿态误差发散,解决了长时间姿态测量问题。为检验算法可行性,分别开展了转台精度实验与静态实验,实验结果表明:俯仰、横滚角精度优于0.1°,航向角精度优于0.2°,2 h内静态试验期间姿态角误差标准差为0.07°,无发散问题,具备长时间姿态测量要求。

Since low-cost MEMS has the feature of low precision and high noise,and these error accumulated over time quickly,it can’t meet requirements of long time attitude measurement. For this, an attitude algorithm based on quaternion extended Kalman filter（ EKF） is designed. The algorithm treats quaternion as EKF filter state,and update state with measured angular rate. Then,update quaternion state by gravitational and geomagnetic data for inhibiting the error divergence. In order to test the feasibility of proposed algorithm, the precision and long time work test were carried out based on turntable. The results showed that pitch and roll angle accuracy were better than 0. 1°, and heading angle accuracy was better than 0. 2°. The standard deviation of attitude error was below 0. 07 in two hours test,with no error divergence. The results verify the feasibility of proposed algorithm for long time use fully.