From 09da8e7dfcadeea574c855d583a9d20a64a68463 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 27 Feb 2019 09:11:40 +1100
Subject: [PATCH] ekf2: Split static pressure correction into separate RH ad LH
ellipsoids
---
src/modules/ekf2/ekf2_main.cpp | 24 +++++++++++++++++-------
src/modules/ekf2/ekf2_params.c | 18 +++++++++++++++---
2 files changed, 32 insertions(+), 10 deletions(-)
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index e847354d5a..c3ba1bd195 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 b2036ba222..9e4513031c 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.
--
GitLab