转载自：https://blog.csdn.net/qq_34994476/article/details/112346815

PX4位置估计源码分析

欲留拂指尖 2021-01-08 20:35:56 50 收藏 1

分类专栏： px4 飞控 c++

本文链接：https://blog.csdn.net/qq_34994476/article/details/112346815

版权

写在前面

源码版本：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则是某个传感器弥补时候所占用的权重（因为不同传感器精度不同，所以所占权重也不同）。

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 //估计值estimatefloat y_est[2] = { 0.0f, 0.0f };  // pos, velfloat z_est[2] = { 0.0f, 0.0f };    // pos, velfloat est_buf[EST_BUF_SIZE][3][2];   // estimated position bufferfloat R_buf[EST_BUF_SIZE][3][3];    // rotation matrix bufferfloat R_gps[3][3];                 // rotation matrix for GPS correction momentmemset(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 calculationstatic const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimationfloat 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 adjustedhrt_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 fixuint16_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 Dfloat 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(&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);

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

 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 ratehrt_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);//姿态矩阵Rif (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 printwarnx("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_validelse {/* 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 mst_prev = t;/* increase EPH/EPV on each step */if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0eph = 0.001;}if (eph < max_eph_epv) {eph *= 1.0f + dt;}if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0epv = 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轴，而不是用下面的GPSinertial_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);}}}}

数据填充、发布

总结：

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

以上是个人对PX4位置估计的源码理解，如有不正确的地方感谢提出批评指正，欢迎一起讨论学习。

PX4位置估计源码分析相关推荐

PX4 Autopilot源码分析 - 代码下载
PX4 Autopilot源码分析 - 代码下载源码地址下载硬件平台编译安装工具链配置运行 px4-Autopilot是目前最流程的无人驾驶类开源项目,社区在持续活跃状态,国内很多无人机 ...
PX4 Autopilot源码分析 - 总体架构
PX4 Autopilot源码分析 - 总体架构应用场景单独飞控飞控+任务计算机场景软件架构译自PX4 user guide,原文请参阅: https://docs.px4.io/maste ...
Yolov3Yolov4网络结构与源码分析
Yolov3&Yolov4网络结构与源码分析从2018年Yolov3年提出的两年后,在原作者声名放弃更新Yolo算法后,俄罗斯的Alexey大神扛起了Yolov4的大旗. 文章目录论文汇总 ...
jieba tfidf_【NLP】【三】jieba源码分析之关键字提取（TF-IDF/TextRank）
[一]综述利用jieba进行关键字提取时,有两种接口.一个基于TF-IDF算法,一个基于TextRank算法.TF-IDF算法,完全基于词频统计来计算词的权重,然后排序,在返回TopK个词作为关键字 ...
FATFS文件系统框架及源码分析
FATFS是一个为小型嵌入式系统设计的通用FAT(File Allocation Table)文件系统模块.FatFs 的编写遵循ANSI C,并且完全与磁盘I/O层分开.因此,它独立(不依赖)于硬件 ...
PostgreSQL源码分析
PostgreSQL源码结构 PostgreSQL的使用形态 PostgreSQL采用C/S(客户机/服务器)模式结构.应用层通过INET或者Unix Socket利用既定的协议与数据库服务器进行通信 ...
tomcat8源码分析-Connector初始化
谈起Tomcat的诞生,最早可以追溯到1995年.近20年来,Tomcat始终是使用最广泛的Web服务器,由于其使用Java语言开发,所以广为Java程序员所熟悉.很多人早期的J2EE项目,由程序员自 ...
Spring IOC 容器源码分析系列文章导读 1
1. 简介 Spring 是一个轻量级的企业级应用开发框架,于 2004 年由 Rod Johnson 发布了 1.0 版本.经过十几年的迭代,现在的 Spring 框架已经非常成熟了.Spring ...
深入理解Spark 2.1 Core （十）：Shuffle Map 端的原理与源码分析
在上一篇<深入理解Spark 2.1 Core (九):迭代计算和Shuffle的原理与源码分析>提到经过迭代计算后, SortShuffleWriter.write中: // 根据排序方 ...
Spark源码分析之Sort-Based Shuffle读写流程
一概述我们知道Spark Shuffle机制总共有三种: # 未优化的Hash Shuffle:每一个ShuffleMapTask都会为每一个ReducerTask创建一个单独的文件,总的文件数是 ...

PX4位置估计源码分析

PX4位置估计源码分析

写在前面

滤波两个函数

PX4位置估计源码分析

PX4位置估计源码分析相关推荐

最新文章

热门文章