From 5842c2212334876979f329b9b6bceba9609d91af Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 May 2013 19:47:38 +0400 Subject: Use GPS velocity in position estimator --- .../kalman_filter_inertial.c | 17 +++++++++++++++- .../kalman_filter_inertial.h | 4 +++- .../position_estimator_inav_main.c | 23 ++++++++++++---------- .../position_estimator_inav_params.c | 9 +++++++++ .../position_estimator_inav_params.h | 5 ++++- 5 files changed, 45 insertions(+), 13 deletions(-) (limited to 'src/modules/position_estimator_inav') 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; }; /** -- cgit v1.2.3