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

yaw emergency: check angle between vel observation and estimate (#975)

Browse Source 
use thresholds based on state and obs variance

* [AUTO COMMIT] update change indication
This commit is contained in:
Mathieu Bresciani
2021-02-15 10:31:16 +01:00
committed by GitHub
parent ffab483504
commit d4258cc66d
3 changed files with 59 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+24 -15
View File
@@ -306,7 +306,6 @@ void Ekf::controlExternalVisionFusion()
// determine if we should use the velocity observations
if (_control_status.flags.ev_vel) {
Vector3f ev_vel_obs_var;
Vector2f ev_vel_innov_gates;
_last_vel_obs = getVisionVelocityInEkfFrame();
@@ -321,12 +320,12 @@ void Ekf::controlExternalVisionFusion()
}
}
ev_vel_obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
_last_vel_obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
ev_vel_innov_gates.setAll(fmaxf(_params.ev_vel_innov_gate, 1.0f));
fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,_last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
}
// determine if we should use the yaw observation
@@ -617,12 +616,22 @@ void Ekf::controlGpsFusion()
bool is_yaw_failure = false;
if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) {
// Do sanity check to see if the innovation failures is likely caused by a yaw angle error
// A 180 deg yaw error would (in the absence of other errors) generate a velocity innovation
// magnitude of twice the measured speed so allow up to 3 times to allow for additional errors.
const float hvel_obs_sq = sq(_last_vel_obs(0)) + sq(_last_vel_obs(1));
const float hvel_innov_sq = sq(_last_fail_hvel_innov(0)) + sq(_last_fail_hvel_innov(1));
if (hvel_innov_sq > 9.0f * hvel_obs_sq) {
is_yaw_failure = true;
// 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)), 1.f);
const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), 1.f);
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
is_yaw_failure = true;
}
}
}
@@ -669,7 +678,6 @@ void Ekf::controlGpsFusion()
Vector2f gps_vel_innov_gates; // [horizontal vertical]
Vector2f gps_pos_innov_gates; // [horizontal vertical]
Vector3f gps_vel_obs_var;
Vector3f gps_pos_obs_var;
// correct velocity for offset relative to IMU
@@ -697,8 +705,8 @@ void Ekf::controlGpsFusion()
gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit));
}
gps_vel_obs_var.setAll(sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)));
gps_vel_obs_var(2) = sq(1.5f) * gps_vel_obs_var(2);
_last_vel_obs_var.setAll(sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)));
_last_vel_obs_var(2) *= sq(1.5f);
// calculate innovations
_last_vel_obs = _gps_sample_delayed.vel;
@@ -710,8 +718,8 @@ void Ekf::controlGpsFusion()
gps_vel_innov_gates(0) = gps_vel_innov_gates(1) = fmaxf(_params.gps_vel_innov_gate, 1.0f);
// fuse GPS measurement
fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates,gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
}
@@ -1329,6 +1337,7 @@ void Ekf::controlAuxVelFusion()
_last_vel_obs = _auxvel_sample_delayed.vel;
_aux_vel_innov = _state.vel - _last_vel_obs;
_last_vel_obs_var = _aux_vel_innov_var;
fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
_aux_vel_innov_var, _aux_vel_test_ratio);
EKF/ekf.h
+1
View File
@@ -391,6 +391,7 @@ private:
Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance
Vector3f _last_vel_obs; ///< last velocity observation (m/s)
Vector3f _last_vel_obs_var; ///< last velocity observation variance (m/s)**2
Vector2f _last_fail_hvel_innov; ///< last failed horizontal velocity innovation (m/s)**2
float _vert_pos_innov_ratio; ///< vertical position innovation divided by estimated standard deviation of innovation
uint64_t _vert_pos_fuse_attempt_time_us; ///< last system time in usec vertical position measurement fuson was attempted