fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 21:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Seperate vision position estimate correction logic from gps. Add vision weight parameters. Fixes false positive of valid position indication

Browse Source
This commit is contained in:
ggregory8
2014-07-22 21:01:10 +08:00
parent ef74f4ad89
commit dfbbc6600e
1 changed files with 87 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+87 -16
View File
@@ -235,6 +235,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float eph_flow = 1.0f;
float eph_vision = 0.5f;
float epv_vision = 0.5f;
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));
@@ -281,6 +284,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
};
float w_gps_xy = 1.0f;
float w_gps_z = 1.0f;
float corr_vision[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float corr_sonar = 0.0f;
float corr_sonar_filtered = 0.0f;
@@ -637,27 +647,33 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
x_est[1] = vision.vx;
y_est[0] = vision.y;
y_est[1] = vision.vy;
z_est[0] = vision.z;
z_est[1] = vision.vz;
/* only reset the z estimate if the z weight parameter is not zero */
if (params.w_z_vision_p > MIN_VALID_W)
{
z_est[0] = vision.z;
z_est[1] = vision.vz;
}
vision_valid = true;
warnx("VISION estimate valid");
mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
}
/* calculate correction for position */
corr_gps[0][0] = vision.x - x_est[0];
corr_gps[1][0] = vision.y - y_est[0];
corr_gps[2][0] = vision.z - z_est[0];
corr_vision[0][0] = vision.x - x_est[0];
corr_vision[1][0] = vision.y - y_est[0];
corr_vision[2][0] = vision.z - z_est[0];
/* calculate correction for velocity */
corr_gps[0][1] = vision.vx - x_est[1];
corr_gps[1][1] = vision.vy - y_est[1];
corr_gps[2][1] = vision.vz - z_est[1];
corr_vision[0][1] = vision.vx - x_est[1];
corr_vision[1][1] = vision.vy - y_est[1];
corr_vision[2][1] = vision.vz - z_est[1];
eph = 0.05f;
epv = 0.05f;
// eph_flow = 0.05f;
// epv_vision = 0.05f;
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv);
// w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph);
// w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv);
}
}
@@ -810,12 +826,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* 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) || vision_valid;
bool use_gps_z = (ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W) || vision_valid;
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;
/* use VISION if it's valid and has a valid weight parameter */
bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || vision_valid;
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy;
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
@@ -834,6 +853,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
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_xy_vision_p = params.w_xy_vision_p;
float w_xy_vision_v = params.w_xy_vision_v;
float w_z_vision_p = params.w_z_vision_p;
/* reduce GPS weight if optical flow is good */
if (use_flow && flow_accurate) {
w_xy_gps_p *= params.w_gps_flow;
@@ -874,6 +897,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
/* accelerometer bias correction for VISION (use buffered rotation matrix) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f;
if (use_vision_xy) {
accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
}
if (use_vision_z) {
accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
}
/* 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 (isfinite(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
/* accelerometer bias correction for flow and baro (assume that there is no delay) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
@@ -916,10 +968,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
}
if (use_vision_z) {
epv = fminf(epv, epv_vision);
inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
}
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", 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);
memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision));
corr_baro = 0;
} else {
@@ -957,11 +1015,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
if (use_vision_xy) {
eph = fminf(eph, eph_vision);
inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);
inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);
if (w_xy_vision_v > MIN_VALID_W) {
inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_vision_v);
inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_vision_v);
}
}
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", 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);
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision));
memset(corr_flow, 0, sizeof(corr_flow));
} else {