fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/ekf2/EKF/control.cpp
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)

2
src/modules/ekf2/EKF/ekf.h
View File

@ -822,7 +822,7 @@ private:
void controlGpsFusion();
bool shouldResetGpsFusion() const;
bool hasHorizontalAidingTimedOut() const;
bool isVelStateAlignedWithObs() const;
bool isYawFailure() const;
void processYawEstimatorResetRequest();
void processVelPosResetRequest();

17
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -71,14 +71,13 @@ void Ekf::controlGpsFusion()
fuseGpsVelPos();
if (shouldResetGpsFusion()){
const bool is_yaw_failure = !isVelStateAlignedWithObs();
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
/* A reset is not performed when getting GPS back after a significant period of no data
* because the timeout could have been caused by bad GPS.
* The total number of resets allowed per boot cycle is limited.
*/
if (is_yaw_failure
if (isYawFailure()
&& _control_status.flags.in_air
&& !was_gps_signal_lost
&& _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) {
@ -188,6 +187,20 @@ bool Ekf::shouldResetGpsFusion() const
return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
}
bool Ekf::isYawFailure() const
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
? getEuler321Yaw(_R_to_earth)
: getEuler312Yaw(_R_to_earth);
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
return fabsf(yaw_error) > math::radians(25.f);
}
void Ekf::processYawEstimatorResetRequest()
{
/* The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.