卡尔曼滤波五个公式_基于ROS的卡尔曼滤波姿态解算

前段时间由于项目关系需要实现基于卡尔曼滤波的姿态解算，也就是说融合加速度计、陀螺仪及磁罗盘进行AHRS姿态解算，得到机器人的姿态角。

本文的学习需要有一定的卡尔曼滤波器基础，可以参考白巧克力亦唯心的卡尔曼滤波博客，或者我的另一篇文章说说我所理解的卡尔曼滤波器。

移动机器人主要工作于二维平面，其位姿用X,Y和Yaw三个状态量表示。目前获得航向角Yaw的方式主要是GPS、磁罗盘和陀螺仪等。GPS容易被墙壁阻隔，穿透性差，不适合在室内使用。陀螺仪主要用于测量车体的角速度，通过角速度的积分可以得到车身转过的角度，其优势在于更新频率高，动态响应快，但由于其存在零点漂移等原因，会存在着持续误差，误差会由于积分而随着时间的推移越来越大。磁罗盘利用地球磁场水平分量指向北极来获取绝对的航向角，其虽然不存在累计误差，但是其动态响应差，所以一般与陀螺仪融合，优势互补，以获得良好的动态响应。

多种滤波器可以用来进行AHRS姿态解算，查了很多资料，目前普遍使用的还是mahony互补滤波器。互补滤波器从加速度计和磁罗盘获得低频数据融合从陀螺仪获得的高频数据进行姿态解算，其显著优势是计算量小，适合在嵌入式平台使用比如无人机姿态解算，是现在商用最广泛的滤波器，但是动态性能低于卡尔曼滤波器，而且对于磁场干扰等情况可能无法有效处理。貌似PX4等飞控的姿态解算也在慢慢转向卡尔曼滤波器。