diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-05-20 19:47:38 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-05-20 19:47:38 +0400 |
commit | 5842c2212334876979f329b9b6bceba9609d91af (patch) | |
tree | 9eec1b7539f17b6a4b02ae01ca8468b1af0a5df5 | |
parent | e8ec2ed93a22089ea8e5190368a78e26aa80fc0b (diff) | |
download | px4-firmware-5842c2212334876979f329b9b6bceba9609d91af.tar.gz px4-firmware-5842c2212334876979f329b9b6bceba9609d91af.tar.bz2 px4-firmware-5842c2212334876979f329b9b6bceba9609d91af.zip |
Use GPS velocity in position estimator
5 files changed, 45 insertions, 13 deletions
diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c index 64031ee7b..390e1b720 100644 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.c +++ b/src/modules/position_estimator_inav/kalman_filter_inertial.c @@ -12,7 +12,7 @@ void kalman_filter_inertial_predict(float dt, float x[3]) { x[1] += x[2] * dt; } -void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { +void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) { float y[2]; // y = z - H x y[0] = z[0] - x[0]; @@ -25,3 +25,18 @@ void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool u } } } + +void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) { + float y[2]; + // y = z - H x + y[0] = z[0] - x[0]; + y[1] = z[1] - x[1]; + y[2] = z[2] - x[2]; + // x = x + K * y + for (int i = 0; i < 3; i++) { // Row + for (int j = 0; j < 3; j++) { // Column + if (use[j]) + x[i] += k[i][j] * y[j]; + } + } +} diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h index 3e5134c33..d34f58298 100644 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.h +++ b/src/modules/position_estimator_inav/kalman_filter_inertial.h @@ -9,4 +9,6 @@ void kalman_filter_inertial_predict(float dt, float x[3]); -void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]); +void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]); + +void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2b485f895..6ecdfe01d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -370,7 +370,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z); + kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z); } if (params.use_gps) { @@ -378,23 +378,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { kalman_filter_inertial_predict(dt, x_est); kalman_filter_inertial_predict(dt, y_est); /* prepare vectors for kalman filter correction */ - float x_meas[2]; // position, acceleration - float y_meas[2]; // position, acceleration - bool use_xy[2] = { false, false }; + float x_meas[3]; // position, velocity, acceleration + float y_meas[3]; // position, velocity, acceleration + bool use_xy[3] = { false, false, false }; if (gps_updated) { x_meas[0] = local_pos_gps[0]; y_meas[0] = local_pos_gps[1]; + x_meas[1] = gps.vel_n_m_s; + y_meas[1] = gps.vel_e_m_s; use_xy[0] = true; + use_xy[1] = true; } if (accelerometer_updated) { - x_meas[1] = accel_NED[0]; - y_meas[1] = accel_NED[1]; - use_xy[1] = true; + x_meas[2] = accel_NED[0]; + y_meas[2] = accel_NED[1]; + use_xy[2] = true; } - if (use_xy[0] || use_xy[1]) { + if (use_xy[0] || use_xy[2]) { /* correction */ - kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy); - kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy); } } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 8466bcd0a..20142b70c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -51,10 +51,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f); int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -68,10 +71,13 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_pos_00 = param_find("INAV_K_POS_00"); h->k_pos_01 = param_find("INAV_K_POS_01"); + h->k_pos_02 = param_find("INAV_K_POS_02"); h->k_pos_10 = param_find("INAV_K_POS_10"); h->k_pos_11 = param_find("INAV_K_POS_11"); + h->k_pos_12 = param_find("INAV_K_POS_12"); h->k_pos_20 = param_find("INAV_K_POS_20"); h->k_pos_21 = param_find("INAV_K_POS_21"); + h->k_pos_22 = param_find("INAV_K_POS_22"); return OK; } @@ -88,10 +94,13 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_pos_00, &(p->k_pos[0][0])); param_get(h->k_pos_01, &(p->k_pos[0][1])); + param_get(h->k_pos_02, &(p->k_pos[0][2])); param_get(h->k_pos_10, &(p->k_pos[1][0])); param_get(h->k_pos_11, &(p->k_pos[1][1])); + param_get(h->k_pos_12, &(p->k_pos[1][2])); param_get(h->k_pos_20, &(p->k_pos[2][0])); param_get(h->k_pos_21, &(p->k_pos[2][1])); + param_get(h->k_pos_22, &(p->k_pos[2][2])); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 8cdc2e10f..c0e0042b6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -43,7 +43,7 @@ struct position_estimator_inav_params { int use_gps; float k_alt[3][2]; - float k_pos[3][2]; + float k_pos[3][3]; }; struct position_estimator_inav_param_handles { @@ -58,10 +58,13 @@ struct position_estimator_inav_param_handles { param_t k_pos_00; param_t k_pos_01; + param_t k_pos_02; param_t k_pos_10; param_t k_pos_11; + param_t k_pos_12; param_t k_pos_20; param_t k_pos_21; + param_t k_pos_22; }; /** |