Skip to content

Commit

Permalink
ekf2: fixed calculation of static pressure error
Browse files Browse the repository at this point in the history
Signed-off-by: RomanBapst <bapstroman@gmail.com>
  • Loading branch information
RomanBapst authored and bresch committed Jun 27, 2019
1 parent 1466d11 commit 2c74216
Showing 1 changed file with 2 additions and 2 deletions.
4 changes: 2 additions & 2 deletions src/modules/ekf2/ekf2_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -915,8 +915,8 @@ void Ekf2::run()
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 * y_v2) + (_param_ekf2_pcoef_z.get() * z_v2);
const float pstatic_err = 0.5f * airdata.rho * (
K_pstatic_coef_x * x_v2 + K_pstatic_coef_y * y_v2 + _param_ekf2_pcoef_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);
Expand Down

0 comments on commit 2c74216

Please sign in to comment.