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

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