基于三子样更新的EKF低成本MEMS姿态估计算法

杜思远 李杰 郑涛 杨雁宇

摘要：針对低成本MEMS惯性器件精度较低、噪声较大、存在误差随时间积累而无法满足长时间载体姿态测量的问题，提出一种基于三子样姿态更新的扩展卡尔曼姿态估计算法。该算法结合三子样构造姿态更新四元数作为扩展卡尔曼滤波的状态量，利用当地导航系下重力场、磁场中相关数据通过比例积分控制器完成对角速率误差的一次补偿，同时作为扩展卡尔曼观测信息，完成对姿态四元数的实时校正，以此降低误差随时间积累的影响。为验证算法精度，设计转台静态及车载动态试验，试验结果表明在测试约为400 s的时间内，航向角误差约为5°，俯仰角及滚转角误差低于2°，且无发散问题，满足长时姿态测量要求。

关键词：惯性测量；低成本MEMS惯性器件；三子样算法；扩展卡尔曼滤波；姿态四元数