mahony 互补滤波器

致守

已于 2024-04-18 09:40:31 修改

阅读量19

点赞数

分类专栏： Laser Navigation 文章标签：导航定位

于 2024-04-17 20:02:11 首次发布

原文链接：https://blog.csdn.net/luoshi006/article/details/51513580?spm=1001.2101.3001.6650.9&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7ERate-9-51513580-blog-107423334.235%5Ev43%5Econtrol&depth_1-utm_source=distribute.pc_re

版权

Laser Navigation 专栏收录该内容

18 篇文章 0 订阅

订阅专栏

by luoshi006

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

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

应用场景

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

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

#名词解释

陀螺仪

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

加速度计

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

磁力计

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

坐标系

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

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

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

姿态表示

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

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

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

mahony 滤波原理

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

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

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

mahony 滤波主要过程

q ^ ˙ = 1 2 q ^ ⊗ P ( Ω ‾ + δ ) − − − − − − − − ( a ) \dot{\hat{q}}=\frac{1}{2} \hat{q} \otimes \mathbf P(\overline \Omega +\delta)--------(a) q^˙=21q^⊗P(Ω+δ)−−−−−−−−(a)

δ = k P ⋅ e + k I ⋅ ∫ e − − − − − − − − ( b ) \delta = k_P \cdot e+k_I \cdot \int e --------(b) δ=kP⋅e+kI⋅∫e−−−−−−−−(b)

e = v ‾ × v ^ − − − − − − − − − − − − ( c ) e=\overline v \times \hat v------------(c) e=v×v^−−−−−−−−−−−−(c)

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

PI 调节器中：[20160901更新]

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

预备知识

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

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

Θ ˙ = W b ω \dot \Theta = W {}^b \omega Θ˙=Wbω
= [ 1 t a n θ s i n ϕ t a n θ c o s ϕ 0 c o s ϕ − s i n ϕ 0 s i n ϕ / c o s θ c o s ϕ / c o s θ ] b ω =

⎡⎣⎢100tanθsinϕcosϕsinϕ/cosθtanθcosϕ−sinϕcosϕ/cosθ⎤⎦⎥[1��0��−��0��/��/��]

{}^b \omega =⎣⎡100tanθsinϕcosϕsinϕ/cosθtanθcosϕ−sinϕcosϕ/cosθ⎦⎤bω

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

R b e ˙ = R b e [ b ω ] × \dot{R^e_b}=R^e_b[{}^b \omega]_× Rbe˙=Rbe[bω]×

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

q ˙ e b ( t ) = 1 2 [ 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y − ω x 0 ] q e b ( t ) − − − − − − − （ 1 ） \dot q^b_e(t)=\frac{1}{2}

⎡⎣⎢⎢⎢⎢0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎤⎦⎥⎥⎥⎥[0−��−��−��0��−��−��0��−��0]

q^b_e(t) -------（1） q˙eb(t)=21⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎦⎥⎥⎤qeb(t)−−−−−−−（1）

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

预测

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

q e b ( k ) = q e b ( k − 1 ) + q ˙ e b ( k ) Δ t q^b_e(k)=q^b_e(k-1)+\dot q^b_e(k)\Delta t qeb(k)=qeb(k−1)+q˙eb(k)Δt

校正

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

通过加速度计得到 Δ q a c c ^ \widehat{\Delta q_{acc}} Δqacc ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。
当磁力计可读时，通过 Δ q m a g ^ \widehat{\Delta q_{mag}} Δqmag 校正四元数中的偏航（yaw）分量。

加速度计校正

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

y ( k ) = R C T + R C y ( k − 1 ) + T T + R C x ( k ) y(k)=\frac{RC}{T+RC}y(k-1)+\frac{T}{T+RC}x(k) y(k)=T+RCRCy(k−1)+T+RCTx(k)

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

Δ q a c c ^ = Δ q a c c ‾ ∣ ∣ Δ q a c c ‾ ∣ ∣ \widehat{\Delta q_{acc}}=\frac{\overline{\Delta q_{acc}}}{||\overline{\Delta q_{acc}}||} Δqacc=∣∣Δqacc∣∣Δqacc

计算偏差：

e = Δ q a c c ^ × v e=\widehat{\Delta q_{acc}} \times v e=Δqacc×v

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

[ v x v y v z ] = C n b [ 0 0 1 ]

⎡⎣⎢vxvyvz⎤⎦⎥[��]

=C^b_n

⎡⎣⎢001⎤⎦⎥[001]

⎣⎡vxvyvz⎦⎤=Cnb⎣⎡001⎦⎤

= [ 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] =

⎡⎣⎢2(q1q3−q0q2)2(q2q3+q0q1)q20−q21−q22+q23⎤⎦⎥[2(�1�3−�0�2)2(�2�3+�0�1)�02−�12−�22+�32]

=⎣⎡2(q1q3−q0q2)2(q2q3+q0q1)q02−q12−q22+q32⎦⎤

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

磁力计校正

数据预处理与加速度计相同，先滤波，然后归一化，得到 Δ q m a g ^ \widehat{\Delta q_{mag}} Δqmag 。

1. 无 GPS 校准时：

计算误差：

e = Δ q m a g ^ × w e=\widehat{\Delta q_{mag}} \times w e=Δqmag×w

式中， w w w 计算过程如下：

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

[ h x h y h z ] = C b n [ m x m y m z ]

⎡⎣⎢hxhyhz⎤⎦⎥[ℎ�ℎ�ℎ�]

=C^n_b

⎡⎣⎢mxmymz⎤⎦⎥[��]

⎣⎡hxhyhz⎦⎤=Cbn⎣⎡mxmymz⎦⎤

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

b x = h x 2 + h y 2 b_x=\sqrt{h^2_x+h^2_y} bx=hx2+hy2

b z = h z b_z=h_z bz=hz

再次变换到机体坐标系：

[ w x w y w z ] = C n b [ b x 0 b z ]

⎡⎣⎢wxwywz⎤⎦⎥[��]

=C^b_n

⎡⎣⎢bx0bz⎤⎦⎥[��0��]

⎣⎡wxwywz⎦⎤=Cnb⎣⎡bx0bz⎦⎤

2. 有 GPS 校准时：

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

[ w x w y w z ] = C n b [ 0 0 m a g ]

⎡⎣⎢wxwywz⎤⎦⎥[��]

=C^b_n

⎡⎣⎢00mag⎤⎦⎥[00��]

⎣⎡wxwywz⎦⎤=Cnb⎣⎡00mag⎦⎤

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

##更新四元数

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

e = e a c c + e m a g e=e_{acc}+e_{mag} e=eacc+emag

由该误差值得到修正值：（只有 ki 修正bias）

δ = K I ⋅ ∫ e d t \delta =K_I \cdot \int e dt δ=KI⋅∫edt

修正后的角速度值：

ω = ω g y r o + δ \omega = \omega_{gyro}+\delta ω=ωgyro+δ

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

q ˙ = f ( q , ω ) \dot q = f(q,\omega) q˙=f(q,ω)

q ( t + T ) = q ( t ) + T ⋅ f ( q , ω ) q(t+T)=q(t)+T \cdot f(q,\omega) q(t+T)=q(t)+T⋅f(q,ω)

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

q 0 ( t + T ) = q 0 ( t ) + T 2 [ − ω x q 1 ( t ) − ω y q 2 ( t ) − ω z q 3 ( t ) ] q_0(t+T)=q_0(t)+\frac{T}{2}[-\omega _xq_1(t)-\omega _yq_2(t)-\omega_zq_3(t)] q0(t+T)=q0(t)+2T[−ωxq1(t)−ωyq2(t)−ωzq3(t)]

四元数规范化：

q = q 0 + q 1 i + q 2 j + q 3 k q 0 2 + q 1 2 + q 2 2 + q 3 2 q=\frac{q_0+q_1i+q_2j+q_3k}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}} q=q02+q12+q22+q32q0+q1i+q2j+q3k

源码分析

该部分源码直接截取 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[])

{

<span style="color:#333333"><span style="background-color:#f9f5e9"><code>attitude_estimator_q::instance->task_main();
</code></span></span>

}

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;

<span style="color:#333333"><span style="background-color:#f9f5e9"><code>/*代码前的注释段（L34~L40）
 * @file attitude_estimator_q_main.cpp
 *
 * Attitude estimator (quaternion based)
 *姿态估计（基于四元数）
 * @author Anton Babushkin <anton.babushkin@me.com>
 */
</code></span></span>

头文件

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">（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>
</code></span></span>

using @@@

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">（l78~l82）
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);

using math::Vector;
using math::Matrix;
using math::Quaternion;
</code></span></span>

此处，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

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">(l84~l89)
class AttitudeEstimatorQ;

namespace attitude_estimator_q
{
AttitudeEstimatorQ *instance;
}//定义命名空间，通过命名空间调用instance;
</code></span></span>

###类定义: class AttitudeEstimatorQ

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">      (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);
};
</code></span></span>

构造函数

<span style="color:#333333"><span style="background-color:#f9f5e9"><code>     (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");
}
</code></span></span>

析构函数

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">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;
}
</code></span></span>

start();

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">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;
}
</code></span></span>

print();

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">l284~l292
void AttitudeEstimatorQ::print()
{//打印当前姿态信息;
	warnx("gyro status:");
	_voter_gyro.print();
	warnx("accel status:");
	_voter_accel.print();
	warnx("mag status:");
	_voter_mag.print();
}
</code></span></span>

task_main_trampoline();

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">l294~l297
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
{
	attitude_estimator_q::instance->task_main();
}
</code></span></span>

task_main();

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">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）校正磁偏角;
			//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);
		}
	}
}
</code></span></span>

update_parameters();

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">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, &param_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);
	}
}
</code></span></span>

init();

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

导航系中，D正方向朝下;
i i i 为导航坐标系（NED）的 x x x 轴（N）在机体坐标系;

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

β 2 = α 2 − ( α 2 , β 1 ) ( β 1 , β 1 ) ⋅ β 1 \beta_2=\alpha_2-\frac{(\alpha_2,\beta_1)}{(\beta_1,\beta_1)} \cdot \beta_1 β2=α2−(β1,β1)(α2,β1)⋅β1

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

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

j = k % i //叉乘求正交向量;
∣ i j k k [ 0 ] k [ 1 ] k [ 2 ] i [ 0 ] i [ 1 ] 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 ] ]

∣∣∣∣∣ik[0]i[0]jk[1]i[1]kk[2]i[2]∣∣∣∣∣|��[0]�[1]�[2]�[0]�[1]�[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]⎤⎦⎥[�[1]�[2]−�[1]�[2]�[0]�[2]−�[0]�[2]�[0]�[1]−�[0]�[1]]
∣∣∣∣∣∣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]⎦⎤

构造旋转矩阵 R R R

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

R = [ i j k ] R=

⎡⎣⎢ijk⎤⎦⎥[��]

R=⎣⎡ijk⎦⎤

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

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">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;
}
</code></span></span>

update();

init();//执行一次;

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

mag_earth = _q.conjugate(_mag);

不使用外部航向，或外部航向异常时，采用磁力计校准;
将磁力计读数从机体坐标系转换到导航坐标系;
R b n ∗ _ m a g R^n_b*\_mag Rbn∗_mag

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

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

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

将磁场偏差转换到机体坐标系，并乘以其对应权重;
R n b [ 0 0 − m a g _ e r r ] ⋅ _ w m a g R^b_n

⎡⎣⎢00−mag_err⎤⎦⎥[00−��_��]
\cdot\_w_mag Rnb⎣⎡00−mag_err⎦⎤⋅_wmag

_q.normalize();

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

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

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

⎡⎣⎢001⎤⎦⎥[001]
k=Rnb⎣⎡001⎦⎤

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

_gyro_bias += corr * (_w_gyro_bias * dt);

得到陀螺仪修正值
此处修正值为 mahony 滤波的 pi 控制器的积分部分;
因为 _gyro_bias 不归零，故不断累积;
g y r o _ b i a s + = [ M a g ∗ w m a g + A c c ∗ w a c c ] ∗ w g y r o ∗ d t gyro\_bias += [Mag * w_{mag}+Acc*w_{acc}]*w_{gyro}*dt gyro_bias+=[Mag∗wmag+Acc∗wacc]∗wgyro∗dt

_rates = _gyro + _gyro_bias;

_rates 表示角速度;
ω = ω g + δ \omega = \omega_g + \delta ω=ωg+δ

corr += _rates;

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

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

更新四元数，derivative为求导函数，使用一阶龙格库塔求导。
q ˙ = 1 2 q ω \dot q=\frac{1}{2} q \omega q˙=21qω

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">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;
}
</code></span></span>

update_mag_declination();

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">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;
	}
}
</code></span></span>

attitude_estimator_q_main();

<span style="color:#333333"><span style="background-color:#f9f5e9"><code class="language-cpp">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;
}
</code></span></span>

文章目录

应用场景
**陀螺仪**
**加速度计**
**磁力计**
**坐标系**
**姿态表示**
mahony 滤波原理
mahony 滤波主要过程
预备知识
预测
校正
加速度计校正
磁力计校正
1. 无 GPS 校准时：
2. 有 GPS 校准时：
源码分析
这里写图片描述
函数功能简述
源码分析
头文件
using @@@
namespace attitude_estimator_q
构造函数
析构函数
start();
print();
task_main_trampoline();
task_main();
update_parameters();
init();
update();
update_mag_declination();
attitude_estimator_q_main();

mahony 互补滤波器_mahony 滤波参数的方向定义-CSDN博客

致守

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
mahony 互补滤波器

陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。通过加速度计得到 Δ q a c c ^ \widehat{\Delta q_{acc}} Δqacc ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。在 px4 中，磁力计使用 GPS 信息 [ 0 , 0 , m a g ] [0, 0, mag] [0,0,mag] 进行校准，故，公式与加速度计相同。在预测环节得到的四元数 q e b ( k ) q^b_e(k) qeb(k) ，通过加速度计和磁力计的值进行校正。
复制链接

扫一扫