PX4代码解析(6)

一、前言

上一节介绍了PX4姿态估计调用函数的流程，这一节分享一下我对PX4姿态解算的解读.首先，要理解PX4姿态解算的程序，要先从传感器的特性入手，这里主要介绍的传感器有加速度计，磁力计，陀螺仪.

二、传感器特性

1.加速度计

pixhawk上使用的为三轴加速度计，主要用于测量x,y,z三轴的加速度值，常用的传感器例如mpu6000与mpu9250，在进行PX4二次开发时，我们并不需要编写加速度计相关驱动程序，其代码已经在PX4_Firmware/src/drivers/imu下进行了实现，感兴趣可以自己查阅.这里我们只需了解加速度计的原理与特性.

如图所示，该图为加速度计简易模型，由弹簧与质量块构成，当外界有加速度时，质量块位置会发生变化，从而使得电容两端距离改变，流经电容的电流也会发生变化，通过测量电流大小，就可以得到质量块移动距离，从而得到加速度大小。理想状态下，当没有外界作用时，质量块会处于零点位置，但由于工艺，使用时间长短等各种因素，质量块可能会处于非零点位置，即所谓零偏，因此，在飞无人机之前往往需要进行校准。
此外，加速度计校准还涉及到另一个参数，这个参数是标度因数，在这里可以理解为一个比例系数，测量值乘以这个比例系数后得到实际值。
从加速度原理可以看出，在测量加速度时质量块需要不断移动，移动需要时间，因此，在高频情况下加速度计值不一定准确，低频率特性比较好.

2.陀螺仪

pixhawk上另一个比较重要的传感器就是陀螺仪，陀螺仪用于测量x,y,z三轴角速度，其基本的原理是动量守恒。当外部系统发生旋转时，内部转动装置会保持恒定的速度与方向旋转，测量这两个系统的差就可以得到当前系统角速度。为了测量x,y,z三轴角速度，通常采用万向节构成转动装置

以其中一个方向为例，当陀螺仪测量装置随着转动轴转动时，在半径方向上会存在力，使得质量块在半径方向进行周期往复运动，从而得到旋转角速度.但由于存在零点漂移，陀螺仪在低频特性较差，高频特性较好.

3.磁力计

磁力计主要用于测量当前磁场强度。为了能够测量地磁方向，通常将地磁分为水平与垂直方向，水平方向可以近似表示地磁方向。但地球的磁轴与地轴有着夹角，一般称为磁偏角，磁偏角在不同地理位置不同，因此在无人机航向计算时，需要gps获得当前经纬度，然后查表得到当前位置磁偏角，对航向进行修正（后续代码中会看到）.同样的磁力计校正也涉及到偏移与标度因数.

三、姿态解算算法

1.为什么采用四元数计算

在介绍姿态解算算法前，先来谈谈姿态的表示方式，常用的姿态表示方法有：四元数，欧拉角，方向余弦这几种方式，并且这几种方式是可以 互相转换，欧拉角虽然计算简单，但是存在退化现象；方向余弦有9个参数，导致其计算量过大，实时性不好；因此，PX4源码中采用四元数来表示姿态。

2.姿态解算的坐标系

在无人机的坐标变换过程中，一般会涉及到以下四个坐标系
1.传感器坐标系
传感器本身具有自己的测量坐标系，在安装过程中会存在安装误差，而传感器读数是在传感器坐标系下测得，为了能让无人机使用，需要将其转换到机体坐标系。但加速度计，磁力计，陀螺仪这些传感器安装时一般与无人机机体中心位置与方向都重合，所以可以粗略认为传感器坐标系与无人机坐标系重合
2.机体坐标系
一般以无人机几何中心为中心，以右手定则建立的三维直角坐标系，x轴位于无人机机体平面，以无人机前进方向为正方向；y轴在机体平面且垂直x轴，z轴垂直机体平面，以向下为正.
3.本地坐标系（local）
为了确定无人机相对于地面的速度与位置，需要引入本地坐标系（仿真中常用的地面坐标系）。一般情况下，本地坐标系是以无人机起点为坐标中心，在水平面上正北方向为x轴，正东为y轴方向，z轴垂直地面向下，这也是所谓的北东地（NED）坐标系
4.全局坐标系（global）
前面提到，在不同经纬度下，地轴与磁轴之间的磁偏角是不一样的，为了修正无人机航向，还需要引入GPS测得的地球经纬度.

3.Mahony滤波算法

下面来讲解算法，如图为mahony滤波的流程图，取自文献[1]

先来解释一下上图：
1.$a,m$与$\omega$分别是加速度计测得的加速度，磁力计测得的磁场强度以及陀螺仪测得的角速度。其实这里就引出了一个问题，为什么需要加速度计与磁力计，光靠陀螺仪不就可以得到无人机姿态吗？
确实，光靠陀螺仪是可以得到无人机姿态的，通过对得到的角速度积分就得到了姿态，但靠积分得到的姿态会存在积分误差，这个光靠陀螺仪是无法解决的，因此引入了加速度计与磁力计来解决陀螺仪积分误差。
2.四元数微分方程为$\dot{Q}=\frac{1}{2}\otimes {\omega }_{nb}^b$,式中$Q$为姿态四元数，${\omega }_{nb}^b$为无人机机体坐标系b相对于北东地（NED）坐标系n的角速度，这个式子是姿态解算的核心
3.由加速度计与磁力计得来的姿态同样存在误差，因此需要PI控制器来对误差进行修正，PI控制器的输入是由加速度计与磁力计算出的姿态与最终通过四元数微分方程计算出的实际姿态的差值，输出角速度修正值，补偿到陀螺仪，抵消陀螺仪误差
4.这个过程在无人机运行过程中循环计算，不断进行姿态解算

四、姿态解算算法代码讲解

在PX4代码解析(5)中我已经介绍了代码运行流程，本节只针对姿态解算算法重点部分进行讲解，我将这部分代码分为以下几个部分：
1.AttitudeEstimatorQ对象建立以及相关数据获取
2.四元数q的初始化
3.姿态解算

1.AttitudeEstimatorQ对象的构造函数

先来看看.AttitudeEstimatorQ对象的构造函数

//在对象的构造函数中，主要是对对象成员变量清0
AttitudeEstimatorQ::AttitudeEstimatorQ() :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
{
	_vel_prev.zero();
	_pos_acc.zero();
//gyro是陀螺仪数据，是一个1*3或者3*1向量
	_gyro.zero();
//accel表示的是加速度计数据
	_accel.zero();
//mag是磁力计数据
	_mag.zero();
//vision是视觉数据，mocap是动作捕捉的数据，暂时用不到
	_vision_hdg.zero();
	_mocap_hdg.zero();
//四元数q清0
	_q.zero();
	_rates.zero();
	_gyro_bias.zero();
	//将参数文件中参数拷贝到当前进程使用，这些参数就是你在地面站里可以修改的那些，比如加速度计的偏移等数据
    update_parameters(true);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

2.Run函数

在执行完构造函数后，该进程会执行run函数

//该函数主要是读取各传感器数据，并分配给相应变量，为后续姿态解算做前置工作
void
AttitudeEstimatorQ::Run()
{
//第一个if主要用于判断传感器那边数据准备好没有
	if (should_exit()) {
		_sensors_sub.unregisterCallback();
		exit_and_cleanup();
		return;
	}
//定义了一个sensor_combine的结构体，用于接收数据
	sensor_combined_s sensors;
//将数据拷贝到sensors
	if (_sensors_sub.update(&sensors)) {

        update_parameters();//默认参数为force
    //检查数据是否为最新陀螺仪
		// Feed validator with recent sensor data
		if (sensors.timestamp > 0) {
			_gyro(0) = sensors.gyro_rad[0];
			_gyro(1) = sensors.gyro_rad[1];
			_gyro(2) = sensors.gyro_rad[2];
		}
//更新加速度计数据
		if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
			_accel(0) = sensors.accelerometer_m_s2[0];
			_accel(1) = sensors.accelerometer_m_s2[1];
			_accel(2) = sensors.accelerometer_m_s2[2];

			if (_accel.length() < 0.01f) {
				PX4_ERR("degenerate accel!");
				return;
			}
		}

        // U更新磁力计数据
		if (_magnetometer_sub.updated()) {
			vehicle_magnetometer_s magnetometer;

			if (_magnetometer_sub.copy(&magnetometer)) {
				_mag(0) = magnetometer.magnetometer_ga[0];
				_mag(1) = magnetometer.magnetometer_ga[1];
				_mag(2) = magnetometer.magnetometer_ga[2];
//如果磁力计数据太小，则返回报错
				if (_mag.length() < 0.01f) {
					PX4_ERR("degenerate mag!");
					return;
				}
			}

		}
//数据更新完成标志位
		_data_good = true;

		// Update vision and motion capture heading
        //是否有视觉或者动作捕捉，false不使用
		_ext_hdg_good = false;

		if (_vision_odom_sub.updated()) {
			vehicle_odometry_s vision;

			if (_vision_odom_sub.copy(&vision)) {
				// validation check for vision attitude data
				bool vision_att_valid = PX4_ISFINITE(vision.q[0])
							&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
									vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
									fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
											vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);

				if (vision_att_valid) {
					Dcmf Rvis = Quatf(vision.q);
					Vector3f v(1.0f, 0.0f, 0.4f);

					// 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.transpose() * v;

					// vision external heading usage (ATT_EXT_HDG_M 1)
					if (_param_att_ext_hdg_m.get() == 1) {
						// Check for timeouts on data
						_ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
					}
				}
			}
		}
        //动捕
		if (_mocap_odom_sub.updated()) {
			vehicle_odometry_s mocap;

			if (_mocap_odom_sub.copy(&mocap)) {
				// validation check for mocap attitude data
				bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
						       && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
								       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
								       fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
										       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);

				if (mocap_att_valid) {
					Dcmf Rmoc = Quatf(mocap.q);
					Vector3f 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.transpose() * v;

					// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
					if (_param_att_ext_hdg_m.get() == 2) {
						// Check for timeouts on data
						_ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
					}
				}
			}
		}
//上面的代码是读取视觉与动捕数据，没有这些传感器的用不到
        //如果gps数据更新
		if (_gps_sub.updated()) {
            vehicle_gps_position_s gps;//定义结构体，该结构体在msg文件夹中

			if (_gps_sub.copy(&gps)) {
			//如果gps数据获取成功并且精度不错，则使用gps数据查阅磁偏角
				if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
//通过gps得到的经纬度查表，得到磁偏角，去修正磁力计数据
					update_mag_declination(math::radians(get_mag_declination(gps.lat, gps.lon)));
				}
			}
		}
//更新在NED坐标系的位置
		if (_local_position_sub.updated()) {
			vehicle_local_position_s lpos;

			if (_local_position_sub.copy(&lpos)) {
//是否进行运动加速度计算，加速度计数据比较好
				if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
				    && lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {

					/* position data is actual */
					const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
//更新速度
					/* velocity updated */
					if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
					//将时间由us转为s
						float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
                        /* calculate acceleration in body frame *///补偿加速度计
						_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
					}

					_vel_prev_t = lpos.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();//高精度定时器，得到当前时间
     //得到用于计算积分的步长时间
		const float dt = math::constrain((now - _last_time) / 1e6f, _dt_min, _dt_max);
		//更新last_time
		_last_time = now;

        if (update(dt)) {//姿态解算
            vehicle_attitude_s att = {};//
			att.timestamp = sensors.timestamp;
			_q.copyTo(att.q);

			/* the instance count is not used here */
            _att_pub.publish(att);//姿态发布

#if defined(ENABLE_LOCKSTEP_SCHEDULER)

			if (_param_est_group.get() == 3) {
				// In this case the estimator_q is running without LPE and needs
				// to publish ekf2_timestamps because SITL lockstep requires it.
				ekf2_timestamps_s ekf2_timestamps;
				ekf2_timestamps.timestamp = now;
				ekf2_timestamps.airspeed_timestamp_rel = 0;
				ekf2_timestamps.distance_sensor_timestamp_rel = 0;
				ekf2_timestamps.optical_flow_timestamp_rel = 0;
				ekf2_timestamps.vehicle_air_data_timestamp_rel = 0;
				ekf2_timestamps.vehicle_magnetometer_timestamp_rel = 0;
				ekf2_timestamps.visual_odometry_timestamp_rel = 0;
				_ekf2_timestamps_pub.publish(ekf2_timestamps);
			}

#endif
		}
	}
}
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

代码中，我对其进行了注释，下面有几个需要强调的点

1.sensor_combined_s是什么？
他是一个结构体，这个结构体是通过定义的消息自动生成，可以在msg文件中查看，在msg文件中找到sensor_combine，文件内容如下：

# Sensor readings in SI-unit form.
# These fields are scaled and offset-compensated where possible and do not
# change with board revisions and sensor updates.

uint64 timestamp				# time since system start (microseconds)

int32 RELATIVE_TIMESTAMP_INVALID = 2147483647	# (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
//陀螺仪数据
# gyro timstamp is equal to the timestamp of the message
float32[3] gyro_rad			# average angular rate measured in the XYZ body frame in rad/s over the last gyro sampling period
uint32 gyro_integral_dt		# gyro measurement sampling period in us
//加速度计数据
int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
float32[3] accelerometer_m_s2		# average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period
uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in us

uint8 CLIPPING_X = 1
uint8 CLIPPING_Y = 2
uint8 CLIPPING_Z = 4
uint8 accelerometer_clipping            # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

可以看到，在结构体sensor_combine_s中，存放了陀螺仪与加速度计的数据，所以run函数一直到_data_good = true;代码之前的工作就是更新陀螺仪，加速度计，磁力计.当_data_good=true，说明数据准备完成

2._pos_acc是运动加速度，运动加速度与加速度计测量的值不同，加速度计的值=运动加速度+重力加速度；在这里，代码利用两次速度之差除以时间得到运动加速度，这个运动加速度在后续姿态解算有用

3.update(dt)函数就是姿态解算的核心代码，下面对这部分代码进行解读

3.update姿态解算函数

我先把姿态解算的代码贴上来

bool
AttitudeEstimatorQ::update(float dt)
{
  //判断是否存在四元数初值
	if (!_inited) {
//如果数据没有准备好，就是加速度计，磁力计这些数据没有更新，则不进行下面操作
		if (!_data_good) {
			return false;
		}
		//执行四元数初始化函数
        return init_attq();//进行初始姿态获取
	}

1
2
3
4
5
6
7
8
9
10
11
12
13

1.从前面的原理可知，姿态解算的核心公式是四元数的微分方程，而为了实现微分方程的计算，我们需要一个四元数初值，四元数初值是通过init_attq（）函数产生，代码如下：

bool
AttitudeEstimatorQ::init_attq()
{
	// Rotation matrix can be easily constructed from acceleration and mag field vectors
	//初始方向余弦矩阵可以由加速度计与磁力计获得
	// 'k' is Earth Z axis (Down) unit vector in body frame
	//以加速度计测得向量反方向作为z轴，因为加速度计测量坐标系z轴向上，与NED坐标系的z轴方向相反
  Vector3f k = -_accel;
  //进行归一化，k向量除以它的二范数，目的是为了构成余弦矩阵
	k.normalize();

	// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
	//_mag*k可以看做
   Vector3f i = (_mag - k * (_mag * k));//以磁力计作为x轴，保证x与z轴正交
	i.normalize();

	// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
	向量叉乘得到y轴
    Vector3f j = k % i;//向量叉乘得到y轴

	// Fill rotation matrix
	//填充旋转矩阵
    Dcmf R;//旋转矩阵
	R.row(0) = i;
	R.row(1) = j;
	R.row(2) = k;

	// Convert to quaternion
	//转换为四元数，PX4中重载了赋值号
	_q = R;

	// Compensate for magnetic declination
	//补偿磁力计偏移
	Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
//四元数更新并重新初始化
	_q = _q * decl_rotation;
	_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
43
44
45
46
47
48

这里面需要提醒的是这句代码
Vector3f i = (_mag - k * (_mag * k))
代码中_mag与k都为向量，且向量k在经历归一化后为单位向量，因此_mag*k可以看做_mag在k向量上的投影长度，然后乘以单位向量k就变成了与k同向，长度为|_mag|*cos$\theta$的向量，再使用_mag减去该向量就得到垂直于k的向量i，具体可见下图
这里就完成了第一段讲解，接着向下看

//还是update函数
//记录上一次四元数值
	Quatf q_last = _q;

	// Angular rate of correction
	Vector3f corr;//
	float spinRate = _gyro.length();
//_ext_hdg_good==false，不使用动作捕捉与视觉
	if (_param_att_ext_hdg_m.get() > 0 && _ext_hdg_good) {
		if (_param_att_ext_hdg_m.get() == 1) {
			// Vision heading correction
			// Project heading to global frame and extract XY component
			Vector3f 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(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
		}

		if (_param_att_ext_hdg_m.get() == 2) {
			// Mocap heading correction
			// Project heading to global frame and extract XY component
			Vector3f 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(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
		}
	}
//使用磁力计
	if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
		// Magnetometer correction
    //我们使用磁力计的目的是得到无人机的偏航角，因此我们需要将磁力计的值投影到NED坐标系的XY平面，
    //磁力计得到数据是位于机体坐标系，需要把其转换到NED坐标系后投影
		// Project mag field vector to global frame and extract XY component
		//从机体坐标系转到NED系
		Vector3f mag_earth = _q.conjugate(_mag);
		//_mag_decl就是刚刚查表得到的磁偏角，这里是对磁力计补偿，得到磁偏角误差
		//wrap_pi（）函数是个限幅函数，保证磁力计误差在-pi到pi（因为我们拿磁力计去算偏航，偏航范围是这个）
		float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
		//以下四句代码，个人理解是在对磁力计进行标度因数的校正
		float gainMult = 1.0f;//标度因数
		const float fifty_dps = 0.873f;
		if (spinRate > fifty_dps) {
			gainMult = math::min(spinRate / fifty_dps, 10.0f);
		}
//转换到机体坐标系，conjugate_inversed（）是将NED坐标系向量转为机体坐标系，_param_att_w_mag为磁力计滤波系数
//磁力计标度因子
        // Project magnetometer correction to body frame
		corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
	}
//四元数归一化
	_q.normalize();

//加速度计的修正
//首先，实际加速度 =加速度计的值-重力加速度
//下面的k向量是重力加速度归一化后从NED系转到机体坐标系后的表示
	// Accelerometer correction
	// Project 'k' unit vector of earth frame to body frame
	// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
	// Optimized version with dropped zeros
 

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

	// If we are not using acceleration compensation based on GPS velocity,
	// fuse accel data only if its norm is close to 1 g (reduces drift).
	//对数据进行限幅处理
	const float accel_norm_sq = _accel.norm_squared();
	const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
	const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;

	if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) &&
					  (accel_norm_sq < upper_accel_limit * upper_accel_limit))) {
//这句代码是重点，我来解释一下：_accel这是加速度计的值，_pos_acc是通过两次速度之差计算出的运动加速度，都在机体坐标系下
//那么，_accel-_pos_acc就是机体坐标系下的重力加速度，然后再归一化。这里%代表向量叉乘，在传感器都是理想情况下，k向量与
//(_accel - _pos_acc).normalized()向量的叉乘应该为0，但由于存在传感器误差，这一项不为0，于是就得到了相应的加速度计误差.
//_param_att_w_acc.get（）为互补滤波中加速度计权重
		corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();
	}
//陀螺仪偏移估计
//_param_att_w_gyro_bias.get()陀螺仪偏移的初始值
	// Gyro bias estimation
	if (spinRate < 0.175f) {
		_gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt);
//对陀螺仪偏移每一项的限幅
		for (int i = 0; i < 3; i++) {
			_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
		}

	}
//经过修正后的角速度
	_rates = _gyro + _gyro_bias;
//得到角速度的校正值
	// Feed forward gyro
	corr += _rates;

	// Apply correction to state
    //四元数微分方程求解
	_q += _q.derivative1(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
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

五、参考文献及博客

[1]储开斌, 赵爽, 冯成涛. 基于Mahony-EKF的无人机姿态解算算法[J]. 电子测量与仪器学报, 2020, 32(12):7.
[2]Valenti, Roberto G , Dryanovski, et al. Sensors, Vol. 15, Pages 19302-19330: Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. 2015.
[3]米刚, 田增山, 金悦,等. 基于MIMU和磁力计的姿态更新算法研究[J]. 传感技术学报, 2015, 28(1):6.
[4]姿态估计（互补滤波）