Pixhawk之姿态解算篇（5）_ECF EKF GD介绍_pixhawk ekf

本文链接：https://blog.csdn.net/2401_85011985/article/details/138986960

收集整理了一份《2024年最新物联网嵌入式全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升的朋友。

如果你需要这些资料，可以戳这里获取

需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人

都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

Software Version：PX4Firmware

Hardware Version：pixhawk

IDE：eclipse Juno （Windows）

四、正文

经过近一段时间的研究，主要是关于多旋翼无人机姿态解算算法的研究，开源PX4源代码里面默认使用的是基于mahony的互补滤波算法（explicit complement filter）进行姿态解算的；还有一套备用姿态解算算法是基于kalman的EKF（extended kalman filter）；然后呢，其实还有一套PX4源代码中尚未涉及的基于madgwick的梯度下降算法（gradient descent）。

1.1、三套不同的算法各有优势可有优缺点。

三套算法都可以使用，很多公司偏向于EKF，所以对EKF的要求还是比较高的，慢慢撸EKF吧，不然工作就愁了。小四轴用的比较多的还是互补滤波算法，尤其是大学生参加比赛也好，个人爱好也好，基于STM32自己制作小四轴的大多基于这套（我见过的很多都是用的CF，可能也有很多用的别的算法吧，不用太纠结这个）。梯度下降用的属于最少的了吧。最后出场的就是大头了：EKF；啥都别说了，好好学习它没错。

1.1.1、mahony算法

Mahony算法即所谓的互补滤波算法，通过PID反馈控制器把误差量反馈补偿修正陀螺仪的误差。Mahony的互补滤波算法是基于IMU系统的，即陀螺仪和加速度计，过程中不涉及磁力计的修正。而我们使用过程中一般都是AHRS系统（即MARG：Magnetic、Angular Rate、Gravity），这个就需要寻求另外的算法或者基于mahony算法进行改进。PX4源代码中就是使用的改进的mahony算法，即加入了磁力计数据和GPS数据进行姿态解算。该算法是最简单和最容易实现的算法，PX4姿态解算默认的就是使用的这个。但是滤波器截止频率不好确定且与采样频率有关，Kp大截止频率就大。

关于这一块，现在研究的比较多就是如何实现自适应调参。固定的参数不能获得所有情况下的最优运动姿态角，可以设计参数可调的自适应算法在不同运动状态下进行调节参数的大小。其参数调节规则为：正常运动状态情况下，Kp和Ki值取为系统初始化值；当运动体具有较大运动加速度或姿态变化剧烈时，应选择较小的Kp值（可取其初始化值的0.1倍），而Ki值应在同一数量级内适当取大一点。具体取值需根据实际应用系统选取。

Mahony论文：《AComplementary Filter for Attitude Estimation of a Fixed-Wing UAV》和《NonlinearComplementary Filters on the Special Orthogonal Group》

Mahony团队发表了太多的论文了，有兴趣的可以到researchgate查看：

链接：https://www.researchgate.net/profile/Robert_Mahony/publications/3

Fig.1 Explicit Complementary Filter

1.1.2 EKF算法

EKF俗称扩展卡尔曼滤波算法，优点不言而喻。补充一句：Kalman还活着，还活着呢~~~

EKF是出了名的计算量大，因为算法实现过程在不范是各种矩阵运算，对处理器的运算速度和精度要求很高对MCU的主频要求比较大，再加上系统的非线性，难以建立稳定可靠的状态方程以及合适的过程噪声协方差阵Q以及测量噪声协方差阵R。一言以蔽之，就是CPU计算负荷大。但是理解了KF以后，对整合滤波过程还是比较清晰的，很多公司都要求必须会EKF算法，优势就是在于处理过程中的预测和修正，其中一点就是可以估计陀螺仪的bias；在算法的计算处理过程中每一次都在不停的修正K和bias，相当于动态自适应算法了，主要就是调好Q、R。所以得把EKF搞懂，虽然比较难，但是必须知难而上。后续会根据直接KF求欧拉角和EKF求取四元数来更新blog，kalman filter相当有研究价值。

相关论文：《A Double-Stage Kalman Filter for Orientation TrackingWith an Integrated Processor in 9-D IMU》

Fig.2 Extended Kalman Filter

1.1.3、梯度下降法

The Gradient Descent是madgwick在2010搞出来的一套算法，其核心思想是将用加速度计和磁力计通过梯度下降法得到的姿态四元数与由陀螺仪积分得到的姿态，进行线性融合，得到最优的姿态。在算法实现过程中需要了解目标函数的建立，梯度下降法是根据目标函数求取误差向量的最小值的，这一点在madgwick的论文中尚未提及，应该是觉得默认大家都懂这个吧。

该算法首先是只用陀螺仪数据根据一阶毕卡更新四元数，然后才是梯度下降法对加速度数据和磁力计数据进行处理以便求出最小的误差四元数，最后根据权重值（线性）把两者融合到一起得到最终的姿态四元数。使用梯度下降法就是对加速度数据和磁力计数据和相应参考向量求取误差量，这一部分和mahony_base算法非常类似，mahony_base是利用向量的叉乘求取量测值和参考向量的误差的，而梯度下降法是根据反梯度方向迭代求取最小误差的（理论基础就是构造目标函数使用梯度下降法求取误差向量的最小值）。过程中最重要就是参数β需要通过实验进行确定gyro的测量噪声（measurement error）。

该算法适用于IMU和AHRS系统，其明显特点就是计算量小（computationally inexpensive），低频有效性（比如10hz）。Madgwick说该算法最后的精度比kalman_base算法要好，我也不知道，没实际测试过；基于梯度下降的数据融合算法能够显著降低对处理器速度的要求。

Madgwick论文：《An efficient orientation filter for inertial and inertial magneticsensor arrays》和《Estimation of IMU and MARG orientation using agradient descent algorithm》。梯度wiki：https://en.wikipedia.org/wiki/Gradient_descent

算法代码试下：http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/