Adding vertical (Z) velocity to inav estimator

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) {

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");

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;