【算法基础学习4】互补滤波算法——PX4姿态估计
⽬录
前⾔：多旋翼的姿态估计算法通常采⽤⾃适应显⽰互补滤波算法、扩展卡尔曼滤波算法、梯度下降算法等，以上三种⽅法均是对姿态解算的实现，采⽤的基本思路都是利⽤陀螺仪的动态稳定性来估计实时姿态，同时由于陀螺仪随时间积分累计漂移误差的固有缺陷，需找⼀个不随时间变化影响的传感器来估计姿态并进⾏修正标定。三种⽅法各有优缺点，互补与梯度算法更适⽤于处理性能受限的飞⾏器，例如微型四轴等采⽤低频Cortex-M0或M1的处理器，⽽在条件允许性能⾜够的情况下，建议考虑采⽤扩展卡尔曼滤波的算法。
（PX4 源码中采⽤的是改进的互补滤波算法，在原有加速度计校准陀螺仪的基础上，增加磁⼒计和GPS数据进⾏更进⼀步的四元数校准补偿，它的优点是运算简单，因此成为PX4中默认的姿态解算算法）
通过下⽂介绍的多旋翼姿态估计来了解互补滤波算法
应⽤场景
如何钓草鱼
本⽂中 mahony 的应⽤场景为 多旋翼⽆⼈机的姿态估计。
陀螺仪、加速度计、MPU6050 详述
名词解释
陀螺仪
陀螺仪，测量⾓速度，具有⾼动态特性，它是⼀个间接测量⾓度的器件。它测量的是⾓度的导数，即⾓速度，要将⾓速度对时间积分才能得到⾓度。由于噪声等误差影响，在积分作⽤下不断积累，最终导致陀螺仪的低频⼲扰和漂移。
加速度计
输出当前加速度（包含重⼒加速度 g）的⽅向【也叫重⼒感应器】，在悬停时，输出为 g。由其测量
原理导致的⾼频信号敏感，使得加速度计在振动环境中⾼频⼲扰较⼤。
磁⼒计（⼜叫磁罗盘）
坐标系
导航坐标系：在多旋翼中，⼜叫地球坐标系、地理坐标系。通常，采⽤北东地（NED）构成坐标系的 X,Y,Z 轴。
机体坐标系 ：固联在多旋翼飞⾏器上，即，坐标系原点定位于飞⾏器中⼼点（假设中⼼点与重⼼点重合）。
关于航空导航⽤到的坐标系，请参考 AIAA 系列丛书。在多旋翼中，因为只在低空飞⾏，且时间较短，只需要以上两个。
姿态表⽰
欧拉⾓ ：较直观，描述刚体在三维欧式空间中的姿态。此时，坐标系为机体坐标系，随着刚体的旋转⽽旋转。缺点：万向节锁。
四元数：由⼀组四维向量，表⽰刚体的三维旋转。适合⽤于计算机计算。
⽅向余弦矩阵DCM：⽤欧拉⾓余弦值或四元数，表⽰的坐标系的旋转。
滤波原理
互补滤波要求两个信号的⼲扰噪声处在不同的频率，通过设置两个滤波器的截⽌频率，确保融合后的信号能够覆盖需求频率。
在 IMU 的姿态估计中，互补滤波器对陀螺仪（低频噪声）使⽤⾼通滤波；对加速度/磁⼒计（⾼频噪声）使⽤低频滤波。
（此处尚未对传感器数据实测，噪声和有⽤频率未知。。。。待后期补⾜）
互补滤波中，两个滤波器的截⽌频率⼀致，此时就需要根据有⽤频率的值对其进⾏取舍。
机体⽔平时，加速度计⽆法测量绕 Z 轴的旋转量，即偏航⾓。磁⼒计也有同样问题，⽆法测得要磁轴的旋转量。故，需要加速度计和磁⼒计同时对陀螺仪进⾏校正。
滤波主要过程
式中，q^ 表⽰系统姿态估计的四元数表⽰； δ 是经过 PI 调节器产⽣的新息； e 表⽰实测的惯性向量 v¯ 和预测的向量v^ 之间的相对旋转（误差）。
恐龙大世界P(⋅) —— pure quaternion operator（四元数实部为0），表⽰只有旋转。
PI 调节器中：
参数 kp ⽤于控制加速度计和陀螺仪之间的交叉频率；
参数 kI ⽤于校正陀螺仪误差。
预备知识
主要是公式，不包含推导过程。。。。
欧拉⾓与机体⾓速度的关系：
旋转矩阵与机体⾓速度的关系：
四元数与机体⾓速度的关系：
参考：北航全权⽼师课件 第五章内容、惯性导航（秦永元）第九章内容。
预测
与卡尔曼滤波相似，互补滤波也分为预测-校正。
在预测环节，由三轴陀螺仪测得的⾓速度，通过式（1）计算出四元数姿态预测。表⽰从地球坐标系到机体坐标系，或机体坐标系姿态在地球坐标系下的表⽰。
校正
在预测环节得到的四元数 ，通过加速度计和磁⼒计的值进⾏校正。该环节通常分为两部分：
彩虹少年通过加速度计得到 ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。
实名认证网站
当磁⼒计可读时，通过 校正四元数中的偏航（yaw）分量。
加速度计校正
读后感30字加速度计信号⾸先经过低通滤波器（消除⾼频噪声）：
然后，对得到的结果进⾏归⼀化（normalized）悟空歌词
北京动物园
计算偏差：
式中， v表⽰重⼒向量在机体坐标系的向量。
此时，由 v 与加速度计向量垂直分量叉乘，得到误差值。【两个物理意义相同的向量，理论上叉乘为零】磁⼒计校正
数据预处理与加速度计相同，先滤波，然后归⼀化，得到
1. ⽆ GPS 校准时：
计算误差：
式中，w 计算过程如下：
磁⼒计的输出（m）在机体坐标系下，将其转换到导航坐标系：
导航坐标系的 x 轴与正北对齐，故，可以将磁⼒计在 xoy 平⾯的投影折算到 x 轴。
再次变换到机体坐标系：
2. 有 GPS 校准时：
在 px4 中，磁⼒计使⽤ GPS 信息 [0,0,mag] 进⾏校准，故，公式与加速度计相同。
此时，由 w 与磁⼒计向量叉乘，得到误差值。【两个物理意义相同的向量，理论上叉乘为零】更新四元数
由加速度计和磁⼒计校准得到的误差值：
由该误差值得到修正值：（只有 ki 修正bias）
修正后的⾓速度值：
根据⼀阶龙格库塔⽅法求解⼀阶微分⽅程：
可以求出四元数微分⽅程的差分形式：
四元数规范化：
源码分析
该部分源码直接截取 px4 开源飞控源码（BSD证书）。
px4 为 pixhawk 飞控的固件代码，内部涉及很多滤波及导航的算法。有较⼤参考价值。
源码，参考⽇期：20160603；
参数 默认值
ATT_W_ACC 0.2f 加速度计权重
ATT_W_MAG 0.1f 磁⼒计权重
ATT_W_EXT_HDG 0.1f 外部航向权重
ATT_W_GYRO_BIAS 0.1f 陀螺仪偏差权重
ATT_MAG_DECL 0.0f 磁偏⾓（°）
ATT_MAG_DECL_A 1 启⽤基于GPS的⾃动磁偏⾓校正
ATT_EXT_HDG_M 0 外部航向模式
ATT_ACC_COMP 1 启⽤基于GPS速度的加速度补偿
ATT_BIAS_MAX 0.05f 陀螺仪偏差上限
ATT_VIBE_THRESH 0.2f 振动⽔平报警阈值
主程序运⾏流程图
函数功能简述
AttitudeEstimatorQ::AttitudeEstimatorQ(); 构造函数，初始化参数;
AttitudeEstimatorQ::~AttitudeEstimatorQ(); 析构函数，杀掉所有任务;
int AttitudeEstimatorQ::start(); 启动【attitude_estimator_q】进程，主函数⼊⼝： task_main_trampoline void AttitudeEstimatorQ::print(); 打印姿态信息；
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
{
attitude_estimator_q::instance->task_main();
}
void AttitudeEstimatorQ::task_main()； 主任务进程;
void AttitudeEstimatorQ::update_parameters(bool force);
fal: 查看参数是否更新；
true: 获取新参数, 并由磁偏⾓更新四元数；
bool AttitudeEstimatorQ::init(); 由加速度计和磁⼒计初始化旋转矩阵，有GPS时，校正磁偏⾓。
bool AttitudeEstimatorQ::update(float dt); 调⽤init(); 互补滤波
void AttitudeEstimatorQ::update_mag_declination(float new_declination); 使⽤磁偏⾓更新四元数
int attitude_estimator_q_main(int argc, char *argv[]);
attitude_estimator_q { start }：实例化instance，运⾏instance->start()；花明月暗笼轻雾
attitude_estimator_q { stop }：delete instance，指针置空；
attitude_estimator_q { status}：instance->print()，打印姿态信息。
