fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 06:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Paul Riseborough 2019-02-27 09:11:40 +11:00 committed by Beat Küng
parent 1079ae3402
commit 09da8e7dfc
2 changed files with 32 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/ekf2/ekf2_main.cpp
View File

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

18
src/modules/ekf2/ekf2_params.c
View File

@ -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.