EKF: Fix bug preventing wind relative dead reckoning

This commit is contained in:
Paul Riseborough
2018-04-21 13:04:04 -04:00
committed by Daniel Agar
parent 9747dc778d
commit ae069b12d6
4 changed files with 5 additions and 3 deletions
+1 -1
View File
@@ -73,7 +73,7 @@ void Ekf::fuseAirspeed()
// Perform fusion of True Airspeed measurement
if (v_tas_pred > 1.0f) {
// determine if we need the sideslip fusion to correct states other than wind
bool update_wind_only = !_is_dead_reckoning;
bool update_wind_only = !_is_wind_dead_reckoning;
// Calculate the observation jacobian
// intermediate variable from algebraic optimisation
+1
View File
@@ -1309,6 +1309,7 @@ void Ekf::update_deadreckoning_status()
bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max);
bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max);
_is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
_is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
// record the time we start inertial dead reckoning
+2 -1
View File
@@ -452,8 +452,9 @@ protected:
float _drag_test_ratio[2] {}; // drag innovation cinsistency check ratio
innovation_fault_status_u _innov_check_fail_status{};
bool _is_dead_reckoning{false}; // true if we are no longer fusing measurements that constrain horizontal velocity drift
bool _is_dead_reckoning{false}; // true if we are no longer fusing measurements that constrain horizontal velocity drift
bool _deadreckon_time_exceeded{false}; // true if the horizontal nav solution has been deadreckoning for too long and is invalid
bool _is_wind_dead_reckoning{false}; // true if we are navigating reliant on wind relative measurements
// IMU vibration monitoring
Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement
File diff suppressed because one or more lines are too long