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

GNSS yaw: use NIS sequence to detect bias in state

Browse Source 
A constant large value in the (signed) normalized innovation test ratio is a sign
of bias in the state estimate. This metric can be used to trigger a
covariance boost or reset
This commit is contained in:
bresch
2021-08-05 11:10:02 +02:00
committed by Mathieu Bresciani
parent 3fe04a91f6
commit 4ebfbc6eab
3 changed files with 17 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_gps_yaw.cpp
+3 -2
View File
@@ -141,7 +141,7 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
// AND WHEN: the the measurement changes
gps_heading += math::radians(2.f);
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(10);
_sensor_simulator.runSeconds(20);
// THEN: the estimate slowly converges to the new measurement
// Note that the process is slow, because the gyro did not detect any motion
@@ -225,11 +225,12 @@ TEST_F(EkfGpsHeadingTest, yaw_jump_on_ground)
const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(45.f));
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(2);
_sensor_simulator.runSeconds(8);
// THEN: the fusion should reset
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f));
}
TEST_F(EkfGpsHeadingTest, yaw_jump_in_air)