From 22f1b784525622ac39c9db50675c3937c7aeecf0 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Fri, 17 Oct 2014 21:07:34 +0400 Subject: Adding vertical (Z) velocity to inav estimator --- .../position_estimator_inav/position_estimator_inav_main.c | 3 +++ .../position_estimator_inav/position_estimator_inav_params.c | 12 ++++++++++++ .../position_estimator_inav/position_estimator_inav_params.h | 2 ++ 3 files changed, 17 insertions(+) (limited to 'src') 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 81bbaa658..a1bfa2dfc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -846,6 +846,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; 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_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; @@ -876,6 +877,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ @@ -960,6 +962,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { 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 cc0fb4155..1a1b3d310 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -63,6 +63,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +/** + * Z velocity weight for GPS + * + * Weight (cutoff frequency) for GPS altitude velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); + /** * Z axis weight for vision * @@ -281,6 +292,7 @@ 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_gps_v = param_find("INAV_W_Z_GPS_V"); 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"); 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 d0a65e42e..51bbda412 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,6 +44,7 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_gps_v; float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; @@ -68,6 +69,7 @@ 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_gps_v; param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; -- cgit v1.2.3 From 0632d882bb734ac52ff5f7f5d1dc7930046e4600 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Wed, 21 Jan 2015 12:22:07 +0300 Subject: INAV_W_Z_GPS_V defaults to 0 --- src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') 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 1a1b3d310..b2771f1a6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); /** * Z axis weight for vision -- cgit v1.2.3