ekf2: Split static pressure correction into separate RH ad LH ellipsoids

src/modules/ekf2/ekf2_main.cpp

@@ -485,8 +485,10 @@ private:
 		_K_pstatic_coef_xp,	///< static pressure position error coefficient along the positive X body axis
 		(ParamFloat<px4::params::EKF2_PCOEF_XN>)
 		_K_pstatic_coef_xn,	///< static pressure position error coefficient along the negative X body axis
-		(ParamFloat<px4::params::EKF2_PCOEF_Y>)
-		_K_pstatic_coef_y,	///< static pressure position error coefficient along the Y body axis
+		(ParamFloat<px4::params::EKF2_PCOEF_YP>)
+		_K_pstatic_coef_yp,	///< static pressure position error coefficient along the positive Y body axis
+		(ParamFloat<px4::params::EKF2_PCOEF_YN>)
+		_K_pstatic_coef_yn,	///< static pressure position error coefficient along the negativeY body axis
 		(ParamFloat<px4::params::EKF2_PCOEF_Z>)
 		_K_pstatic_coef_z,	///< static pressure position error coefficient along the Z body axis
 
@@ -914,12 +916,11 @@ void Ekf2::run()
 					_ekf.set_air_density(airdata.rho);
 
 					// calculate static pressure error = Pmeas - Ptruth
-					// model position error sensitivity as a body fixed ellipse with different scale in the positive and negtive X direction
-					const float max_airspeed_sq = _aspd_max.get() * _aspd_max.get();
-					float K_pstatic_coef_x;
-
+					// model position error sensitivity as a body fixed ellipse with a different scale in the positive and 
+					// negative X and Y directions
 					const Vector3f vel_body_wind = get_vel_body_wind();
 
+					float K_pstatic_coef_x;
 					if (vel_body_wind(0) >= 0.0f) {
 						K_pstatic_coef_x = _K_pstatic_coef_xp.get();
 
@@ -927,12 +928,21 @@ void Ekf2::run()
 						K_pstatic_coef_x = _K_pstatic_coef_xn.get();
 					}
 
+					float K_pstatic_coef_y;
+					if (vel_body_wind(1) >= 0.0f) {
+						K_pstatic_coef_y = _K_pstatic_coef_yp.get();
+
+					} else {
+						K_pstatic_coef_y = _K_pstatic_coef_yn.get();
+					}
+
+					const float max_airspeed_sq = _aspd_max.get() * _aspd_max.get();
 					const float x_v2 = fminf(vel_body_wind(0) * vel_body_wind(0), max_airspeed_sq);
 					const float y_v2 = fminf(vel_body_wind(1) * vel_body_wind(1), max_airspeed_sq);
 					const float z_v2 = fminf(vel_body_wind(2) * vel_body_wind(2), max_airspeed_sq);
 
 					const float pstatic_err = 0.5f * airdata.rho *
-								  (K_pstatic_coef_x * x_v2) + (_K_pstatic_coef_y.get() * y_v2) + (_K_pstatic_coef_z.get() * z_v2);
+								  (K_pstatic_coef_x * x_v2) + (K_pstatic_coef_y * y_v2) + (_K_pstatic_coef_z.get() * z_v2);
 
 					// correct baro measurement using pressure error estimate and assuming sea level gravity
 					balt_data_avg += pstatic_err / (airdata.rho * CONSTANTS_ONE_G);

src/modules/ekf2/ekf2_params.c

@@ -1203,15 +1203,27 @@ PARAM_DEFINE_FLOAT(EKF2_PCOEF_XN, 0.0f);
 
 /**
  * Pressure position error coefficient for the Y axis.
- * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the Y body axis.
- * If the baro height estimate rises during sideways flight, then this will be a negative number.
+ * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the positive Y (RH) body axis. 
+ * If the baro height estimate rises during sideways flight to the right, then this will be a negative number.
  *
  * @group EKF2
  * @min -0.5
  * @max 0.5
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(EKF2_PCOEF_Y, 0.0f);
+PARAM_DEFINE_FLOAT(EKF2_PCOEF_YP, 0.0f);
+
+/**
+ * Pressure position error coefficient for the Y axis.
+ * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the negative Y (LH) body axis.
+ * If the baro height estimate rises during sideways flight to the left, then this will be a negative number.
+ *
+ * @group EKF2
+ * @min -0.5
+ * @max 0.5
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_PCOEF_YN, 0.0f);
 
 /**
  * Static pressure position error coefficient for the Z axis.