基于ROS的卡尔曼滤波姿态解算

  以后不会在CSDN更新了，欢迎关注知乎 楼上观光。链接：基于ROS的卡尔曼滤波姿态解算
  前段时间由于项目关系需要实现基于卡尔曼滤波的姿态解算，也就是说融合加速度计、陀螺仪及磁罗盘进行AHRS姿态解算，得到机器人的姿态角。
  本文的学习需要有一定的卡尔曼滤波器基础，可以参考白巧克力亦唯心的博客。https://blog.csdn.net/heyijia0327/article/details/17487467
  移动机器人主要工作于二维平面，其位姿用X,Y和Yaw三个状态量表示。目前获得航向角Yaw的方式主要是GPS、磁罗盘和陀螺仪等。GPS容易被墙壁阻隔，穿透性差，不适合在室内使用。陀螺仪主要用于测量车体的角速度，通过角速度的积分可以得到车身转过的角度，其优势在于更新频率高，动态响应快，但由于其存在零点漂移等原因，会存在着持续误差，误差会由于积分而随着时间的推移越来越大。磁罗盘利用地球磁场水平分量指向北极来获取绝对的航向角，其虽然不存在累计误差，但是其动态响应差，所以一般与陀螺仪融合，优势互补，以获得良好的动态响应。
  多种滤波器可以用来进行AHRS姿态解算，查了很多资料，目前普遍使用的还是mahony互补滤波器。互补滤波器从加速度计和磁罗盘获得低频数据融合从陀螺仪获得的高频数据进行姿态解算，其显著优势是计算量小，适合在嵌入式平台使用比如无人机姿态解算，是现在商用最广泛的滤波器，但是动态性能低于卡尔曼滤波器，而且对于磁场干扰等情况可能无法有效处理。貌似PX4等飞控的姿态解算也在慢慢转向卡尔曼滤波器。
  决定用卡尔曼滤波器姿态解算后，网上的资料尤其是高质量的代码还是少之又少的，直到我在github上找到了大疆libing实现的基于ROS的卡尔曼滤波器姿态解算，github链接如下：https://github.com/libing64/att_ekf。
  本文首先对他的代码核心进行分析，再指出其代码存在的不足之处，也不一定是不足，只是我参考了经典论文觉得那样写滤波器更加稳定。
  Libing的这个源码应该是参考了px4的飞控，代码的具体实现与这个博客https://blog.csdn.net/hxudhdjuf/article/details/79594866基本吻合，结合该博客应该能看懂作者是如何实现卡尔曼滤波器的。
  作者的核心代码在att_ekf.cpp中，在void Att_ekf::predict(double t) 函数中实现了卡尔曼滤波中的预测，即根据运动方程预测下一时刻状态量，在void Att_ekf::update_magnetic函数中根据测量的磁场实现了状态量的更新。卡尔曼滤波器状态矩阵和测量矩阵如下图。

代码解析如下，开头为行号。

//卡尔曼滤波第一个公式，根据上一时刻状态量及角速度进行状态预测，得到先验状态量。
58、x.segment<3>(0) += wa*dt;
59、x.segment<3>(6) += -skew_symmetric(w)*ra*dt;
60、x.segment<3>(9) += -skew_symmetric(w)*rm*dt;               

//卡尔曼滤波第二个公式，根据上一时刻协方差和运动方程的协方差得到先验协方差矩阵。
//Q表明的是运动方程状态转移的不确定性。
71、P = A*P*A.transpose() + Q;//Q?

//该代码对应卡尔曼滤波第三个公式，即计算K，K表明是更相信预测值还是测量值。
//假设K = 0，则表明完全相信预测值。R表明的是测量过程的不确定性。
80、MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose() + R_mag).inverse();

//卡尔曼滤波第四个公式，融合先验状态量和测量数据得到后验状态量。
83、x = x + K*(z - H*x); 
//卡尔曼滤波第五个公式，更新后验协方差矩阵。
84、P = (I- K*H)*P.inverse();  

  而对于IMU计算出的raw和pitch更新，也和上面一样。对此，对于源代码的分析也算是结束了，但是个人在实际运行的时候发现滤波器很容易发散，后来发现是83行代码的问题，根据我所查看的论文 A Double-Stage Kalman Filter 分析，卡尔曼滤波器的架构应该如下图所示。

  根据论文中的算法，如我下图红线部分所示。在使用磁罗盘数据进行测量更新的时候，此时需要将预测值与测量值差值中除磁场部分的其余分量置为0。

具体的代码实现如下。经过此代码后，滤波器运行稳定，不再发散。

Matrix x_mag=  K*(z - H*x);
x_mag.segment<9>(0) = MatrixXd::Zero(9,1);
x = x + x_mag; 

  同样，使用IMU更新的时候也要将更新部分以外的变量置为0。

Matrix x_gyro_acc =  K*(z - H*x);   //只更新gyro和acc             
 x_gyro_acc.segment<3>(3) = MatrixXd::Zero(3,1);
 x_gyro_acc.segment<3>(9) = MatrixXd::Zero(3,1);
 x = x + x_gyro_acc;

  有同学应该对卡尔曼滤波器协方差矩阵Q和R的取值表示困惑，Q的取值应该是偏小，作者可能是认为短时间内由于角速度变化不大，预测更新可信度高。在我自己的代码实现过程中，我是参考了PX4的取法。


  r1 表示的是对陀螺仪的置信度，由于陀螺仪在短时间内非常准确，所以将其设的很小，我将其固定不变，主要调节的是加速度计协方差 r2和磁罗盘协方差r3 。对于r3 来说，这个值越大，则表明越不相信磁罗盘，抗磁干扰能力越强，静止的时候数据越稳定，数据较为平滑，但是延迟大，将其设为较小值如10的时候，灵敏度很高，静止的时候输出的航向角变化幅度相对较大。

  到这里，对于卡尔曼滤波器的姿态解算应该讲完了，结合我推荐的资料和我自己的分析，相信你应该能够理解卡尔曼滤波器的实现源码。