PX4中的mahony姿态算法解析

最近抽空看了下PX4中的mahony算法，也发现了一个好的代码分析展示模型，因此参考博客PX4二次开发——基于mahony算法的姿态估计的形式，写了下自己阅读和思考的一些过程。可惜这个博客的博主貌似很久不更新了。

PX4中有两种姿态估计算法：

• EKF（扩展卡尔曼滤波），在之前的博客中也有模型的一些解析，具体参见PX4 EKF中的多传感器融合方法
• mahony AHRS算法，代码量、计算量更小、更简单

在之前介绍mahony算法的博客AHRS互补滤波（Mahony）算法及开源代码中，当时就觉得PI参数调节是难点。PX4作为应用型工程，所给出的权重参数、阈值参数应该也是经过了很多的理论分析和数据实践，因此很有参考价值。如果想看Github上的源码，参见attitude_estimator_q_main.cpp。

init_attq和update两个函数是其中最重要的函数，因此下面主要解析了这两个函数。

四元数初始化

初始化函数如下：

注释1：第396-397行代码，由于加速度测得是方向向上的1g支持力，在NED导航系中该矢量表示为[0，0，-1g]。假设加速度计测量值为[a1,a2,a3]，则可由如下推到得到k向量的值：

注释2： 400-401行代码，假设磁力计在机体系中测量值为[m1 m2 m3]，推导如下（感觉自己不推导下公式，就想不太清楚为什么这么做，不过打公式是真的费劲啊）：

四元数更新

四元数更新函数如下，由于我只看陀螺、加速度计和磁力计，所以视觉相关部分的代码就截掉了。

中间和其他传感器相关部分代码截掉了。

注释3：代码494到498行，四元数所表示的Cnb矩阵如下：

注释4：代码509行的计算和之前博客中描述的加速度计修正值计算方法一致，具体可参见AHRS互补滤波（Mahony）算法及开源代码

PX4中对mahony算法的优化

从参数设置文件attitude_estimator_q_params.c中可以看到，加速度权重为0.2，磁力计为0.1。

PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);

PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);

PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);

相比于之前博客AHRS互补滤波（Mahony）算法及开源代码中介绍的AHRS算法，我感觉改进主要有以下方面：

1. 能想明白的部分

• 初始化上，如果直接将四元数初始化为（1，0，0，0）也可以，只是需要花一些时间收敛。我比较熟悉、也比较通用的初始化方法，是先用加速度计的值计算出俯仰和横滚角，然后利用这两个角度将磁力计的测量值转换到水平方向，再利用水平方向上的磁力计值计算航向，从而进行姿态的初始化。PX4 mahony算法中的初始化还挺巧妙，但是我感觉不推公式的话，只看计算没有那么直观。
• PX4 mahony算法中，当有GPS计算出的载体加速度，补偿该加速度再进行叉乘得到误差修正值；当没有载体加速度时，对加速度计的测量值上下限进行约束，保证载体大致处于静止状态的情况下，再采用加速度计进行修正。这个限制条件的设置避免了动态情况下较大的加速度计修正误差，因为如果载体加速度较大的情况下，加速度计值不是1g。
• 在转速低于10deg/s（0.175rad/s）的情况，修正陀螺偏差_gyo_bias，实际就相当于mahony算法中的积分环节。可能是由于转速较低的情况下，静态零偏bias占据主要误差。如果转速较大，由标度因子、非正交等引起的误差就会比较显著，采用积分环节无法有效消除。这里对零偏的进行上下限限制也能有效避免出错。

2. 暂时不是特别明白的部分

• 当转速超过50deg/s时，会增加磁力计的增益（实际就是增加了权重）。如果是因为转速越快，累计误差越大，所以增加权重的话，为什么不同时增加加速度计的权重呢？

写在最后的一点碎碎念：

最近忙于健身，这篇博客断断续续写了好几天，其实每篇博客的产出都要花不少时间。写博客要思考、整理，贴图、写公式又挺花时间，加上如果工作忙，还是有点难坚持。所以，我还挺希望有人能一起讨论，交流问题，这样感觉能开拓一下思路。对于我而言，写博客的意义大概就在于写下自己思考的过程和结论，下次又想不明白的时候，翻开自己写的博客又有点恍然大悟，当然如果能帮助到其他人就更好了。

参考文献

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