多传感器滤波融合的惯性定位算法

针对在导航系统姿态解算中,陀螺仪和电子罗盘在解算姿态时分别存在积分误差和磁场干扰的问题,提出了利用Kalman滤波和互补滤波相融合的算法进行定位。首先将电子罗盘和陀螺仪通过Kalman滤波得出最优估计四元数,然后利用互补滤波算法对陀螺仪的漂移进行补偿得到校正后的四元数,将此次得到的四元数和Kalman滤波得出最优估计四元数再次通过Kalman滤波对四元数进行第二次最优估计,进而输出姿态角。实验中对比了本算法和互补滤波算法、无滤波算法的效果。实验证明,该算法不仅可以有效解决方位角误差发散问题,还有效解决了磁场干扰问题,实现了高精度的方位输出。