diff options
3 files changed, 74 insertions, 22 deletions
diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 071ec6ad4..2b485f895 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -261,6 +261,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; + bool gps_updated = false; + float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; + int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ @@ -322,14 +325,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), - vehicle_gps_position_sub, &gps); - static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); /* Project gps lat lon (Geographic coordinate system) to plane */ map_projection_project(((double) (gps.lat)) * 1e-7, ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + gps_updated = true; pos.valid = gps.fix_type >= 3; gps_updates++; } @@ -352,7 +354,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } } accel_NED[2] += CONSTANTS_ONE_G; - /* kalman filter prediction */ + + /* kalman filter for altitude */ kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ float z_meas[2]; // position, acceleration @@ -367,8 +370,32 @@ 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, - use_z); + kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z); + } + + if (params.use_gps) { + /* kalman filter for position */ + 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 }; + if (gps_updated) { + x_meas[0] = local_pos_gps[0]; + y_meas[0] = local_pos_gps[1]; + use_xy[0] = true; + } + if (accelerometer_updated) { + x_meas[1] = accel_NED[0]; + y_meas[1] = accel_NED[1]; + use_xy[1] = true; + } + if (use_xy[0] || use_xy[1]) { + /* 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); + } } } if (verbose_mode) { @@ -390,10 +417,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = 0.0f; - pos.vx = 0.0f; - pos.y = 0.0f; - pos.vy = 0.0f; + pos.x = x_est[0]; + pos.vx = x_est[1]; + pos.y = y_est[0]; + pos.vy = y_est[1]; pos.z = z_est[0]; pos.vz = z_est[1]; pos.timestamp = hrt_absolute_time(); @@ -402,9 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { && (isfinite(pos.vy)) && (isfinite(pos.z)) && (isfinite(pos.vz))) { - orb_publish(ORB_ID( - vehicle_local_position), vehicle_local_position_pub, - &pos); + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); } } } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index bb2b01481..8466bcd0a 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -40,8 +40,6 @@ #include "position_estimator_inav_params.h" -/* Kalman Filter covariances */ -/* gps measurement noise standard deviation */ PARAM_DEFINE_INT32(INAV_USE_GPS, 0); PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); @@ -51,6 +49,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); 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_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); + int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -61,18 +66,32 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); + h->k_pos_00 = param_find("INAV_K_POS_00"); + h->k_pos_01 = param_find("INAV_K_POS_01"); + h->k_pos_10 = param_find("INAV_K_POS_10"); + h->k_pos_11 = param_find("INAV_K_POS_11"); + h->k_pos_20 = param_find("INAV_K_POS_20"); + h->k_pos_21 = param_find("INAV_K_POS_21"); + return OK; } int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { param_get(h->use_gps, &(p->use_gps)); - param_get(h->k_alt_00, &(p->k[0][0])); - param_get(h->k_alt_01, &(p->k[0][1])); - param_get(h->k_alt_10, &(p->k[1][0])); - param_get(h->k_alt_11, &(p->k[1][1])); - param_get(h->k_alt_20, &(p->k[2][0])); - param_get(h->k_alt_21, &(p->k[2][1])); + param_get(h->k_alt_00, &(p->k_alt[0][0])); + param_get(h->k_alt_01, &(p->k_alt[0][1])); + param_get(h->k_alt_10, &(p->k_alt[1][0])); + param_get(h->k_alt_11, &(p->k_alt[1][1])); + param_get(h->k_alt_20, &(p->k_alt[2][0])); + param_get(h->k_alt_21, &(p->k_alt[2][1])); + + 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_10, &(p->k_pos[1][0])); + param_get(h->k_pos_11, &(p->k_pos[1][1])); + param_get(h->k_pos_20, &(p->k_pos[2][0])); + param_get(h->k_pos_21, &(p->k_pos[2][1])); return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index eaa1bbc95..8cdc2e10f 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -42,7 +42,8 @@ struct position_estimator_inav_params { int use_gps; - float k[3][2]; + float k_alt[3][2]; + float k_pos[3][2]; }; struct position_estimator_inav_param_handles { @@ -54,6 +55,13 @@ struct position_estimator_inav_param_handles { param_t k_alt_11; param_t k_alt_20; param_t k_alt_21; + + param_t k_pos_00; + param_t k_pos_01; + param_t k_pos_10; + param_t k_pos_11; + param_t k_pos_20; + param_t k_pos_21; }; /** |