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

ekf2: fusion helpers return success/fail and set pos/vel update timestamps centrally (if healthy)

Browse Source
This commit is contained in:
Daniel Agar
2022-03-08 11:16:35 -05:00
parent c4bc062714
commit c10ea97967
6 changed files with 105 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+76 -39
View File
@@ -47,7 +47,6 @@
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(4, 4) + obs_var(0);
innov_var(1) = P(5, 5) + obs_var(1);
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate) * innov_var(0)),
@@ -56,13 +55,12 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate,
const bool innov_check_pass = (test_ratio(0) <= 1.0f);
if (innov_check_pass) {
_time_last_hor_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_hor_vel = false;
fuseVelPosHeight(innov(0), innov_var(0), 0);
fuseVelPosHeight(innov(1), innov_var(1), 1);
bool fuse_vx = fuseVelPosHeight(innov(0), innov_var(0), 0);
bool fuse_vy = fuseVelPosHeight(innov(1), innov_var(1), 1);
return true;
return fuse_vx && fuse_vy;
} else {
_innov_check_fail_status.flags.reject_hor_vel = true;
@@ -73,7 +71,6 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate,
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(2) = P(6, 6) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate) * innov_var(2));
_vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2));
@@ -94,12 +91,9 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, co
}
if (innov_check_pass) {
_time_last_ver_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_vel = false;
fuseVelPosHeight(innovation, innov_var(2), 2);
return true;
return fuseVelPosHeight(innovation, innov_var(2), 2);
} else {
_innov_check_fail_status.flags.reject_ver_vel = true;
@@ -124,19 +118,12 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate,
return false;
}
if (!_fuse_hpos_as_odom) {
_time_last_hor_pos_fuse = _time_last_imu;
} else {
_time_last_delpos_fuse = _time_last_imu;
}
_innov_check_fail_status.flags.reject_hor_pos = false;
fuseVelPosHeight(innov(0), innov_var(0), 3);
fuseVelPosHeight(innov(1), innov_var(1), 4);
bool fuse_x = fuseVelPosHeight(innov(0), innov_var(0), 3);
bool fuse_y = fuseVelPosHeight(innov(1), innov_var(1), 4);
return true;
return fuse_x && fuse_y;
} else {
_innov_check_fail_status.flags.reject_hor_pos = true;
@@ -167,11 +154,9 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const
}
if (innov_check_pass) {
_time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_pos = false;
fuseVelPosHeight(innovation, innov_var, 5);
return true;
return fuseVelPosHeight(innovation, innov_var, 5);
} else {
_innov_check_fail_status.flags.reject_ver_pos = true;
@@ -180,9 +165,8 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const
}
// Helper function that fuses a single velocity or position measurement
void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
{
Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
@@ -212,7 +196,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
}
}
setVelPosFaultStatus(obs_index, !healthy);
setVelPosStatus(obs_index, healthy);
if (healthy) {
// apply the covariance corrections
@@ -222,27 +206,80 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
// apply the state corrections
fuse(Kfusion, innov);
return true;
}
return false;
}
void Ekf::setVelPosFaultStatus(const int index, const bool status)
void Ekf::setVelPosStatus(const int index, const bool healthy)
{
if (index == 0) {
_fault_status.flags.bad_vel_N = status;
switch (index) {
case 0:
if (healthy) {
_fault_status.flags.bad_vel_N = false;
_time_last_hor_vel_fuse = _time_last_imu;
} else if (index == 1) {
_fault_status.flags.bad_vel_E = status;
} else {
_fault_status.flags.bad_vel_N = true;
}
} else if (index == 2) {
_fault_status.flags.bad_vel_D = status;
break;
} else if (index == 3) {
_fault_status.flags.bad_pos_N = status;
case 1:
if (healthy) {
_fault_status.flags.bad_vel_E = false;
_time_last_hor_vel_fuse = _time_last_imu;
} else if (index == 4) {
_fault_status.flags.bad_pos_E = status;
} else {
_fault_status.flags.bad_vel_E = true;
}
} else if (index == 5) {
_fault_status.flags.bad_pos_D = status;
break;
case 2:
if (healthy) {
_fault_status.flags.bad_vel_D = false;
_time_last_ver_vel_fuse = _time_last_imu;
} else {
_fault_status.flags.bad_vel_D = true;
}
break;
case 3:
if (healthy) {
_fault_status.flags.bad_pos_N = false;
_time_last_hor_pos_fuse = _time_last_imu;
} else {
_fault_status.flags.bad_pos_N = true;
}
break;
case 4:
if (healthy) {
_fault_status.flags.bad_pos_E = false;
_time_last_hor_pos_fuse = _time_last_imu;
} else {
_fault_status.flags.bad_pos_E = true;
}
break;
case 5:
if (healthy) {
_fault_status.flags.bad_pos_D = false;
_time_last_hgt_fuse = _time_last_imu;
} else {
_fault_status.flags.bad_pos_D = true;
}
break;
}
}