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

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

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

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


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

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

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

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

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

从公式来看，如果角度较大，毕卡求解法依然可以进行求解，而一阶龙格库塔法误差会相对较大。但是在小角度的情况下，如果将毕卡求解法中的旋转角看作小量，取其一阶近似（$sin\alpha =\alpha ,cos\alpha =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 gyro
	corr += _rates;

	// Apply correction to state
	_q += _q.derivative1(corr) * dt;

	// Normalize quaternion
	_q.normalize();