PX4位置估计源码分析_px4位置速度估计

写在前面

源码版本：1.6.0rc1
源码位置：Firmware-1.6.0rc1\src\modules\position_estimator_inav\position_estimator_inav_main.cpp

整体框架：

整个算法的核心思想是由地理坐标系下的加速度通过积分，来获得速度、位置信息；经过2次修正产生可利用的信息，第一次是利用传感器计算修正系数产生加速度的偏差修正加速度，第二次是利用修正系数修正位置；最后可利用速度经过加速度修正，可利用的位置经过了加速度和位置修正。加速度的修正过程是由机体测量的加速度通过减去偏差，再转换到地理坐标系；位置的修正统一利用inertial_filter_correct()函数。

滤波两个函数

函数位置：Firmware-1.6.0rc1\src\modules\position_estimator_inav\inertial_filter.cpp

//x[0]: 位置
//x[1]：速度
void inertial_filter_predict(float dt, float x[2], float acc)
{
	if (PX4_ISFINITE(dt)) {
		if (!PX4_ISFINITE(acc)) {
			acc = 0.0f;
		}
		x[0] += x[1] * dt + acc * dt * dt / 2.0f;
		x[1] += acc * dt;
	}
}

//e : 每个传感器所代表的动态误差(corr 型)
//i : i=0 弥补位置 i=1 弥补速度
//w : 这个传感器弥补所占的权重
void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
{
	if (PX4_ISFINITE(e) && PX4_ISFINITE(w) && PX4_ISFINITE(dt)) {
	//ki * e *dt   相当于积分环节 用于消除静态误差
		float ewdt = e * w * dt;
		x[i] += ewdt;

		if (i == 0) {
			x[1] += w * ewdt;
		}
	}
}
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

第一个函数很好理解，就是利用物理公式 Pk = Pk-1 +Vkdt +1/2adt^2 Vk = Vk-1 + adt 得到预测的位置和速度。
第二个函数可以把它理解成一个积分环节，而积分环节作用是消除静态误差，e表示每个传感器的动态误差(corr型)，i = 0 弥补位置，i = 1 弥补速度， w则是某个传感器弥补时候所占用的权重（因为不同传感器精度不同，所以所占权重也不同）。

PX4位置估计源码分析

由于代码篇幅过长，且是多个相同功能的传感器写在了同一个函数中，而我们只关注气压传感器、GPS传感器用于修正加速度计，其他传感器用的较少，所以雷达、视觉、mocap、传感器的源代码部分就不在做粘贴分析。

从功能函数入口开始 int position_estimator_inav_thread_main(int argc, char *argv[])

int position_estimator_inav_thread_main(int argc, char *argv[])
{
	orb_advert_t mavlink_log_pub = nullptr;

	float x_est[2] = { 0.0f, 0.0f };	// pos, vel //估计值estimate
	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
	float z_est[2] = { 0.0f, 0.0f };	// pos, vel

	float est_buf[EST_BUF_SIZE][3][2];	// estimated position buffer
	float R_buf[EST_BUF_SIZE][3][3];	// rotation matrix buffer
	float R_gps[3][3];					// rotation matrix for GPS correction moment
	memset(est_buf, 0, sizeof(est_buf));
	memset(R_buf, 0, sizeof(R_buf));
	memset(R_gps, 0, sizeof(R_gps));
	int buf_ptr = 0;

	static const float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
	static const float max_eph_epv = 20.0f;	// max EPH/EPV acceptable for estimation

	float eph = max_eph_epv;
	float epv = 1.0f;//水平因子越小代表获得的数据准确度越高

	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
	memset(x_est_prev, 0, sizeof(x_est_prev));
	memset(y_est_prev, 0, sizeof(y_est_prev));
	memset(z_est_prev, 0, sizeof(z_est_prev));

	int baro_init_cnt = 0;
	int baro_init_num = 200;
	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted

	hrt_abstime accel_timestamp = 0;
	hrt_abstime baro_timestamp = 0;

	bool ref_inited = false;
	hrt_abstime ref_init_start = 0;
	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
	
	uint16_t accel_updates = 0;
	uint16_t baro_updates = 0;
	uint16_t gps_updates = 0;
	uint16_t attitude_updates = 0;
	
	hrt_abstime updates_counter_start = hrt_absolute_time();
	hrt_abstime pub_last = hrt_absolute_time();
	hrt_abstime t_prev = 0;
	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
	float acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
	float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame//静态误差
	float corr_baro = 0.0f;		//D 
	float corr_gps[3][2] = {//动态误差correction
		{ 0.0f, 0.0f },		// N (pos, vel)
		{ 0.0f, 0.0f },		// E (pos, vel)
		{ 0.0f, 0.0f },		// D (pos, vel)
	};
	float w_gps_xy = 1.0f;//动态调节参数
	float w_gps_z = 1.0f;
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

开头定义了很多变量，用于之后的计算，变量意义注释都有，没啥好说的。

/* subscribe */
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
	int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
	int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));

	/* advertise */
	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
	orb_advert_t vehicle_global_position_pub = NULL;
struct position_estimator_inav_params params;
	memset(&params, 0, sizeof(params));
	struct position_estimator_inav_param_handles pos_inav_param_handles;
	/* initialize parameter handles */
	inav_parameters_init(&pos_inav_param_handles);

	/* first parameters read at start up */
	struct parameter_update_s param_update;
	orb_copy(ORB_ID(parameter_update), parameter_update_sub,
		 &param_update); /* read from param topic to clear updated flag */
	/* first parameters update */
	inav_parameters_update(&pos_inav_param_handles, &params);
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

订阅及公告一些数据，参数初始化及参数更新。

	px4_pollfd_struct_t fds_init[1] = {};
	fds_init[0].fd = sensor_combined_sub;
	fds_init[0].events = POLLIN;
1
2
3

定义阻塞等待某个消息，消息名称为：sensor_combined_sub。

/* wait for initial baro value */
	bool wait_baro = true;
	TerrainEstimator terrain_estimator;

	thread_running = true;
	hrt_abstime baro_wait_for_sample_time = hrt_absolute_time();
	
	while (wait_baro && !thread_should_exit) {
		int ret = px4_poll(&fds_init[0], 1, 1000);
		if (ret < 0) {
			/* poll error */
			mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
		} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
			wait_baro = false;
			mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample");

		} else if (ret > 0) {
			if (fds_init[0].revents & POLLIN) {
				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);

				if (wait_baro && sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {
					baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
					baro_wait_for_sample_time = hrt_absolute_time();
					/* mean calculation over several measurements */
					if (baro_init_cnt < baro_init_num) {
						if (PX4_ISFINITE(sensor.baro_alt_meter)) {
							baro_offset += sensor.baro_alt_meter;
							baro_init_cnt++;
						}
					} else {
						wait_baro = false;
						baro_offset /= (float) baro_init_cnt;
						local_pos.z_valid = true;
						local_pos.v_z_valid = true;
					}
				}
			}
		} else {
			PX4_WARN("INAV poll timeout");
		}
	}//只是上电运行一次
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

阻塞等待消息句柄为：sensor_combined_sub的数据，获取气压值，200次求平均值。（注意：这个循环上电只会循环一次，循环结束会将wait_baro这个bool变量置为false）

下面将进入功能函数的主循环。

/* main loop */
px4_pollfd_struct_t fds[1];
fds[0].fd = vehicle_attitude_sub;
fds[0].events = POLLIN;

while (!thread_should_exit) {
	int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
	hrt_abstime t = hrt_absolute_time();

	if (ret < 0) {
		/* poll error */
		mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
		continue;

	} else if (ret > 0) {
		/* act on attitude updates */

		/* vehicle attitude */
		orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
		attitude_updates++;

		bool updated;

		/* parameter update */
		//参数更新
		orb_check(parameter_update_sub, &updated);
		if (updated) {
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
			inav_parameters_update(&pos_inav_param_handles, &params);
		}

		/* actuator */
		orb_check(actuator_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
		}
		
		/* armed */
		orb_check(armed_sub, &updated);
		if (updated) {
			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
		}

		/* sensor combined */
		orb_check(sensor_combined_sub, &updated);

		matrix::Dcmf R = matrix::Quatf(att.q);//姿态矩阵R

		if (updated) {
			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);

			if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
				/* correct accel bias */
				sensor.accelerometer_m_s2[0] -= acc_bias[0];
				sensor.accelerometer_m_s2[1] -= acc_bias[1];
				sensor.accelerometer_m_s2[2] -= acc_bias[2];

				/* transform acceleration vector from body frame to NED frame */
				for (int i = 0; i < 3; i++) {
					acc[i] = 0.0f;

					for (int j = 0; j < 3; j++) {
						acc[i] += R(i, j) * sensor.accelerometer_m_s2[j];
						//acc 表示地理坐标系下准确加速度
					}
				}

				acc[2] += CONSTANTS_ONE_G;//z轴弥补1G重力加速度

				accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative;
				accel_updates++;
			}
			if (sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {
				//高度差（气压计动态误差 corr型） = 起飞点高度 - 气压计当前高度 - z轴估计高度（负）
				//baro_offset:气压计上电初值，即起飞前气压
				//sensor.baro_alt_meter：气压计测得的当前气压高度
				corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
				
				baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
				baro_updates++;
			}
		}
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

代码开头又定义了一个需要阻塞等待的消息，其消息句柄为：vehicle_attitude_sub。这段代码的作用就是开篇给出的整体框架中的一部分（如下图），其中 加速度计获得的是机体坐标系下的加速度值，加速度的偏差（acc_bias）也要是机体系下的偏差，而后面用于积分的真实加速度（acc）是地理系下的加速度，所以要乘以旋转矩阵。

这里先提出一个问题：acc_bias[x] 从哪里获得？

每个传感器都有两种误差 ： 静态误差 动态误差（corr） 其中静态误差可以通过静止之后取平均值得到并储存，但是动态误差需要事实计算得到。

在上面这段代码最后，通过 高度差（气压计动态误差 corr型） = 起飞点高度 - 气压计当前高度 - z轴估计高度（负） 得到气压计 的corr（动态误差），下面代码也是获取其他传感器（雷达、视觉、光流、mocap、GPS）的corr，这里只粘贴出GPS如何获取的corr。

/* vehicle GPS position */
orb_check(vehicle_gps_position_sub, &updated);
if (updated) {
	orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
	bool reset_est = false;
	
	/* hysteresis for GPS quality */
	if (gps_valid) {
		if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
			gps_valid = false;
			mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost");
			warnx("[inav] GPS signal lost");
		}

	} else {
		if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
			gps_valid = true;
			reset_est = true;
			mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found");
			warnx("[inav] GPS signal found");
		}
	}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

上面部分是检测GPS数据是否有更新，并且每次使用GPS数据前都要判断GPS数据是否有效，（在上一次有效的时候判断本次是否无效，在上一次无效的时候判断本次是否有效），判断标准：GPS的定位因子是否在合理范围内，GPS提供的定位卫星是否>=3。

if (gps_valid) {
	double lat = gps.lat * 1e-7;
	double lon = gps.lon * 1e-7;
	float alt = gps.alt * 1e-3;

	/* initialize reference position if needed */
	if (!ref_inited) {//初始化GPS 数据
		if (ref_init_start == 0) {
			ref_init_start = t;

		} else if (t > ref_init_start + ref_init_delay) {
			ref_inited = true;

			/* set position estimate to (0, 0, 0), use GPS velocity for XY */
			x_est[0] = 0.0f;
			x_est[1] = gps.vel_n_m_s;
			y_est[0] = 0.0f;
			y_est[1] = gps.vel_e_m_s;

			local_pos.ref_lat = lat;
			local_pos.ref_lon = lon;
			local_pos.ref_alt = alt + z_est[0];
			local_pos.ref_timestamp = t;

			/* initialize projection */
			map_projection_init(&ref, lat, lon);
			// XXX replace this print
			warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
			mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
		}
	}
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

这一部分上电只会运行一次，通过ref_inited变量判断GPS是否初始化，初始化完成后会将其赋值true。

	if (ref_inited) {
		/* project GPS lat lon to plane */
		float gps_proj[2];
		map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
	
		/* reset position estimate when GPS becomes good */
		if (reset_est) {
			x_est[0] = gps_proj[0];
			x_est[1] = gps.vel_n_m_s;
			y_est[0] = gps_proj[1];
			y_est[1] = gps.vel_e_m_s;
		}
	
		/* calculate index of estimated values in buffer */
		int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
	
		if (est_i < 0) {
			est_i += EST_BUF_SIZE;
		}
	
		/* calculate correction for position */
		//位置corr计算 GPS值 - 估计值
		corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
		corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
		corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
	
		/* calculate correction for velocity */
		//速度corr计算  GPS值 - 估计值
		if (gps.vel_ned_valid) {
			corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
			corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
			corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
	
		} else {
			corr_gps[0][1] = 0.0f;
			corr_gps[1][1] = 0.0f;
			corr_gps[2][1] = 0.0f;
		}
	
			/* save rotation matrix at this moment */
			memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
			//计算动态权重，分母部分min_eph_epv = 2，gps.eph：水平定位因子、fmaxf(x1,x2)取两着最大
			//定位因子越小，定位精度越高，当gps.eph > min_eph_epv 相当于放大分母，使得整体权重缩小
			w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
			w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
		}//end if ref_inited.
	
	} //end if gps_valid
	else {
		/* no GPS lock */
		memset(corr_gps, 0, sizeof(corr_gps));
		ref_init_start = 0;
	}
		gps_updates++;
	}
}	
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

w_gps_xy：GPS水平定位权重，w_gps_z：GPS垂直定位权重（看注释）

因为GPS有时延，所以计算的并不是本时刻的corr。

est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
1

这样一句话是将时间戳向前移步。

//当程序走到这里时候已经获取所有传感器的corr型
//下面就是这个corr型怎么使用

matrix::Dcm<float> R = matrix::Quatf(att.q);//得到旋转矩阵
//先判断上面计算的corr型是否在合理范围内
		/* check for timeout on GPS topic */
		if (gps_valid && (t > (gps.timestamp + gps_topic_timeout))) {
			gps_valid = false;
			warnx("GPS timeout");
			mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout");
		}
		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
		dt = fmaxf(fminf(0.02, dt), 0.0002);		// constrain dt from 0.2 to 20 ms
		t_prev = t;
		
		/* increase EPH/EPV on each step */
		if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
			eph = 0.001;
		}
		if (eph < max_eph_epv) {
			eph *= 1.0f + dt;
		}
		if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
			epv = 0.001;
		}
		if (epv < max_eph_epv) {
			epv += 0.005f * dt;	// add 1m to EPV each 200s (baro drift)
//判断使用哪一个传感器(这里因为篇幅限制，只粘贴出GPS的定义变量)
/* use GPS if it's valid and reference position initialized */
bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;

float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
float w_z_gps_p = params.w_z_gps_p * w_gps_z;
float w_z_gps_v = params.w_z_gps_v * w_gps_z;
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

上面这段代码，首先通过姿态数据中定义旋转矩阵，判断上面计算的GPS的corr是否有效，（判断标准，GPS是否有效，GPS数据获取是否超时），并改变每次获取GPS之后的定位因子精度（我的理解：认为连续获取的GPS数据，越往后面的数据准确度越低），其次，定义一些使用GPS的标志位（需满足1. GPS已经初始化、2.GPS数据有效、3. GPS权重大于最小值。），最后计算动态权重（其中包含两部分 1.h文件中自带的权重 这些是定值、2.刚刚计算的动态权重）。

/* baro offset correction */
		if (use_gps_z) {
			float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
			baro_offset += offs_corr;
			corr_baro += offs_corr;
		}

		/* accelerometer bias correction for GPS (use buffered rotation matrix) */
		float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };

		if (use_gps_xy) {//地理坐标系下的
			accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
			accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
			accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
			accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
		}

		if (use_gps_z) {//我认为这里可能不用gps的z轴去计算加速度z轴动态误差，而是用下面的气压计
			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
			accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
		}
		
		//	这一部分可以看到z轴用气压计去融合的加速度计  而上面用的是GPS的z轴融合的加速度计
		//	在代码中没有雷达就会用气压计去融合加速度计z轴数据
		if (use_lidar) {
			accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar;

		} else {
			accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
		}
	*/
		/* transform error vector from NED frame to body frame */
		for (int i = 0; i < 3; i++) {
			float c = 0.0f;

			for (int j = 0; j < 3; j++) {
				c += R_gps[j][i] * accel_bias_corr[j];
			}

			if (PX4_ISFINITE(c)) {
				acc_bias[i] += c * params.w_acc_bias * dt;//用于下一次 acc 矫正
			}
		}

		/* inertial filter prediction for altitude */
		inertial_filter_predict(dt, z_est, acc[2]);
		
		/* inertial filter correction for altitude */
		if (use_lidar) {
			inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar);

		} else {//这里同样我认为会使用气压计去校正z轴，而不是用下面的GPS
			inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
		}

		if (use_gps_z) {
			epv = fminf(epv, gps.epv);

			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
			inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
		}
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

inav里面高度估计算法的理解：主要是用 acc.z 二次积分得到高度，但是 mpu6000 直接得到的 acc.z 并不能直接使用，所以用气压计算出一个矫正系数用来矫正 acc.z，然后二次积分得到高度，然后用气压计得到的高度直接矫正高度。也就是 说里面有 2 次矫正。这里得到的acc_bias[ ]用于下一次加速度计测量值减去的偏差（校正加速度）。

下面校正xy轴。

if (can_estimate_xy) {
			/* inertial filter prediction for position */
			inertial_filter_predict(dt, x_est, acc[0]);//先预测
			inertial_filter_predict(dt, y_est, acc[1]);

			if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
						acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
						corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
				memcpy(x_est, x_est_prev, sizeof(x_est));
				memcpy(y_est, y_est_prev, sizeof(y_est));
			}
			if (use_gps_xy) {//后校正
				eph = fminf(eph, gps.eph);

				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);

				if (gps.vel_ned_valid && t < gps.timestamp + gps_topic_timeout) {
					inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
					inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
				}
			}
			else {
			/* gradually reset xy velocity estimates */
			inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
		}
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

先预测，后校正的形式，预测函数与校正函数就是前文提到的两个函数。

在后面是一个冗余切换的保护机制这里没有粘贴。

	if (t > pub_last + PUB_INTERVAL) {
			pub_last = t;

			/* push current estimate to buffer */
			est_buf[buf_ptr][0][0] = x_est[0];
			est_buf[buf_ptr][0][1] = x_est[1];
			est_buf[buf_ptr][1][0] = y_est[0];
			est_buf[buf_ptr][1][1] = y_est[1];
			est_buf[buf_ptr][2][0] = z_est[0];
			est_buf[buf_ptr][2][1] = z_est[1];

			/* push current rotation matrix to buffer */
			memcpy(R_buf[buf_ptr], &R._data[0][0], sizeof(R._data));

			buf_ptr++;

			if (buf_ptr >= EST_BUF_SIZE) {
				buf_ptr = 0;
			}
			
			/* publish local position */
			local_pos.xy_valid = can_estimate_xy;
			local_pos.v_xy_valid = can_estimate_xy;
			local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
			local_pos.z_global = local_pos.z_valid && use_gps_z;
			local_pos.x = x_est[0];
			local_pos.vx = x_est[1];
			local_pos.y = y_est[0];
			local_pos.vy = y_est[1];
			local_pos.z = z_est[0];
			local_pos.vz = z_est[1];
			matrix::Eulerf euler(R);
			local_pos.yaw = euler.psi();
			local_pos.dist_bottom_valid = dist_bottom_valid;
			local_pos.eph = eph;
			local_pos.epv = epv;

			if (local_pos.dist_bottom_valid) {
				local_pos.dist_bottom = dist_ground;
				local_pos.dist_bottom_rate = - z_est[1];
			}

			local_pos.timestamp = t;

			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);

			if (local_pos.xy_global && local_pos.z_global) {
				/* publish global position */
				global_pos.timestamp = t;
				global_pos.time_utc_usec = gps.time_utc_usec;

				double est_lat, est_lon;
				map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);

				global_pos.lat = est_lat;
				global_pos.lon = est_lon;
				global_pos.alt = local_pos.ref_alt - local_pos.z;

				global_pos.vel_n = local_pos.vx;
				global_pos.vel_e = local_pos.vy;
				global_pos.vel_d = local_pos.vz;

				global_pos.yaw = local_pos.yaw;

				global_pos.eph = eph;
				global_pos.epv = epv;

				if (terrain_estimator.is_valid()) {
					global_pos.terrain_alt = global_pos.alt - terrain_estimator.get_distance_to_ground();
					global_pos.terrain_alt_valid = true;

				} else {
					global_pos.terrain_alt_valid = false;
				}

				global_pos.pressure_alt = sensor.baro_alt_meter;

				if (vehicle_global_position_pub == NULL) {
					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);

				} else {
					orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
				}
			}
		}
	}

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

数据填充、发布

总结：

1. 变量初始化
2. 计算气压计高度的零点偏移，主要是取 200 个数据求平均
3. 各传感器计算得带各自的修正系数和权重
4. 判断是否超时
5. 判断是用哪一个传感器
6. 计算权重
7. 根据使用的传感器计算加速度偏差
8. 预计位置
9. 修正位置
10. 发布

以上是个人对PX4位置估计的源码理解，同时也参考了阿木实验室better的理解思路，如有不正确的地方感谢提出批评指正，欢迎一起讨论学习QQ：1103706199。