dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1221 from ggregory8/vision_estimate 

Separate position correction for vision estimate out from GPS logic. Add weight parameters. Fixes valid position indication.
This commit is contained in:
Lorenz Meier 2014-07-24 07:54:14 -07:00
parent d9f3352d72 25ef4bc4a0
commit 1b3a775e1b
3 changed files with 98 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
src/modules/position_estimator_inav/position_estimator_inav_main.c
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,27 @@ 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];
eph = 0.05f;
epv = 0.05f;
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv);
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];
}
}
@ -810,12 +820,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 +847,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 +891,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 += att.R[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 +962,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 +1009,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_vision[0][1], dt, x_est, 1, w_xy_vision_v);
inertial_filter_correct(corr_vision[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 {

9
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -43,9 +43,12 @@
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
@ -64,9 +67,12 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
{
h->w_z_baro = param_find("INAV_W_Z_BARO");
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
h->w_z_sonar = param_find("INAV_W_Z_SONAR");
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
@ -88,9 +94,12 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
{
param_get(h->w_z_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
param_get(h->w_z_vision_p, &(p->w_z_vision_p));
param_get(h->w_z_sonar, &(p->w_z_sonar));
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
param_get(h->w_xy_flow, &(p->w_xy_flow));
param_get(h->w_xy_res_v, &(p->w_xy_res_v));
param_get(h->w_gps_flow, &(p->w_gps_flow));

6
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -44,9 +44,12 @@
struct position_estimator_inav_params {
float w_z_baro;
float w_z_gps_p;
float w_z_vision_p;
float w_z_sonar;
float w_xy_gps_p;
float w_xy_gps_v;
float w_xy_vision_p;
float w_xy_vision_v;
float w_xy_flow;
float w_xy_res_v;
float w_gps_flow;
@ -65,9 +68,12 @@ struct position_estimator_inav_params {
struct position_estimator_inav_param_handles {
param_t w_z_baro;
param_t w_z_gps_p;
param_t w_z_vision_p;
param_t w_z_sonar;
param_t w_xy_gps_p;
param_t w_xy_gps_v;
param_t w_xy_vision_p;
param_t w_xy_vision_v;
param_t w_xy_flow;
param_t w_xy_res_v;
param_t w_gps_flow;