源码版本：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;
}
}
}
第一个函数很好理解，就是利用物理公式 Pk = Pk-1 +Vkdt +1/2adt^2 Vk = Vk-1 + adt 得到预测的位置和速度。
第二个函数可以把它理解成一个积分环节，而积分环节作用是消除静态误差，e表示每个传感器的动态误差(corr型)，i = 0 弥补位置，i = 1 弥补速度， w则是某个传感器弥补时候所占用的权重（因为不同传感器精度不同，所以所占权重也不同）。
由于代码篇幅过长，且是多个相同功能的传感器写在了同一个函数中，而我们只关注气压传感器、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;
开头定义了很多变量，用于之后的计算，变量意义注释都有，没啥好说的。
/* 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(¶ms, 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,
¶m_update); /* read from param topic to clear updated flag */
/* first parameters update */
inav_parameters_update(&pos_inav_param_handles, ¶ms);
订阅及公告一些数据，参数初始化及参数更新。
px4_pollfd_struct_t fds_init[1] = {};
fds_init[0].fd = sensor_combined_sub;
fds_init[0].events = POLLIN;
定义阻塞等待某个消息，消息名称为：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");
}
}//只是上电运行一次
阻塞等待消息句柄为：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, ¶ms);
}
/* 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++;
}
}
代码开头又定义了一个需要阻塞等待的消息，其消息句柄为：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");
}
}
上面部分是检测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);
}
}
这一部分上电只会运行一次，通过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++;
}
}
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)));
这样一句话是将时间戳向前移步。
//当程序走到这里时候已经获取所有传感器的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;
上面这段代码，首先通过姿态数据中定义旋转矩阵，判断上面计算的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);
}
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);
}
先预测，后校正的形式，预测函数与校正函数就是前文提到的两个函数。
在后面是一个冗余切换的保护机制这里没有粘贴。
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);
}
}
}
}
数据填充、发布
总结：
以上是个人对PX4位置估计的源码理解，同时也参考了阿木实验室better的理解思路，如有不正确的地方感谢提出批评指正，欢迎一起讨论学习QQ：1103706199。