dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

GNSS yaw: use NIS sequence to detect bias in state 

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-02 14:37:57 +02:00 committed by Mathieu Bresciani
parent 3fe04a91f6
commit 4ebfbc6eab
3 changed files with 17 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -328,6 +328,7 @@ protected:
// innovation consistency check monitoring ratios
float _yaw_test_ratio{}; // yaw innovation consistency check ratio
AlphaFilter<float>_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
Vector3f _mag_test_ratio{}; // magnetometer XYZ innovation consistency check ratios
Vector2f _gps_vel_test_ratio{}; // GPS velocity innovation consistency check ratios
Vector2f _gps_pos_test_ratio{}; // GPS position innovation consistency check ratios

25
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
View File

@ -151,23 +151,23 @@ void Ekf::fuseGpsYaw()
if (_yaw_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_yaw = true;
// if we are in air we don't want to fuse the measurement
// we allow to use it when on the ground because the large innovation could be caused
// by interference or a large initial gyro bias
if (_control_status.flags.in_air) {
return;
} else {
// constrain the innovation to the maximum set by the gate
const float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var));
_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
}
return;
} else {
_innov_check_fail_status.flags.reject_yaw = false;
}
_yaw_signed_test_ratio_lpf.update(matrix::sign(_heading_innov) * _yaw_test_ratio);
if (!_control_status.flags.in_air
&& fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f) {
// A constant large signed test ratio is a sign of wrong gyro bias
// Reset the yaw gyro variance to converge faster and avoid
// being stuck on a previous bad estimate
resetZDeltaAngBiasCov();
}
// calculate observation jacobian
// Observation jacobian and Kalman gain vectors
SparseVector24f<0,1,2,3> Hfusion;
@ -213,6 +213,7 @@ bool Ekf::resetYawToGps()
resetQuatStateYaw(measured_yaw, yaw_variance, true);
_time_last_gps_yaw_fuse = _time_last_imu;
_yaw_signed_test_ratio_lpf.reset(0.f);
return true;
}

5
src/modules/ekf2/test/test_EKF_gps_yaw.cpp
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)