Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ekf2: Only reset to GNSS heading if necessary #23068

Merged
merged 1 commit into from
Apr 29, 2024
Merged

ekf2: Only reset to GNSS heading if necessary #23068

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
1 change: 0 additions & 1 deletion src/modules/ekf2/EKF/ekf.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -672,7 +672,6 @@ class Ekf final : public EstimatorInterface

# if defined(CONFIG_EKF2_GNSS_YAW)
estimator_aid_source1d_s _aid_src_gnss_yaw{};
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS

Expand Down
46 changes: 19 additions & 27 deletions src/modules/ekf2/EKF/gps_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -358,35 +358,14 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
&& !_gps_intermittent;

if (_control_status.flags.gps_yaw) {

if (continuing_conditions_passing) {

fuseGpsYaw(gps_sample.yaw_offset);

const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);

if (is_fusion_failing) {
if (_nb_gps_yaw_reset_available > 0) {
// Data seems good, attempt a reset
resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset);

if (_control_status.flags.in_air) {
_nb_gps_yaw_reset_available--;
}

} else if (starting_conditions_passing) {
// Data seems good, but previous reset did not fix the issue
// something else must be wrong, declare the sensor faulty and stop the fusion
_control_status.flags.gps_yaw_fault = true;
stopGpsYawFusion();

} else {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
stopGpsYawFusion();
}

// TODO: should we give a new reset credit when the fusion does not fail for some time?
stopGpsYawFusion();
}

} else {
Expand All @@ -397,14 +376,27 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
} else {
if (starting_conditions_passing) {
// Try to activate GPS yaw fusion
if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
ECL_INFO("starting GPS yaw fusion");
const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;

if (!_control_status.flags.in_air
|| !_control_status.flags.yaw_align
|| not_using_ne_aiding) {

_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
// Reset before starting the fusion
if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
_control_status.flags.gps_yaw = true;
_control_status.flags.yaw_align = true;
}

} else if (!_aid_src_gnss_yaw.innovation_rejected) {
// Do not force a reset but wait for the consistency check to pass
_control_status.flags.gps_yaw = true;
_control_status.flags.yaw_align = true;
fuseGpsYaw(gps_sample.yaw_offset);
}

_nb_gps_yaw_reset_available = 1;
if (_control_status.flags.gps_yaw) {
ECL_INFO("starting GPS yaw fusion");
}
}
}
Expand Down
22 changes: 11 additions & 11 deletions src/modules/ekf2/test/test_EKF_gps_yaw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -275,17 +275,23 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround)
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(8);

// THEN: the fusion should reset
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
// THEN: the fusion should stop, reset to mag
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);

// AND THEN: restart GNSS yaw fusion
_sensor_simulator.runSeconds(5);
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2);
EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f));
}

TEST_F(EkfGpsHeadingTest, yawJumpInAir)
{
// GIVEN: the GPS yaw fusion activated
float gps_heading = _ekf_wrapper.getYawAngle();
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator._gps.setYaw(gps_heading + math::radians(90.f));
_sensor_simulator.runSeconds(5);
_ekf->set_in_air_status(true);

Expand All @@ -295,20 +301,14 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir)
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(7.5);

// THEN: the fusion should reset
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);

// BUT WHEN: the measurement jumps a 2nd time
gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f));
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(7.5);
// THEN: the fusion should not reset as heading is still observable through GNSS vel/pos fusion
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);

// THEN: after a few seconds, the fusion should stop and
// the estimator doesn't fall back to mag fusion because it has
// been declared inconsistent with the filter states
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isMagHeadingConsistent());
//TODO: should we force a reset to mag if the GNSS yaw fusion was forced to stop?
EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
}

Expand Down