前言

一个人可以走的更快，一群人才能走的更远，交流学习加qq:2096723956
更多保姆级PX4+ROS学习视频：https://b23.tv/ZeUDKqy
PX4固件版本　1.11.3

一、传感器特性

互补滤波的作用是将不同传感器的数据进行融合，得出一个更加准确的姿态信息．
陀螺仪
陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，最终导致陀螺仪的低频干扰和漂移。
加速度计
输出当前加速度（包含重力加速度 g）的方向【也叫重力感应器】，在悬停时，输出为 g。由其测量原理导致的高频信号敏感，使得加速度计在振动环境中高频干扰较大。
磁力计（又叫磁罗盘）
输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好，易受周围磁场干扰。

二、四元数互补滤波代码解析

代码位置

bool
AttitudeEstimatorQ::update(float dt)
{

如果初始化失败并且加速度计，陀螺仪，磁力计的数据不能正常获取，则返回错误，如果如果初始化失败并且加速度计，陀螺仪，磁力计的数据能正常获取，则返回继续初始化．

if (!_inited) {
	if (!_data_good) {
		return false;
	}
			return init_attq();
}

记录最新的四元数

Quatf q_last = _q;

定义角速率误差修正量

Vector3f corr;

float spinRate = _gyro.length();

如果航向角的外部数据来源为视觉

if (_param_att_ext_hdg_m.get() > 0 && _ext_hdg_good) {
		if (_param_att_ext_hdg_m.get() == 1) {

将视觉读数从机体坐标系旋转到导航坐标系（地理坐标系）

Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg);

将视觉的读数由向量换算到XY方向的角度，即偏航角的初始方向的角度（外部视觉得出的偏航角的初始角度不一定是地理上的正北方向加上磁偏角，这一点与磁力计不同，因为视觉不依赖于地磁）．在姿态四元数没有误差的情况下，vision_hdg_err应为零，但由于姿态四元数存在误差，因此vision_hdg_err通常不为零，该项的值即为姿态四元数中偏航角的误差．

float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));

将误差从地理坐标系旋转到机体坐标系并乘以权重加到纠正项中．

	corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
}

如果航向角的外部数据来源为mocap(运动捕捉)，其误差补偿的计算同视觉，不继续赘述

	if (_param_att_ext_hdg_m.get() == 2) {
		Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
		float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
		corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
	}
}

如果航向角的外部数据来源为磁力计

if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {

将磁力计读数从机体坐标系转换到导航坐标系

Vector3f mag_earth = _q.conjugate(_mag);

将磁力计的读数由向量换算到角度; 该角度减去磁偏;得到磁场偏差;磁偏角通过当前的GPS坐标通过查表的方式获得。

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);
}

将转换到机体坐标系，并乘以权重，作为航向角的误差补偿

	corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
}

四元数归一化，归一化之后的四元数的逆即是其共轭

_q.normalize();

将单位化的重力加速度向量（０,0,1）旋转到机体坐标系

Vector3f k(
	2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
	2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
	(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
);

在没有使用GPS补偿加速度的情况下，仅在加速度计读数在0.9g到1.1g之间时融合加速度计数据（减少漂移）

const float accel_norm_sq = _accel.norm_squared();
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) &&
				  (accel_norm_sq < upper_accel_limit * upper_accel_limit))) {

在理想条件下，旋转到机体坐标系的单位化的重力加速度向量和经过GPS运动补偿的加速度计的数据是相等的，两者的叉乘应为０。由于MEMS陀螺仪存在漂移，长时间工作有累积误差;因此实际不为０。与之间的夹角越大，则越大，所以将经过补偿的加速度计的数据和旋转到机体坐标系下的单位化重力加速度向量进行叉乘，并乘以误差权重，加到误差补偿项上．

	corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();
}

通过误差积分计算陀螺仪漂移量

if (spinRate < 0.175f) {
	_gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt);
	for (int i = 0; i < 3; i++) {
		_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
	}
}

补偿陀螺仪误差，计算经过互补滤波修正后的角速度

_rates = _gyro + _gyro_bias;

陀螺仪前馈

corr += _rates;

利用修正的角速度更新四元数，使用一阶龙格库塔法求解，四元数的导数由四元数微分方程求得，通过角速度向量可以对四元数微分方程进行更新．
_q += _q.derivative1(corr) * dt;

四元数归一化

_q.normalize();

如果四元数数据出现异常，恢复到最近一次的正常的状态，并将漂移和角速率置为零．

if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
      PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
	_q = q_last;
	_rates.zero();
	_gyro_bias.zero();
	return false;
}

return true;

}

总结

PX4中的四元数互补滤波算法主要是利用磁力计/视觉/mocap计算航向角速率的误差，利用加速度计计算俯仰/横滚角速率的误差，最后用纠正后的陀螺仪角速率更新姿态四元数．