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

ekf2: estimator aid source status (starting with fake position)

Browse Source
This commit is contained in:
Daniel Agar
2022-05-23 16:21:49 -04:00
parent 79a34b5aed
commit 1ae467e9cd
13 changed files with 287 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+2 -7
View File
@@ -102,7 +102,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, co
}
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio, bool inhibit_gate)
Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(7, 7) + obs_var(0);
@@ -112,12 +112,7 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate,
const bool innov_check_pass = test_ratio(0) <= 1.0f;
if (innov_check_pass || inhibit_gate) {
if (inhibit_gate && test_ratio(0) > sq(100.0f / innov_gate)) {
// always protect against extreme values that could result in a NaN
return false;
}
if (innov_check_pass) {
_innov_check_fail_status.flags.reject_hor_pos = false;
bool fuse_x = fuseVelPosHeight(innov(0), innov_var(0), 3);