Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

ekf2: inhibit accel bias dynamically when fake position/height fusion active

Browse Source 
- inhibit accel bias learning dynamically depending on the angle between an accelerometer and the vertical axis when fake position fusion is active
This commit is contained in:
Daniel Agar
2022-09-22 20:32:20 -04:00
committed by GitHub
parent 1806f0d2b4
commit 38d332a553
4 changed files with 748 additions and 735 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+14 -1
View File
@@ -149,7 +149,20 @@ void Ekf::predictCovariance()
for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) {
const unsigned index = stateIndex - 13;
const bool do_inhibit_axis = do_inhibit_all_axes || _imu_sample_delayed.delta_vel_clipping[index];
bool is_bias_observable = true;
if (_control_status.flags.vehicle_at_rest) {
is_bias_observable = true;
} else if (_control_status.flags.fake_hgt) {
is_bias_observable = false;
} else if (_control_status.flags.fake_pos) {
// when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector
is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
}
const bool do_inhibit_axis = do_inhibit_all_axes || _imu_sample_delayed.delta_vel_clipping[index] || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv
+385 -385
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf2/test/change_indication/iris_gps.csv
+347 -347
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf2/test/test_EKF_externalVision.cpp
+2 -2
View File
@@ -288,7 +288,7 @@ TEST_F(EkfExternalVisionTest, velocityFrameBody)
// THEN: As the drone is turned 90 degrees, velocity variance
// along local y axis is expected to be bigger
const Vector3f velVar_new = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_new(1) / velVar_new(0), 40.f, 15.f);
EXPECT_NEAR(velVar_new(1) / velVar_new(0), 70.f, 15.f);
const Vector3f vel_earth_est = _ekf->getVelocity();
EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f);
@@ -322,7 +322,7 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal)
// THEN: Independently on drones heading, velocity variance
// along local x axis is expected to be bigger
const Vector3f velVar_new = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_new(0) / velVar_new(1), 40.f, 15.f);
EXPECT_NEAR(velVar_new(0) / velVar_new(1), 70.f, 15.f);
const Vector3f vel_earth_est = _ekf->getVelocity();
EXPECT_NEAR(vel_earth_est(0), 1.0f, 0.1f);