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: simplify logic for inhibited consider states and covariance matrix fixes #22597

Merged

dagar merged 10 commits into main from pr-ekf2_sensor_bias_consider_states

Jan 24, 2024

Member

dagar commented Jan 3, 2024 •

edited

Loading

#19142 (comment)

dagar requested a review from bresch

January 3, 2024 18:14

dagar assigned dagar and bresch

dagar added the EKF2 label

dagar force-pushed the pr-ekf2_sensor_bias_consider_states branch from a719f20 to 1b48dfe Compare

January 4, 2024 00:35

bresch reviewed

View reviewed changes

Member

bresch left a comment

Looks good. We need some testing now.

dagar commented

View reviewed changes

src/modules/ekf2/EKF/covariance.cpp Outdated Show resolved Hide resolved

dagar commented

View reviewed changes

src/modules/ekf2/EKF/covariance.cpp Outdated Show resolved Hide resolved

dagar commented

View reviewed changes

src/modules/ekf2/EKF/covariance.cpp Outdated Show resolved Hide resolved

dagar commented

View reviewed changes

src/modules/ekf2/EKF/covariance.cpp Outdated Show resolved Hide resolved

dagar commented

View reviewed changes

src/modules/ekf2/EKF/ekf.h Outdated Show resolved Hide resolved

dagar commented

View reviewed changes

src/modules/ekf2/EKF/covariance.cpp Outdated Show resolved Hide resolved

dagar force-pushed the pr-ekf2_sensor_bias_consider_states branch from f3c0877 to f111988 Compare

January 11, 2024 17:20

dagar changed the title ~~[WIP] ekf2 simplify logic for inhibited consider states~~ ekf2: simplify logic for inhibited consider states and covariance matrix fixes

dagar marked this pull request as ready for review

January 11, 2024 17:20

dagar force-pushed the pr-ekf2_sensor_bias_consider_states branch from f111988 to 3b98bb3 Compare

January 12, 2024 16:46


          ekf2: simplify logic for inhibited consider states and covariance mat…

acb0051

…rix fixes

 - removes conservative accel bias variance limiting
 - force symmetry is skipped after fusion of NED vel/pos (a direct measurement)
 - mag and wind covariances no longer continuously reset when stopped

Co-authored-by: bresch <brescianimathieu@gmail.com>

dagar force-pushed the pr-ekf2_sensor_bias_consider_states branch from 3b98bb3 to acb0051 Compare

January 12, 2024 20:20

dagar and others added 7 commits

January 16, 2024 10:29


          Merge remote-tracking branch 'px4/main' into pr-ekf2_sensor_bias_cons…

7aba710

…ider_states


          adjust tests

a6c5ae0


          max ratio limit increase to 1,000,000

53c27c5


          ekf2: do not clear covariance when stopping fusion

62a9898

When a state stops to be estimated it becomes a "consider state". Its
value and variance are frozen but its covariance with other states
continue to evolve normally.


          Merge remote-tracking branch 'px4/main' into pr-ekf2_sensor_bias_cons…

…ider_states


          test adjust and change indication update

fbfcef9


          Merge remote-tracking branch 'px4/main' into pr-ekf2_sensor_bias_cons…

dabb31b

…ider_states

dagar commented

View reviewed changes

src/modules/ekf2/EKF/airspeed_fusion.cpp Outdated

@@ @@ -60,7 +60,6 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) @@
               	if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.drag_ctrl == 0))) {
               		_control_status.flags.wind = false;
-              		P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.f);

Member Author

dagar Jan 22, 2024

Fully stopping wind might still make sense?

dagar requested a review from bresch

January 23, 2024 00:31

bresch reviewed

View reviewed changes

src/modules/ekf2/test/test_EKF_height_fusion.cpp

@@ @@ -182,7 +182,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) @@
               	EXPECT_NEAR(baro_status.bias, baro_initial + baro_increment, 1.3f);
               	const BiasEstimator::status &rng_status = _ekf->getRngHgtBiasEstimatorStatus();
-              	EXPECT_NEAR(rng_status.bias, 0.f, 1.f);
+              	EXPECT_NEAR(rng_status.bias, 0.f, 1.1f); // TODO: why?

Member

bresch Jan 24, 2024

Most likely a tiny change in convergence speed due to the lower limit on the accel bias change

bresch previously approved these changes

View reviewed changes


          Merge remote-tracking branch 'origin/main' into pr-ekf2_sensor_bias_c…

ea6f1ec

…onsider_states


          update change indicator

ab8601d

dagar dismissed bresch’s stale review via

ab8601d

January 24, 2024 19:08

dagar merged commit 51155f7 into main

6 of 89 checks passed

dagar deleted the pr-ekf2_sensor_bias_consider_states branch

January 24, 2024 19:14

jwwaite pushed a commit to AIVS-Inc/PX4-Autopilot that referenced this pull request


          ekf2: treat inhibited states as "consider states" and remove conserva…

1b3fa8f

…tive covariance matrix fixes (PX4#22597)

When a state stops to be estimated it becomes a "consider state". Its value and variance are frozen but its covariance with other states continue to evolve normally.

 - removes conservative accel bias variance limiting
 - force symmetry is skipped after fusion of NED vel/pos (a direct measurement)

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>

MaEtUgR mentioned this pull request

[Bug] MAVSDK test failing after EKF change, accelerometer simulation issues not learned anymore? #22792

Open

Peize-Liu pushed a commit to Peize-Liu/PX4-Autopilot that referenced this pull request


          ekf2: treat inhibited states as "consider states" and remove conserva…

6dde163

…tive covariance matrix fixes (PX4#22597)

When a state stops to be estimated it becomes a "consider state". Its value and variance are frozen but its covariance with other states continue to evolve normally.

 - removes conservative accel bias variance limiting
 - force symmetry is skipped after fusion of NED vel/pos (a direct measurement)

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>

timzarhansen pushed a commit to timzarhansen/PX4-Autopilot that referenced this pull request


          ekf2: treat inhibited states as "consider states" and remove conserva…

24d8a07

…tive covariance matrix fixes (PX4#22597)

When a state stops to be estimated it becomes a "consider state". Its value and variance are frozen but its covariance with other states continue to evolve normally.

 - removes conservative accel bias variance limiting
 - force symmetry is skipped after fusion of NED vel/pos (a direct measurement)

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

EKF2