Pixhawk之姿态解算篇（6）_Gradient Descent

最新推荐文章于 2024-07-30 16:05:23 发布

_Summer__

最新推荐文章于 2024-07-30 16:05:23 发布

阅读量1.1w

点赞数 4

分类专栏： UAV 文章标签： uva 算法开发人员版权技术

本文链接：https://blog.csdn.net/qq_21842557/article/details/51881355

版权

本文介绍了Pixhawk在姿态解算中如何使用梯度下降（Gradient Descent）算法，针对多旋翼飞行器姿态估计的问题，探讨了gyro、加速度计和磁力计的补偿修正方法，以及GD在姿态估计中的应用。文章通过源代码分析解释了GD的实现过程，并强调理解算法思想的重要性。

摘要由CSDN通过智能技术生成

一、开篇

在多旋翼进行姿态估计的过程中，最简单的就是直接使用gyro测量角速度进行积分求取欧拉角（RPY），但是由于gyro自身存在的bias和drift，导致直接测量过程随着时间的推荐变得越来越不精确。所以，许多研究者想到了使用加速度计和磁力计测量重力加速度和地球磁场强度对gyro的bias和drift进行补偿修正（不是eliminate gyro’s bias and drift，该叙述是错误的，加速度计和磁力计不可能排除gyro的bias和drift）。常用的方法就是CF、GD、EKF。

但是由于磁力计测量地球磁场时极易被干扰（distortion），而且在补偿修正时磁场时无法修正RP的。所以，又有很多研究者想到了分开修正补偿求取欧拉角的方法，即通过加速度计和gyro一起求取RP，用磁力计和gyro一起修正补偿求取Yaw。该类解决方法中常见的就是two_stage EKF、two_stage GD、GD_EKF。

写了那么多就是为了引出GD~~~

三、实验平台

Software Version：PX4Firmware

Hardware Version：pixhawk

IDE：eclipse Juno （Windows）

四、正文

1、写在前面

该篇博文主要结合源代码介绍一下GD的实现过程，不再赘述理论的推导过程。说了那么多主要还是引出GD（gradient descent），关于该算法的介绍大家自行阅读madgwick的论文吧，该部分就不叙述算法推导过程的。 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。还有一个写的比较详细的关于GD介绍的blog，其中大部分都是论文翻译，英语不好的可以结合着看。链接：http://blog.csdn.net/nemol1990/article/details/23102643

2、源代码

IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError){
    firstUpdate = 0;
    //Estimated orientation quaternion elements with initial conditions.
    SEq_1 = 1;
    SEq_2 = 0;
    SE