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

yaw_reset: reduce minimum vector length to compute yaw error

Browse Source 
A value of 1m/s was too conservative and the EKF takes too much time to
trigger a yaw reset. The value can be safely reduced because the
threshold is computed using the accuracy of the measurement and estimate
before being limited by this value.
This commit is contained in:
bresch
2021-05-05 10:36:58 +02:00
committed by Paul Riseborough
parent aad48407c2
commit 29243ac5cb
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+2 -2
View File
@@ -617,8 +617,8 @@ void Ekf::controlGpsFusion()
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);
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());