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

ekf: use float instead of Vector2f for innov gate in pos/vel fusion

Browse Source 
We always use the same innovation gate for X and Y anyway
This commit is contained in:
bresch
2022-02-02 17:43:25 -05:00
committed by Daniel Agar
parent 5fb0084524
commit 3077f27821
5 changed files with 24 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+5 -7
View File
@@ -243,7 +243,6 @@ void Ekf::controlExternalVisionFusion()
}
Vector3f ev_pos_obs_var;
Vector2f ev_pos_innov_gates;
// correct position and height for offset relative to IMU
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
@@ -307,9 +306,9 @@ void Ekf::controlExternalVisionFusion()
}
// innovation gate size
ev_pos_innov_gates(0) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f);
fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gate, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
}
// determine if we should use the velocity observations
@@ -332,10 +331,9 @@ void Ekf::controlExternalVisionFusion()
const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
const Vector2f ev_vel_innov_gates{innov_gate, innov_gate};
fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
fuseHorizontalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
fuseVerticalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
}
// determine if we should use the yaw observation
@@ -1073,7 +1071,7 @@ void Ekf::controlAuxVelFusion()
if (isHorizontalAidingActive()) {
const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate);
const float aux_vel_innov_gate = fmaxf(_params.auxvel_gate, 1.f);
_aux_vel_innov = _state.vel - auxvel_sample_delayed.vel;