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

ekf2: replace yaw failure detection criteria

Browse Source 
The existing logic using the angle between velocity vectors failed to
determine a yaw failure in practice because the state velocity is often
too small compared to its uncertainty to be used. In all the failures
reported, the yaw emergency estimator converged properly and yaw reset
would have fixed the issue.
A much simpler check using the yaw difference between the main EKF
and the emergency estimator is now used to tell if the vel/pos update
failure is most likely caused by a wrong heading.
This commit is contained in:
bresch
2022-01-18 16:55:45 +01:00
committed by Mathieu Bresciani
parent 446729566d
commit a2260e53da
3 changed files with 16 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
-26
View File
@@ -1150,32 +1150,6 @@ void Ekf::controlAuxVelFusion()
}
}
bool Ekf::isVelStateAlignedWithObs() const
{
/* Do sanity check to see if the innovation failures is likely caused by a yaw angle error
* by measuring the angle between the velocity estimate and the last velocity observation
* Only use those vectors if their norm if they are larger than 4 times their noise standard deviation
*/
const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared();
const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared();
const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f));
const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f));
if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) {
const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy());
const float cos_sq = sq(obs_dot_vel) / (vel_state_xy_norm_sq * vel_obs_xy_norm_sq);
if (cos_sq < sq(cosf(math::radians(25.f))) || obs_dot_vel < 0.f) {
// The angle between the observation and the velocity estimate is greater than 25 degrees
return false;
}
}
return true;
}
bool Ekf::hasHorizontalAidingTimedOut() const
{
return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)