PX4二次开发——基于mahony算法的姿态估计

    PX4里面有两种姿态估计算法，一种是基于EKF的，还有一种是基于mahony的，虽然mahony算法简单而且计算量少，但性能并不输EKF多少。这里我们讲一下基于mahony的姿态估计算法，本来我还想在正式开始前为大家补充一下坐标系、四元数、欧拉角和旋转矩阵等一些基础知识，不过笔者最近又是期末考试又是啥的，破事挺多，有点忙，也考虑到如果把这些加进去的话会使重心不突出，所以就放弃了这个想法，如果以后有机会的话我们可以再补上。

    那我们就正式开始吧。

    在讲代码之前先不做解释地抛出一张流程图和几个公式，当后面代码看不懂时，记得回来翻一下。

    基于mahony算法的姿态估计模块对应于attitude_estimator_q_main.cpp这个源文件，前面一堆代码只是简单地获取数据和参数等，并没有什么实质性地内容，我们把目光放到算法的具体实现上，以代码注释作为讲解，但是截图有点小，有时候一些很重要的内容没办法展开，我会在图的后面加上文字做补充。我截取了两个函数，一个是初始化函数，另一个是姿态更新函数，下面我们分别来讨论一下。

初始化：AttitudeEstimatorQ::init()

    首先从初始化开始，这个函数会使用加速度计和磁力计计算出代表飞行器姿态的最初的四元数，具体过程如图。

姿态更新：AttitudeEstimatorQ::update()

   接下来是获取到传感器数据后的姿态更新，update()函数比上面那个稍微复杂一点，也有几个稍微难理解的地方，我会在下面标注。在这部分开始前，请再回过头去看一下流程图和那几个公式，以方便理解下面的内容。

    由四元数微分方程可知，若初始四元数没有误差，则四元数（代表姿态）误差来自求解四元数导数时所用到的角速率，角速率的微小偏差经过累积之后会形成很大的姿态偏差，所以我们需要借助加速度计、磁力计等对陀螺仪进行校正，经过多次循环后，校正后的角速率逐渐趋向真值（类似于加了一个积分环节）。

    下面正式开始这部分内容。

第563-573行代码，使用磁力计补偿：
  若角速率没有误差，则表示姿态的四元数也不会误差，这样使用该四元数把磁力计的值旋转到NED坐标系中，arctan值补偿磁偏角之后将为0，若不为0，则代表四元数（即姿态）有误差，可以用这个值代表误差，来补偿N系偏航角角速率分量，构造补偿向量变换到机体坐标系中补偿陀螺仪得到的角速度。

第583-589行代码，使用加速度计补偿：
  R'*(0,0,1)'拆开就是上面的一堆，意思是把重力加速度向量从N系旋转到B系，而加速度计测量值扣除机体自身的加速度就是重力加速度，如果上面的旋转中四元数没有误差，则测得的重力加速度和旋转得到的重力加速度应该的重合的，即二者之间的叉乘为0，若不为零，则代表四元数（即姿态）有误差，这样叉乘得到的值就可以用来表示误差，取相反数加权重即可得到补偿向量。

第593行，_gyro_bias中存放的是陀螺仪偏置，就是对它的补偿值，每次执行update()函数，它都会累加本次对陀螺仪的补偿值，相当于一个积分环节，而我们都知道，积分环节能够消除静差，所以这样经过几次循环后，_gyro_bias会逐渐趋向真值，姿态估计误差也变得非常小。

  

    PX4的姿态估计就先讲到这里，讲的不是很详细，不过应该也很容易理解，毕竟就这么一丢丢。

    流程图是笔者亲手画的，所有的注释也都是笔者亲手加上去的，如果有问题还请多多包涵，另外尊重原创哦

    这是笔者今天发的第二篇博客，还有一些积累下来的东西，以后如果有时间的话会继续分享给大家的