目录

应用场景

名词解释

陀螺仪

加速度计

磁力计（又叫磁罗盘）

坐标系

姿态表示

滤波原理

滤波主要过程

预备知识

预测

校正

加速度计校正

磁力计校正

更新四元数

源码分析

主程序运行流程图

函数功能简述

源码分析

头文件

using @@@

namespace attitude_estimator_q

类定义: class AttitudeEstimatorQ

构造函数   

析构函数

start();

print();

task_main_trampoline();

task_main();

update_parameters();

init();

update();

update_mag_declination();

attitude_estimator_q_main();

---

前言：多旋翼的姿态估计算法通常采用自适应显示互补滤波算法、扩展卡尔曼滤波算法、梯度下降算法等，以上三种方法均是对姿态解算的实现，采用的基本思路都是利用陀螺仪的动态稳定性来估计实时姿态，同时由于陀螺仪随时间积分累计漂移误差的固有缺陷，需找一个不随时间变化影响的传感器来估计姿态并进行修正标定。三种方法各有优缺点，互补与梯度算法更适用于处理性能受限的飞行器，例如微型四轴等采用低频Cortex-M0或M1的处理器，而在条件允许性能足够的情况下，建议考虑采用扩展卡尔曼滤波的算法。

（PX4 源码中采用的是改进的互补滤波算法，在原有加速度计校准陀螺仪的基础上，增加磁力计和GPS数据进行更进一步的四元数校准补偿，它的优点是运算简单，因此成为PX4中默认的姿态解算算法）

通过下文介绍的多旋翼姿态估计来了解互补滤波算法

应用场景

本文中 mahony 的应用场景为 多旋翼无人机的姿态估计。 

 
陀螺仪、加速度计、MPU6050 详述

名词解释

陀螺仪

陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，最终导致陀螺仪的低频干扰和漂移。

加速度计

输出当前加速度（包含重力加速度 g）的方向【也叫重力感应器】，在悬停时，输出为 g。由其测量原理导致的高频信号敏感，使得加速度计在振动环境中高频干扰较大。

磁力计（又叫磁罗盘）

输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好，易受周围磁场干扰。 
磁力计的工作原理参考：http://blog.sina.com.cn/s/blog_402c071e0102v8ig.html

坐标系

• 导航坐标系：在多旋翼中，又叫地球坐标系、地理坐标系。通常，采用北东地（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）分量。

加速度计校正

加速度计信号首先经过低通滤波器（消除高频噪声）：

然后，对得到的结果进行归一化（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；

https://github.com/hsteinhaus/PX4Firmware/blob/master/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

https://github.com/ArduPilot/PX4Firmware/blob/master/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

           参数            默认值    

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

false: 查看参数是否更新；

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()，打印姿态信息。

---

源码分析

此处源码逐行分析，可以使用 Ctrl+f 快速定位;
uORB 相关的数据结构，请参考 px4/Firmware/msg;

/*代码前的注释段（L34~L40）* @file attitude_estimator_q_main.cpp** Attitude estimator (quaternion based)*姿态估计（基于四元数）* @author Anton Babushkin <anton.babushkin@me.com>*/

头文件

（l42~l76）
#include <px4_config.h>
#include <px4_posix.h>//add missing check;
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <poll.h>
#include <fcntl.h>
#include <float.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vision_position_estimate.h>//视觉位置估计, 未找到文件【待查】;
#include <uORB/topics/att_pos_mocap.h>//mocap-->vicon;
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/estimator_status.h>
#include <drivers/drv_hrt.h>#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/geo/geo.h>
#include <lib/ecl/validation/data_validator_group.h>#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>

using @@@

（l78~l82）
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);using math::Vector;
using math::Matrix;
using math::Quaternion;

此处，extern “C” 表示以 C 格式编译； __EXPORT 表示 将函数名输出到链接器（Linker）； using 关键字 表示引入名称到 using 说明出现的声明区域。。

__export 
This keyword aids those programming Microsoft Windows. __export causes the function name to be exported to the linker.

namespace attitude_estimator_q

(l84~l89)
class AttitudeEstimatorQ;namespace attitude_estimator_q
{
AttitudeEstimatorQ *instance;
}//定义命名空间，通过命名空间调用instance;

类定义: class AttitudeEstimatorQ

 

    (l92~l210)
class AttitudeEstimatorQ
{//类定义; 
public:AttitudeEstimatorQ();//构造函数~AttitudeEstimatorQ();//析构函数int        start();//开始任务，成功--返回OK；static void task_main_trampoline(int argc, char *argv[]);//跳转到 task_main() ，未使用传入参数；static函数只能被本文件中的函数调用；void        task_main();void        print();
private:static constexpr float _dt_max = 0.02;//最大时间间隔;//constexpr(constant expression)常数表达式，c11新关键字；//优化语法检查和编译速度；bool        _task_should_exit = false;//如果为 true ，任务退出；int     _control_task = -1;//进程ID, 默认-1表示没有任务；int     _sensors_sub = -1;//sensor_combined subscribe(订阅);int     _params_sub = -1;//parameter_update subscribe;int     _vision_sub = -1;//视觉位置估计;int     _mocap_sub = -1;//vicon姿态位置估计;int     _airspeed_sub = -1;//airspeed subscribe;int     _global_pos_sub = -1;//vehicle_global_position subscribe;orb_advert_t    _att_pub = nullptr;//vehicle_attitude publish(发布);orb_advert_t    _ctrl_state_pub = nullptr;//发布控制状态主题control_state;orb_advert_t    _est_state_pub = nullptr;//estimator_statusstruct {param_t w_acc;//ATT_W_ACCparam_t w_mag;//ATT_W_MAGparam_t w_ext_hdg;//ATT_W_EXT_HDG 外部航向权重;param_t w_gyro_bias;//ATT_W_GYRO_BIASparam_t mag_decl;//ATT_MAG_DECLparam_t mag_decl_auto;//ATT_MAG_DECL_A 磁偏角自动校正;param_t acc_comp;//ATT_ACC_COMPparam_t bias_max;//ATT_BIAS_MAX 陀螺仪偏差上限;param_t vibe_thresh;//ATT_VIBE_THRESH 振动报警阈值;param_t ext_hdg_mode;//ATT_EXT_HDG_M 外部航向模式;}       _params_handles;//有用参数的句柄；float       _w_accel = 0.0f;//ATT_W_ACC >>> w_acc >>> _w_accel;float       _w_mag = 0.0f;float       _w_ext_hdg = 0.0f;float       _w_gyro_bias = 0.0f;float       _mag_decl = 0.0f;bool        _mag_decl_auto = false;bool        _acc_comp = false;float       _bias_max = 0.0f;float       _vibration_warning_threshold = 1.0f;//振动警告阈值;hrt_abstime _vibration_warning_timestamp = 0;int     _ext_hdg_mode = 0;Vector<3>   _gyro;//陀螺仪;Vector<3>   _accel;//加速度计;Vector<3>   _mag;//磁力计;vision_position_estimate_s _vision = {};//buffer;Vector<3>   _vision_hdg;att_pos_mocap_s _mocap = {};//buffer;Vector<3>   _mocap_hdg;airspeed_s _airspeed = {};//buffer;Quaternion  _q;//四元数;Vector<3>   _rates;//姿态角速度;Vector<3>   _gyro_bias;//陀螺仪偏差;vehicle_global_position_s _gpos = {};//buffer;Vector<3>   _vel_prev;//前一时刻的速度：Vector<3>   _pos_acc;//加速度（body frame??）DataValidatorGroup _voter_gyro;//数据验证，剔除异常值;DataValidatorGroup _voter_accel;DataValidatorGroup _voter_mag;//姿态速度的二阶低通滤波器；math::LowPassFilter2p _lp_roll_rate;math::LowPassFilter2p _lp_pitch_rate;math::LowPassFilter2p _lp_yaw_rate;//绝对时间(ms)hrt_abstime _vel_prev_t = 0;//前一时刻计算速度时的绝对时间;bool        _inited = false;//初始化标识;bool        _data_good = false;//数据可用;bool        _failsafe = false;//故障保护;bool        _vibration_warning = false;//振动警告;bool        _ext_hdg_good = false;//外部航向可用;orb_advert_t    _mavlink_log_pub = nullptr;//mavlink log advert;//performance measuring tools (counters)perf_counter_t _update_perf;perf_counter_t _loop_perf;//未看到使用。。。;void update_parameters(bool force);//更新参数;int update_subscriptions();//未使用【待查？？】bool init();bool update(float dt);// 偏航角旋转后，立即更新磁偏角；void update_mag_declination(float new_declination);
};

构造函数
   

(l213~l235)
AttitudeEstimatorQ::AttitudeEstimatorQ() :_vel_prev(0, 0, 0),_pos_acc(0, 0, 0),_voter_gyro(3),//数据成员3个;_voter_accel(3),_voter_mag(3),_lp_roll_rate(250.0f, 30.0f),//低通滤波（采样频率,截止频率）;_lp_pitch_rate(250.0f, 30.0f),_lp_yaw_rate(250.0f, 20.0f)
{_voter_mag.set_timeout(200000);//磁力计超时;//读取Attitude_estimator_q_params.c中的参数;_params_handles.w_acc       = param_find("ATT_W_ACC");_params_handles.w_mag       = param_find("ATT_W_MAG");_params_handles.w_ext_hdg   = param_find("ATT_W_EXT_HDG");//外部航向权重;_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");_params_handles.mag_decl    = param_find("ATT_MAG_DECL");_params_handles.mag_decl_auto   = param_find("ATT_MAG_DECL_A");//磁偏角自动校正;_params_handles.acc_comp    = param_find("ATT_ACC_COMP");_params_handles.bias_max    = param_find("ATT_BIAS_MAX");//陀螺仪偏差上限;_params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");//振动警告阈值;_params_handles.ext_hdg_mode    = param_find("ATT_EXT_HDG_M");
}

析构函数

l240~l262
AttitudeEstimatorQ::~AttitudeEstimatorQ()
{//杀掉所有任务;if (_control_task != -1) {/* task wakes up every 100ms or so at the longest */_task_should_exit = true;/* wait for a second for the task to quit at our request */unsigned i = 0;do {/* wait 20ms */usleep(20000);/* if we have given up, kill it */if (++i > 50) {px4_task_delete(_control_task);break;}} while (_control_task != -1);}attitude_estimator_q::instance = nullptr;
}

start();

l264~l282
int AttitudeEstimatorQ::start()
{ASSERT(_control_task == -1);/* start the task *///启动任务，返回进程ID;_control_task = px4_task_spawn_cmd("attitude_estimator_q",/*name*/SCHED_DEFAULT,/*任务调度程序*/SCHED_PRIORITY_MAX - 5,/*优先级*/2500,/*栈大小*/(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,/*主函数入口*/nullptr);if (_control_task < 0) {warn("task start failed");return -errno;}return OK;
}

print();

l284~l292
void AttitudeEstimatorQ::print()
{//打印当前姿态信息;warnx("gyro status:");_voter_gyro.print();warnx("accel status:");_voter_accel.print();warnx("mag status:");_voter_mag.print();
}

task_main_trampoline();

l294~l297
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
{attitude_estimator_q::instance->task_main();
}

task_main();

l299~l655
void AttitudeEstimatorQ::task_main()
{#ifdef __PX4_POSIX
//记录事件执行所花费的时间，performance counters;perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay"));
#endif
//从uORB订阅主题;_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
//订阅传感器读数，包含数据参见：Firmware/msg/sensor_combined.msg_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));//视觉;_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));//vicon; _airspeed_sub = orb_subscribe(ORB_ID(airspeed));//空速，见Firmware/msg/airspeed.msg;_params_sub = orb_subscribe(ORB_ID(parameter_update));//bool saved;_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));//位置估计值(GPS);update_parameters(true);//获取新参数;hrt_abstime last_time = 0;px4_pollfd_struct_t fds[1] = {};fds[0].fd = _sensors_sub;//文件描述符;fds[0].events = POLLIN;//读取事件标识;//POLLIN: data other than high-priority data may be read without blocking;while (!_task_should_exit) {int ret = px4_poll(fds, 1, 1000);//timeout = 1000; fds_size = 1; 详见Linux的poll函数;//对字符设备读写;if (ret < 0) {// Poll error, sleep and try againusleep(10000);PX4_WARN("Q POLL ERROR");continue;} else if (ret == 0) {// Poll timeout, do nothingPX4_WARN("Q POLL TIMEOUT");continue;}update_parameters(false);//检查orb是否更新;// Update sensorssensor_combined_s sensors;int best_gyro = 0;int best_accel = 0;int best_mag = 0;if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {// Feed validator with recent sensor data//（put）将最近的传感器数据送入验证组（DataValidatorGroup）for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {//遍历每个陀螺仪数据;/* ignore empty fields *///忽略空值;if (sensors.gyro_timestamp[i] > 0) {float gyro[3];for (unsigned j = 0; j < 3; j++) {if (sensors.gyro_integral_dt[i] > 0) {//delta time 大于零;gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);//=角度/时间（1e6用于us->s转换）;} else {/* fall back to angular rate *///没有数据更新，回退;gyro[j] = sensors.gyro_rad_s[i * 3 + j];}}_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);//最后一个参数gyro_priority[]用于支持传感器优先级;}/* ignore empty fields */if (sensors.accelerometer_timestamp[i] > 0) {_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);}//NED 坐标系下, 单位 m/s^2/* ignore empty fields */if (sensors.magnetometer_timestamp[i] > 0) {_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);}//NED 坐标系下, 单位 Gauss}// Get best measurement values//获取最佳测量值（DataValidatorGroup）hrt_abstime curr_time = hrt_absolute_time();_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));_accel.set(_voter_accel.get_best(curr_time, &best_accel));_mag.set(_voter_mag.get_best(curr_time, &best_mag));if (_accel.length() < 0.01f) {warnx("WARNING: degenerate accel!");continue;}//退化，即非满秩，此处为奇异值（0）;if (_mag.length() < 0.01f) {warnx("WARNING: degenerate mag!");continue;}//同上;_data_good = true;//数据可用;if (!_failsafe) {uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;#ifdef __PX4_POSIXperf_end(_perf_accel);//执行事件结束，计算均值方差等;perf_end(_perf_mpu);perf_end(_perf_mag);
#endifif (_voter_gyro.failover_count() > 0) {//陀螺仪数据故障计数大于0, 记录错误日志;_failsafe = true;flags = _voter_gyro.failover_state();mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",_voter_gyro.failover_index(),((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));}if (_voter_accel.failover_count() > 0) {//同上，故障日志;_failsafe = true;flags = _voter_accel.failover_state();mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",_voter_accel.failover_index(),((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));}if (_voter_mag.failover_count() > 0) {//同上，故障日志;_failsafe = true;flags = _voter_mag.failover_state();mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",_voter_mag.failover_index(),((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));}if (_failsafe) {//故障安全机制;mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");}}//若启用振动报警，且振动级别超过设定阈值，触发报警; //振动级别由数据的方均根（RMS）给出;if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {if (_vibration_warning_timestamp == 0) {_vibration_warning_timestamp = curr_time;} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {_vibration_warning = true;mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),(int)(100 * _voter_accel.get_vibration_factor(curr_time)),(int)(100 * _voter_mag.get_vibration_factor(curr_time)));}} else {_vibration_warning_timestamp = 0;}}// Update vision and motion capture heading//更新视觉和vicon航向bool vision_updated = false;orb_check(_vision_sub, &vision_updated);bool mocap_updated = false;orb_check(_mocap_sub, &mocap_updated);if (vision_updated) {orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);//将订阅主题的内容复制到buffer(_vision)中;math::Quaternion q(_vision.q);math::Matrix<3, 3> Rvis = q.to_dcm();math::Vector<3> v(1.0f, 0.0f, 0.4f);//没看出 v 向量具体含义，疑似磁偏校正;// Rvis is Rwr (robot respect to world) while v is respect to world.// Hence Rvis must be transposed having (Rwr)' * Vw// Rrw * Vw = vn. This way we have consistency_vision_hdg = Rvis.transposed() * v;}//通过视觉得到的姿态估计q->Rvis，将v转换到机体坐标系;if (mocap_updated) {orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);math::Quaternion q(_mocap.q);math::Matrix<3, 3> Rmoc = q.to_dcm();math::Vector<3> v(1.0f, 0.0f, 0.4f);// Rmoc is Rwr (robot respect to world) while v is respect to world.// Hence Rmoc must be transposed having (Rwr)' * Vw// Rrw * Vw = vn. This way we have consistency_mocap_hdg = Rmoc.transposed() * v;}// Update airspeedbool airspeed_updated = false;orb_check(_airspeed_sub, &airspeed_updated);if (airspeed_updated) {orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);}// Check for timeouts on dataif (_ext_hdg_mode == 1) {_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);} else if (_ext_hdg_mode == 2) {_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);}bool gpos_updated;orb_check(_global_pos_sub, &gpos_updated);if (gpos_updated) {orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {/* set magnetic declination automatically */update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon)));}//磁偏自动校正，且水平偏差的标准差小于20，根据位置估计值(GPS)（vehicle_global_position）校正磁偏角;//get_mag_declination()函数查表得到地磁偏角，进行补偿。}if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {/* position data is actual *///基于GPS的位置信息，微分得到加速度值;if (gpos_updated) {Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);/* velocity updated */if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;//时间间隔，单位（s）/* calculate acceleration in body frame */_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);}//由ned坐标系下的速度求出机体坐标系下的加速度;_vel_prev_t = _gpos.timestamp;_vel_prev = vel;}} else {/* position data is outdated, reset acceleration *///位置信息已过时，重置;_pos_acc.zero();_vel_prev.zero();_vel_prev_t = 0;}/* time from previous iteration */hrt_abstime now = hrt_absolute_time();float dt = (last_time > 0) ? ((now  - last_time) / 1000000.0f) : 0.00001f;//用极小值0.00001表示零，预防除零错误;last_time = now;if (dt > _dt_max) {dt = _dt_max;}//时间间隔上限;if (!update(dt)) {continue;}//调用update(dt)，**互补滤波**，更新四元数;//############若不熟悉update()，请转到函数查看;Vector<3> euler = _q.to_euler();struct vehicle_attitude_s att = {};att.timestamp = sensors.timestamp;att.roll = euler(0);att.pitch = euler(1);att.yaw = euler(2);att.rollspeed = _rates(0);att.pitchspeed = _rates(1);att.yawspeed = _rates(2);for (int i = 0; i < 3; i++) {att.g_comp[i] = _accel(i) - _pos_acc(i);}//补偿重力向量;/* copy offsets */memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));//memcpy(*dest,*src,size);Matrix<3, 3> R = _q.to_dcm();/* copy rotation matrix */memcpy(&att.R[0], R.data, sizeof(att.R));att.R_valid = true;memcpy(&att.q[0], _q.data, sizeof(att.q));att.q_valid = true;//获取姿态振动, RMS;att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());/* the instance count is not used here */int att_inst;orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);//广播姿态信息;{//使用当前姿态，更新control_state，并发布;struct control_state_s ctrl_state = {};ctrl_state.timestamp = sensors.timestamp;/* attitude quaternions for control state */ctrl_state.q[0] = _q(0);ctrl_state.q[1] = _q(1);ctrl_state.q[2] = _q(2);ctrl_state.q[3] = _q(3);/* attitude rates for control state *///低通滤波，输入参数为当前值;ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));/* Airspeed - take airspeed measurement directly here as no wind is estimated */if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6&& _airspeed.timestamp > 0) {ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;ctrl_state.airspeed_valid = true;} else {ctrl_state.airspeed_valid = false;}/* the instance count is not used here */int ctrl_inst;/* publish to control state topic *///发布控制状态主题,control_state.msg。orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);}{struct estimator_status_s est = {};est.timestamp = sensors.timestamp;est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0);est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1);est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2);/* the instance count is not used here */int est_inst;/* publish to control state topic */orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);}}
}

update_parameters();

l657~l686
void AttitudeEstimatorQ::update_parameters(bool force)
{bool updated = force;if (!updated) {orb_check(_params_sub, &updated);//查看参数是否更新;}if (updated) {//获取新参数;parameter_update_s param_update;orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);param_get(_params_handles.w_acc, &_w_accel);param_get(_params_handles.w_mag, &_w_mag);param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);float mag_decl_deg = 0.0f;param_get(_params_handles.mag_decl, &mag_decl_deg);update_mag_declination(math::radians(mag_decl_deg));int32_t mag_decl_auto_int;param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);_mag_decl_auto = mag_decl_auto_int != 0;//自动磁偏角校正;int32_t acc_comp_int;param_get(_params_handles.acc_comp, &acc_comp_int);_acc_comp = acc_comp_int != 0;param_get(_params_handles.bias_max, &_bias_max);//陀螺仪偏差上限;param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);//振动警告阈值;param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);}
}

init();

1、k 为导航坐标系（NED）的 z 轴（D）在机体坐标系中的表示;

导航系中，D正方向朝下;

2、i 为导航坐标系（NED）的 x 轴（N）在机体坐标系;

i = (_mag - k * (_mag * k)); //施密特正交化;

//因 向量 k 已归一化，故分母等于1;

3、为导航坐标系（NED）的 yy 轴（E）在机体坐标系;

j = k % i //叉乘求正交向量; 

4、构造旋转矩阵 R
R.set_row(0, i); R.set_row(1, j); R.set_row(2, k);

5、转换为四元数 qq ，根据设定校正磁偏，归一化;

l688~l728
bool AttitudeEstimatorQ::init()
{// Rotation matrix can be easily constructed from acceleration and mag field vectors// 'k' is Earth Z axis (Down) unit vector in body frameVector<3> k = -_accel;k.normalize();// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'Vector<3> i = (_mag - k * (_mag * k));i.normalize();// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'Vector<3> j = k % i;// Fill rotation matrixMatrix<3, 3> R;R.set_row(0, i);R.set_row(1, j);R.set_row(2, k);// Convert to quaternion_q.from_dcm(R);// Compensate for magnetic declinationQuaternion decl_rotation;decl_rotation.from_yaw(_mag_decl);_q = decl_rotation * _q;_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;
}

update();

1、init();//执行一次;

由加速度计和磁力计初始化旋转矩阵和四元数;

2、mag_earth = _q.conjugate(_mag);

不使用外部航向，或外部航向异常时，采用磁力计校准; 
将磁力计读数从机体坐标系转换到导航坐标系; 

3、mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);

将磁力计的读数由向量换算到角度; 该角度减去磁偏，得到磁场偏差; 
_mag_decl 由GPS得到; 
**atan2f: 2 表示两个输入参数; 支持四象限内角度换算; 输出弧度值; 
**_wrap_pi: 将(0~2pi)的角度映射到(-pi~pi);

4、corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;

将磁场偏差转换到机体坐标系，并乘以其对应权重; 
 

5、_q.normalize();

四元数归一化; 
这里的归一化用于校正update_mag_declination后的偏差。

corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;

6、向量 k 是重力加速度向量(0,0,1)转换到机体坐标系; 
_accel 是加速度计的读数; 
_pos_acc 是由位置估计(GPS) 微分得到的加速度; 
_accel - _pos_acc 表示飞行器加速度向量减去其水平分量; 
k 与 (_accel - _pos_acc)叉乘，得到偏差; 

7、_gyro_bias += corr * (_w_gyro_bias * dt);

得到陀螺仪修正值 
此处修正值为 mahony 滤波的 pi 控制器的 积分部分; 
因为 _gyro_bias 不归零，故不断累积; 

8、_rates = _gyro + _gyro_bias;

_rates 表示角速度; 

9、corr += _rates;

这里的 corr = (Ea+Em) + (Ea+Em)*dt + gyro; 
个人认为这里的 corr 才是更新后的角速度;

10、_q += _q.derivative(corr) * dt;

更新四元数，derivative为求导函数，使用一阶龙格库塔求导。 

l730~l817
bool AttitudeEstimatorQ::update(float dt)
{if (!_inited) {if (!_data_good) {return false;}return init();}Quaternion q_last = _q;//保存上一状态，以便恢复;// Angular rate of correctionVector<3> corr;//初始化元素为0;//_ext_hdg_good表示外部航向数据可用;if (_ext_hdg_mode > 0 && _ext_hdg_good) {if (_ext_hdg_mode == 1) {// Vision heading correction//视觉航向校正;// Project heading to global frame and extract XY component//将航向投影到导航坐标系，提取XY分量;Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));// Project correction to body framecorr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;}if (_ext_hdg_mode == 2) {// Mocap heading correction// Project heading to global frame and extract XY componentVector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));// Project correction to body framecorr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;}}if (_ext_hdg_mode == 0  || !_ext_hdg_good) {// Magnetometer correction// Project mag field vector to global frame and extract XY componentVector<3> mag_earth = _q.conjugate(_mag);float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);// Project magnetometer correction to body framecorr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;}_q.normalize();// Accelerometer correction// Project 'k' unit vector of earth frame to body frame// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));// Optimized version with dropped zerosVector<3> 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)));corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;// Gyro bias estimation_gyro_bias += corr * (_w_gyro_bias * dt);for (int i = 0; i < 3; i++) {//陀螺仪最大偏差上限;_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);}_rates = _gyro + _gyro_bias;// Feed forward gyrocorr += _rates;// Apply correction to state_q += _q.derivative(corr) * dt;// Normalize quaternion_q.normalize();//判断四元数是否发散，若发散，则沿用之前的四元数;if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {// Reset quaternion to last good state_q = q_last;_rates.zero();_gyro_bias.zero();return false;}return true;
}

update_mag_declination();

l819~l832
void AttitudeEstimatorQ::update_mag_declination(float new_declination)
{// Apply initial declination or trivial rotations without changing estimation//忽略微小旋转;if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {_mag_decl = new_declination;} else {// Immediately rotate current estimation to avoid gyro bias growth//磁偏超过一定值后，校正姿态;Quaternion decl_rotation;decl_rotation.from_yaw(new_declination - _mag_decl);//由磁偏角度转化为四元数;_q = decl_rotation * _q;//四元数相乘表示角度相加;_mag_decl = new_declination;}
}

attitude_estimator_q_main();

l834~l890
int attitude_estimator_q_main(int argc, char *argv[])
{//外部调用接口;if (argc < 2) {warnx("usage: attitude_estimator_q {start|stop|status}");return 1;}if (!strcmp(argv[1], "start")) {if (attitude_estimator_q::instance != nullptr) {warnx("already running");return 1;}//实例化，instance;attitude_estimator_q::instance = new AttitudeEstimatorQ;if (attitude_estimator_q::instance == nullptr) {warnx("alloc failed");return 1;}if (OK != attitude_estimator_q::instance->start()) {delete attitude_estimator_q::instance;attitude_estimator_q::instance = nullptr;warnx("start failed");return 1;}return 0;}if (!strcmp(argv[1], "stop")) {if (attitude_estimator_q::instance == nullptr) {warnx("not running");return 1;}//删除实例化对象，指针置空;delete attitude_estimator_q::instance;attitude_estimator_q::instance = nullptr;return 0;}//打印当前姿态信息;if (!strcmp(argv[1], "status")) {if (attitude_estimator_q::instance) {attitude_estimator_q::instance->print();warnx("running");return 0;} else {warnx("not running");return 1;}}warnx("unrecognized command");return 1;
}

原文：https://blog.csdn.net/luoshi006/article/details/51513580