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

EKF: Only record successful sideslip fusion when states updated

Browse Source
This commit is contained in:
Paul Riseborough
2020-08-15 22:21:10 +10:00
committed by Paul Riseborough
parent 0fa7ef3517
commit 64ab2b087a
1 changed files with 1 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/sideslip_fusion.cpp
+1 -4
View File
@@ -69,7 +69,7 @@ void Ekf::fuseSideslip()
// perform fusion of assumed sideslip = 0 // perform fusion of assumed sideslip = 0
if (rel_wind_body.norm() > 7.0f) { if (rel_wind_body.norm() > 7.0f) {
float R_BETA = sq(_params.beta_noise); // observation noise variance const float R_BETA = sq(_params.beta_noise); // observation noise variance
// determine if we need the sideslip fusion to correct states other than wind // determine if we need the sideslip fusion to correct states other than wind
bool update_wind_only = !_is_wind_dead_reckoning; bool update_wind_only = !_is_wind_dead_reckoning;
@@ -203,9 +203,6 @@ void Ekf::fuseSideslip()
Kfusion(22) = HK45*HK52; Kfusion(22) = HK45*HK52;
Kfusion(23) = HK42*HK52; Kfusion(23) = HK42*HK52;
// synthetic sideslip measurement sample has passed check so record it
_time_last_beta_fuse = _time_last_imu;
// apply covariance correction via P_new = (I -K*H)*P // apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP // first calculate expression for KHP
// then calculate P - KHP // then calculate P - KHP