【PX4源码】attitude_estimator_q_main.cpp姿态更新解读AttitudeEstimatorQ::init_attq()&update()

飞控的代码很有意思，我阅读这段代码是想知道飞控如何利用多传感器的数据来估计飞行器姿态，包括怎么使用四元数这个很抽象但是天才的姿态表示方法。
其实这个也就是标题的程序就是有两个函数特别重要 一是init_attq也就是姿态的初始化，另一个当然就是update姿态的更新。后面是我的理解，废话可能会比较多，俺们脑子不好的人是这样的，见谅哈哈
Part 1 init_attq()
初始化的第一步是利用加速度计的读数，可以点进去看到_accel是直接读sensors的值，那么_accel加速度计在静止时的读数是等于一个反向的重力加速度（这个关于加速度计的原理，也是个坑）。重力加速度在ned系中等于0,0，g由于只关心方向所以看作0，0，1也一样。这个矢量表示在机体系frd系中，就等于accel读数的相反，也就是-accel。
所以简单的说，k是ned系的Down方向单位向量在frd系中的投影。

	Vector3f k = -_accel;
	k.normalize();

i是什么？i是地球系ned系的x轴也就是north北，北是由磁力计mag测出来的（mag测出来的不是北，仍存在一个磁偏角，后面马上会纠正）。那么由于k是单位向量，_mag - k * (_mag * k)的意思就是把_mag中沿k方向的分量减去，从而保证求出来的i和k是正交的，orthogonal with ‘k’
j是ned的y轴指向East，就不用这么麻烦，直接叉乘得到

	// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
	Vector3f i = (_mag - k * (_mag * k));
	i.normalize();

	// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
	Vector3f j = k % i;

ijk分别表示ned三轴在frd机身系中的投影，ijk分别作为三行，可以得到一个DCM方向余弦矩阵——R
所谓的欧拉角-四元数-旋转矩阵表示的含义都是——旋转，至于谁转到谁，转坐标系还是转向量，太容易记错，我一两句也说不清（其中非常多的坑，有空单开一帖）
接下来就是_q=R得到四元数

	// Fill rotation matrix
	Dcmf R;
	R.row(0) = i;
	R.row(1) = j;
	R.row(2) = k;
	// Convert to quaternion
	_q = R;

由于上面说的mag测出的不是真正的北，用磁偏角对四元数做修正
Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
_q = _q * decl_rotation;
_mag_decl就是这个磁偏角，磁偏角和地球上的位置有关，点开磁偏角会发现这个值就是根据gps测得的经纬度计算得到的，具体什么原因我也不懂
decl_rotation是一个四元数，四元数表示一个旋转，这个旋转对应欧拉角（因为欧拉角表示旋转很直观），这里显然旋转可以用yaw上转一个磁偏角来定义
_q = _q * decl_rotation;
这个就是四元数乘法
最后记得对四元数进行归一化。PX4中的四元数使用时都是单位四元数，四元数在乘法等操作完模长就不再是1，所以每次都要归一化

bool
AttitudeEstimatorQ::init_attq()
{
	// Rotation matrix can be easily constructed from acceleration and mag field vectors
	// 'k' is Earth Z axis (Down) unit vector in body frame
	Vector3f k = -_accel;
	k.normalize();

	// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
	Vector3f i = (_mag - k * (_mag * k));
	i.normalize();

	// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
	Vector3f j = k % i;

	// Fill rotation matrix
	Dcmf R;
	R.row(0) = i;
	R.row(1) = j;
	R.row(2) = k;

	// Convert to quaternion
	_q = R;

	// Compensate for magnetic declination 
	Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
	_q = _q * decl_rotation;

	_q.normalize();

	if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
	    PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
	    _q.length() > 0.95f && _q.length() < 1.05f) {
		_inited = true;

	} else {
		_inited = false;
	}

	return _inited;
}

Part 2 update(dt)
这部分就是在运行时不断按照dt的时间间隔更新 姿态四元数q
该部分程序围绕的核心是corr
Angular rate of correction 这个of的语法我不太熟悉了，英语太烂了，所以怎么翻译都不舒服

陀螺仪能够测出飞机的三轴角速度，那么其实四元数的导数
dq=1/2Matrix_wq
可以看这篇文章中的一阶龙格库塔法
https://blog.youkuaiyun.com/weixin_42918498/article/details/127623806
也就是update函数最后的
_q += _q.derivative1(corr) * dt;
好像就通过这一行就可以完成四元数姿态q的更新，那么为什么这里代码这么多都在做什么呢？

if (_param_att_ext_hdg_m.get() > 0 && _ext_hdg_good) {
首先这里一大段都是不用看的，因为是关于视觉外部测量判断姿态的内容
从下面这段开始看，_mag是北分量，mag_earth的操作是将frd系中表示的北分量表示在

	if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
		// Magnetometer correction
		// Project mag field vector to global frame and extract XY component
		Vector3f mag_earth = _q.conjugate(_mag);
		float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
		float gainMult = 1.0f;
		const float fifty_dps = 0.873f;

		if (spinRate > fifty_dps) {
			gainMult = math::min(spinRate / fifty_dps, 10.0f);
		}

		// Project magnetometer correction to body frame
		corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
	}