diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index e847354d5a66b9411ff527537cf1b5eeb8c8362c..c3ba1bd1958e38b0fac00cfa7bf689507017c6b3 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/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); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index b2036ba22297881754b79ebd7843195085ba8256..9e4513031c1269c3187c0f52385c5e5d5877cba2 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/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.