Adding vertical (Z) velocity to inav estimator

3 files changed, 17 insertions, 0 deletions

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
@@ -64,6 +64,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
  *
  * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset.
@@ -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;