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

ekf2: don't use Vectors for height innovations (baro, rng, etc)

Browse Source
This commit is contained in:
Daniel Agar
2022-01-03 09:20:20 -05:00
parent eac92ec671
commit 35502c249d
6 changed files with 69 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+10 -11
View File
@@ -146,33 +146,32 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
}
}
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio)
bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var,
float &innov_var, float &test_ratio)
{
innov_var(2) = P(9, 9) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
_vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2));
innov_var = P(9, 9) + obs_var;
test_ratio = sq(innov) / (sq(innov_gate) * innov_var);
_vert_pos_innov_ratio = innov / sqrtf(innov_var);
_vert_pos_fuse_attempt_time_us = _time_last_imu;
bool innov_check_pass = test_ratio(1) <= 1.0f;
bool innov_check_pass = test_ratio <= 1.0f;
// if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
float innovation;
if (_fault_status.flags.bad_acc_vertical && !innov_check_pass) {
const float innov_limit = innov_gate(1) * sqrtf(innov_var(2));
innovation = math::constrain(innov(2), -innov_limit, innov_limit);
const float innov_limit = innov_gate * sqrtf(innov_var);
innovation = math::constrain(innov, -innov_limit, innov_limit);
innov_check_pass = true;
} else {
innovation = innov(2);
innovation = innov;
}
if (innov_check_pass) {
_time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_pos = false;
fuseVelPosHeight(innovation, innov_var(2), 5);
fuseVelPosHeight(innovation, innov_var, 5);
return true;