PX4中四元数更新（毕卡求解法与一阶龙格库塔）

文章目录

一、四元数更新的毕卡求解
二、四元数更新的一阶龙格库塔
三、两种方法的区别与联系
四、PX4中的四元数更新

PX4的EKF算法和mahony算法都涉及了四元数的更新，但是它们的更新方法稍微有点区别，分别采用了毕卡求解法和一阶龙格库塔更新。这篇博客主要是记录一下这两种四元数更新方法的联系与区别。

一、四元数更新的毕卡求解

参考秦永元老师的《惯性导航》以及严恭敏老师的《捷联惯导算法与组合导航原理讲义》的以下内容：

由以上可知，根据四元数的微分方程（2.4-37）进行毕卡求解，其解的形式为（2.4-43）。

二、四元数更新的一阶龙格库塔

参考文章四元数与姿态解算完全解析及资料汇总，一阶龙格库塔的更新方法如下。

三、两种方法的区别与联系

由（2.4-43）对于毕卡求解而言，最终的结果是在原来的四元数基础上，后乘了一个增量四元数；而对于一阶龙格库塔法而言，是在原来的四元数上加了一个四元数增量。毕卡求解法，输入是角增量，而一阶龙格库塔法，输入是角速率与积分时间。毕卡求解法涉及了正余弦函数的计算，相比于一阶龙格库塔而言，计算量更大。

从公式来看，如果角度较大，毕卡求解法依然可以进行求解，而一阶龙格库塔法误差会相对较大。但是在小角度的情况下，如果将毕卡求解法中的旋转角看作小量，取其一阶近似（sinα=α,cosα=1sin\alpha=\alpha, cos\alpha=1sinα=α,cosα=1），其表达式实际和一阶龙格库塔更新法相同。

有时候，我们根据旋转前和旋转后的四元数，来计算绕body系每个轴旋转的角度（一般角度较大），这时候我们也是用的毕卡求解法的公式进行逆向运算，即先求四元数增量dq，再计算角度。

四、PX4中的四元数更新

在EKF算法中采用了毕卡求解，具体可参见eck.cpp，Ekf::predictState()函数中的相关代码如下，根据修正后的陀螺积分值corrected_delta_ang计算四元数增量dq，并乘在原来的四元数之后。打开AxisAnglef就可以看到计算方法与毕卡求解法一致。

// apply imu bias corrections
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;// subtract component of angular rate due to earth rotation
corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;const Quatf dq(AxisAnglef{corrected_delta_ang});// rotate the previous quaternion by the delta quaternion using a quaternion multiplication
_state.quat_nominal = (_state.quat_nominal * dq).normalized();_R_to_earth = Dcmf(_state.quat_nominal);

在PX4的mahony算法中则采用了一阶龙格库塔法，具体可参见attitude_estimator_q_main.cpp，attitudeEstimatorQ::update函数中相关代码如下，打开derivative1函数可以看到和一阶龙格库塔的计算方法一致：

 _rates = _gyro + _gyro_bias;// Feed forward gyrocorr += _rates;// Apply correction to state_q += _q.derivative1(corr) * dt;// Normalize quaternion_q.normalize();