Mahony 互补滤波

版权声明：本文为博主原创文章，转载请注明，欢迎交流~

Mahony互补滤波

目录(?)[+]

上接【互补滤波器】，继续学习互补滤波。。。。

参考： 
Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. 
PX4/Pixhawk—uORB深入理解和应用

---

by luoshi006

如有错误或遗漏，还请留言指出。欢迎各位留言交流，或邮箱 [email protected]。

应用场景

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

陀螺仪、加速度计、MPU6050 详述，请参考：传送门

名词解释

陀螺仪

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

加速度计

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

磁力计

输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好，易受周围磁场干扰。 
磁力计的工作原理参考：WalkAnt的博客

坐标系

导航坐标系：在多旋翼中，又叫地球坐标系、地理坐标系。通常，采用北东地（NED）构成坐标系的  X,Y,Z 轴。

机体坐标系 ：固联在多旋翼飞行器上，即，坐标系原点定位于飞行器中心点（假设中心点与重心点重合）。

关于航空导航用到的坐标系，请参考 AIAA 系列丛书。在多旋翼中，因为只在低空飞行，且时间较短，只需要以上两个。

姿态表示

欧拉角 ：较直观，描述刚体在三维欧式空间中的姿态。此时，坐标系为机体坐标系，随着刚体的旋转而旋转。缺点：万向节锁。 
详情参加：Wikipedia 传送门

四元数：由一组四维向量，表示刚体的三维旋转。适合用于计算机计算。 
详情参加：Wikipedia 传送门

方向余弦矩阵DCM：用欧拉角余弦值或四元数，表示的坐标系的旋转。

mahony 滤波原理

互补滤波要求两个信号的干扰噪声处在不同的频率，通过设置两个滤波器的截止频率，确保融合后的信号能够覆盖需求频率。 
在 IMU 的姿态估计中，互补滤波器对陀螺仪（低频噪声）使用高通滤波；对加速度/磁力计（高频噪声）使用低频滤波。 
（此处尚未对传感器数据实测，噪声和有用频率未知。。。。待后期补足）

互补滤波中，两个滤波器的截止频率一致，此时就需要根据有用频率的值对其进行取舍。

机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角。磁力计也有同样问题，无法测得要磁轴的旋转量。故，需要加速度计和磁力计同时对陀螺仪进行校正。

mahony 滤波主要过程

q^˙=12q^⊗P(Ω¯¯¯+δ)−−−−−−−−(a)

δ=kP⋅e+kI⋅∫e−−−−−−−−(b)

e=v¯×v^−−−−−−−−−−−−(c)

式中， q^  表示系统姿态估计的四元数表示；  δ  是经过 PI 调节器产生的新息；  e  表示实测的惯性向量  v¯  和预测的向量  v^  之间的相对旋转（误差）。 
P(⋅)  —— pure quaternion operator（四元数实部为0），表示只有旋转。

---

PI 调节器中：[20160901更新]

• 参数  kp  用于控制加速度计和陀螺仪之间的交叉频率；
• 参数  kI  用于校正陀螺仪误差。

---

预备知识

主要是公式，不包含推导过程。。。。

欧拉角与机体角速度的关系：

Θ˙=Wbω

=⎡⎣⎢100tanθsinϕcosϕsinϕ/cosθtanθcosϕ−sinϕcosϕ/cosθ⎤⎦⎥bω

旋转矩阵与机体角速度的关系：

Reb˙=Reb[bω]×

四元数与机体角速度的关系：

q˙be(t)=12⎡⎣⎢⎢⎢⎢0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎤⎦⎥⎥⎥⎥qbe(t)−−−−−−−（1）

参考：北航全权老师课件 第五章内容、惯性导航（秦永元）第九章内容。

预测

与卡尔曼滤波相似，mahony 滤波也分为预测-校正。 
在预测环节，由三轴陀螺仪测得的角速度，通过式（1）计算出四元数姿态预测。 qbe  表示从地球坐标系到机体坐标系，或机体坐标系姿态在地球坐标系下的表示。

qbe(k)=qbe(k−1)+q˙be(k)Δt

校正

在预测环节得到的四元数  qbe(k)  ，通过加速度计和磁力计的值进行校正。该环节通常分为两部分：

1. 通过加速度计得到  Δqaccˆ  ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。
2. 当磁力计可读时，通过  Δqmagˆ  校正四元数中的偏航（yaw）分量。

加速度计校正

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

y(k)=RCT+RCy(k−1)+TT+RCx(k)

然后，对得到的结果进行归一化（normalized）

Δqaccˆ=Δqacc¯¯¯¯¯¯¯¯||Δqacc¯¯¯¯¯¯¯¯||

计算偏差：

e=Δqaccˆ×v

式中，  v  表示重力向量在机体坐标系的向量。

⎡⎣⎢vxvyvz⎤⎦⎥=Cbn⎡⎣⎢001⎤⎦⎥

=⎡⎣⎢2(q1q3−q0q2)2(q2q3+q0q1)q20−q21−q22+q23⎤⎦⎥

此时，由  v  与加速度计向量垂直分量叉乘，得到误差值。【两个物理意义相同的向量，理论上叉乘为零】

磁力计校正

数据预处理与加速度计相同，先滤波，然后归一化，得到 Δqmagˆ  。

1. 无 GPS 校准时：

计算误差：

e=Δqmagˆ×w

式中， w  计算过程如下：

磁力计的输出（ m ）在机体坐标系下，将其转换到导航坐标系：

⎡⎣⎢hxhyhz⎤⎦⎥=Cnb⎡⎣⎢mxmymz⎤⎦⎥

导航坐标系的  x  轴与正北对齐，故，可以将磁力计在  xoy  平面的投影折算到  x  轴。

bx=h2x+h2y−−−−−−√

bz=hz

再次变换到机体坐标系：

⎡⎣⎢wxwywz⎤⎦⎥=Cbn⎡⎣⎢bx0bz⎤⎦⎥

2. 有 GPS 校准时：

在 px4 中，磁力计使用 GPS 信息  [0,0,mag]  进行校准，故，公式与加速度计相同。

⎡⎣⎢wxwywz⎤⎦⎥=Cbn⎡⎣⎢00mag⎤⎦⎥

此时，由  w  与磁力计向量叉乘，得到误差值。【两个物理意义相同的向量，理论上叉乘为零】

更新四元数

由加速度计和磁力计校准得到的误差值：

e=eacc+emag

由该误差值得到修正值：

δ=KP⋅e+KI⋅∫edt

修正后的角速度值：

ω=ωgyro+δ

根据一阶龙格库塔方法求解一阶微分方程：

q˙=f(q,ω)

q(t+T)=q(t)+T⋅f(q,ω)

可以求出四元数微分方程的差分形式：

q0(t+T)=q0(t)+T2[−ωxq1(t)−ωyq2(t)−ωzq3(t)]

四元数规范化：

q=q0+q1i+q2j+q3kq20+q21+q22+q23−−−−−−−−−−−−−√

源码分析

该部分源码直接截取 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	振动水平报警阈值

---

主程序运行流程图：

函数功能简述

1. AttitudeEstimatorQ::AttitudeEstimatorQ();

   构造函数，初始化参数;

2. AttitudeEstimatorQ::~AttitudeEstimatorQ();

   析构函数，杀掉所有任务;

3. int AttitudeEstimatorQ::start();

   启动【attitude_estimator_q】进程，主函数入口： task_main_trampoline

4. void AttitudeEstimatorQ::print();

   打印姿态信息；

5. void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])

   {

   attitude_estimator_q::instance->task_main();

   }

6. void AttitudeEstimatorQ::task_main()；

   主任务进程;

7. void AttitudeEstimatorQ::update_parameters(bool force);

   false: 查看参数是否更新；

   true: 获取新参数, 并由磁偏角更新四元数；

8. bool AttitudeEstimatorQ::init();

   由加速度计和磁力计初始化旋转矩阵，有GPS时，校正磁偏角。

9. bool AttitudeEstimatorQ::update(float dt);

   调用init(); 互补滤波

10. void AttitudeEstimatorQ::update_mag_declination(float new_declination);

    使用磁偏角更新四元数

11. 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 @me.com>
 */
1
2
3
4
5
6
7
1
2
3
4
5
6
7

头文件

（l42~l76）
#include 
#include //add missing check;
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include //视觉位置估计, 未找到文件【待查】;
#include //mocap-->vicon;
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

using @@@

（l78~l82）
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);

using math::Vector;
using math::Matrix;
using math::Quaternion;
1
2
3
4
5
6
1
2
3
4
5
6

此处，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;
1
2
3
4
5
6
7
1
2
3
4
5
6
7

类定义: 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_status

    struct {
        param_t w_acc;//ATT_W_ACC
        param_t w_mag;//ATT_W_MAG
        param_t w_ext_hdg;//ATT_W_EXT_HDG 外部航向权重;
        param_t w_gyro_bias;//ATT_W_GYRO_BIAS
        param_t mag_decl;//ATT_MAG_DECL
        param_t mag_decl_auto;//ATT_MAG_DECL_A 磁偏角自动校正;
        param_t acc_comp;//ATT_ACC_COMP
        param_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);
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114

构造函数

     (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");
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

析构函数

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

print();

l284~l292
void AttitudeEstimatorQ::print()
{//打印当前姿态信息;
    warnx("gyro status:");
    _voter_gyro.print();
    warnx("accel status:");
    _voter_accel.print();
    warnx("mag status:");
    _voter_mag.print();
}
1
2
3
4
5
6
7
8
9
10
1
2
3
4
5
6
7
8
9
10

task_main_trampoline();

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

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 again
            usleep(10000);
            PX4_WARN("Q POLL ERROR");
            continue;

        } else if (ret == 0) {
            // Poll timeout, do nothing
            PX4_WARN("Q POLL TIMEOUT");
            continue;
        }

        update_parameters(false);//检查orb是否更新;

        // Update sensors
        sensor_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_POSIX
                perf_end(_perf_accel);//执行事件结束，计算均值方差等;
                perf_end(_perf_mpu);
                perf_end(_perf_mag);
#endif

                if (_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 airspeed
        bool airspeed_updated = false;
        orb_check(_airspeed_sub, &airspeed_updated);

        if (airspeed_updated) {
            orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
        }

        // Check for timeouts on data
        if (_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）校正磁偏角;
        }
        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);
        }
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387

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);
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

init();

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

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

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

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

   β2=α2−(α2,β1)(β1,β1)⋅β1

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

3. j  为导航坐标系（NED）的  y  轴（E）在机体坐标系;

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

   ∣∣∣∣∣ik[0]i[0]jk[1]i[1]kk[2]i[2]∣∣∣∣∣=⎡⎣⎢k[1]i[2]−i[1]k[2]i[0]k[2]−k[0]i[2]k[0]i[1]−i[0]k[1]⎤⎦⎥

4. 构造旋转矩阵  R

   R.set_row(0, i); R.set_row(1, j); R.set_row(2, k);

   R=⎡⎣⎢ijk⎤⎦⎥

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

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 frame
    Vector<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 matrix
    Matrix<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 declination
    Quaternion 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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

update();

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

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

2. mag_earth = _q.conjugate(_mag);

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

   Rnb∗_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;

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

   Rbn⎡⎣⎢00−mag_err⎤⎦⎥⋅_wmag

5. _q.normalize();

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

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

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

   k=Rbn⎡⎣⎢001⎤⎦⎥

   e=k×[机体测量重力加速度]

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

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

   gyro_bias+=[Mag∗wmag+Acc∗wacc]∗wgyro∗dt

8. _rates = _gyro + _gyro_bias;

   _rates 表示角速度; 
   

   ω=ωg+δ

9. corr += _rates;

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

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

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

    q˙=12qω

l730~l817
bool AttitudeEstimatorQ::update(float dt)
{
    if (!_inited) {

        if (!_data_good) {
            return false;
        }

        return init();
    }

    Quaternion q_last = _q;//保存上一状态，以便恢复;

    // Angular rate of correction
    Vector<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 frame
            corr += _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 component
            Vector<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 frame
            corr += _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 component
        Vector<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 frame
        corr += _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 zeros
    Vector<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 gyro
    corr += _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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

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;
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

---