ekf2: separate mag and mag heading control logic #21212

dagar · 2023-02-28T22:09:33Z

This is built on standalone mag fusion. #20933

requires ekf2: add and share centralized method to clear inhibited state Kalman gains #21196

This ended up being a huge overhaul of ekf2 mag control logic, and needs a very thorough review of the full logic, both how it actually works and original intent.

dagar · 2023-05-30T18:15:56Z

~~#21305 could be reviewed and merged separately as an incremental piece.~~

dagar · 2023-07-26T14:50:41Z

So far so good in SITL. https://logs.px4.io/plot_app?log=59b15e06-54e2-4c6a-b6d9-d4c71f278435

src/modules/ekf2/EKF/estimator_interface.cpp

src/modules/ekf2/EKF/gps_checks.cpp

bresch · 2023-07-28T11:52:46Z

src/modules/ekf2/EKF/mag_3d_control.cpp

+	// before they are used to constrain heading drift
+	_control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
+				       && _control_status.flags.mag
+				       && _control_status.flags.mag_aligned_in_flight && isTimedOut(_flt_mag_align_start_time, (uint64_t)5e6)


Do we really to wait for a defined amount of time if we already check if the mag bias variance is low enough?

I assumed not, but these are the details I wanted us to review and possibly remove (conservatively).

I removed the magic timing check, but perhaps we should review the magic "variance good" threshold.

bresch · 2023-07-28T11:58:32Z

src/modules/ekf2/EKF/mag_3d_control.cpp

+	// then the declination must be fused as an observation to prevent long term heading drift
+	// fusing declination when gps aiding is available is optional, but recommended to prevent
+	// problem if the vehicle is static for extended periods of time
+	const bool mag_decl_user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL);


maybe we should also remove the option to force the fusion and only do it if there is no GNSS aiding

It's not clear to me why someone would want/need to have control over this.

Me neither, and I don't think anyone is actually changing this parameter, I guess it was introduced for testing purposes.

bresch · 2023-07-28T12:07:12Z

src/modules/ekf2/EKF/mag_3d_control.cpp

+
+	if (_control_status.flags.mag) {
+		aid_src.timestamp_sample = mag_sample.time_us;
+		aid_src.fusion_enabled = true;


but if the continuing_conditions_passing == false, do we still want to have fusion_enabled = true?

Yes that's not quite right, we need to revamp the aid_src population so it's correct and useful on start/reset/stop cycles.

For now I'll move setting fusion_enabled to the end.

is that one still needed then?

Yes for now because of the check in fuseYaw(), but like I said needs a bit of a review/revamp overall.

bresch · 2023-07-28T12:11:31Z

src/modules/ekf2/EKF/mag_3d_control.cpp

+				} 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.mag_fault = true;


yes, we could make it faulty here and add a logic to clear the fault if we see that he innovation is continuously low

bresch · 2023-07-28T12:20:04Z

src/modules/ekf2/EKF/mag_3d_control.cpp

+
+	resetEstimatorAidStatus(aid_src);
+
+	const bool wmm_updated = (_wmm_gps_time_last_set > aid_src.time_last_fuse);


This is a bit misleading: the wmm is constantly updated, but we only need to do a reset when the wmm is set for the first time.

Actually it's only updated twice, first when we get a 2d fix with eph < 1000, then again when the NED origin is initialised.

https://github.com/PX4/PX4-Autopilot/blob/444e5d2d4abbb0adaabf1ad3cddaa2f9d2f9ee2b/src/modules/ekf2/EKF/gps_checks.cpp#L63C30-L63C53

It would probably be better to continue updating it periodically just in case anyone wants to fly around the world.

Right, we should update it periodically (but only trigger a reset after 2d fix and NED init)

I changed it to check for WMM changes periodically, but only update if the declination or inclination changed by more than a degree (better arbitrary threshold?).

src/modules/ekf2/EKF/mag_control.cpp

src/modules/ekf2/EKF2.cpp

…t_and_refactor

- periodically check for WMM changes - cleanup mag declination limiting logic and remove mag_3d arbitrary 5s requirement

src/modules/ekf2/EKF/ekf.h

bresch · 2023-07-31T08:46:00Z

src/modules/ekf2/EKF/mag_3d_control.cpp

+	// then the declination must be fused as an observation to prevent long term heading drift
+	// fusing declination when gps aiding is available is optional, but recommended to prevent
+	// problem if the vehicle is static for extended periods of time
+	const bool mag_decl_user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL);


Me neither, and I don't think anyone is actually changing this parameter, I guess it was introduced for testing purposes.

src/modules/ekf2/EKF/mag_control.cpp

bresch · 2023-07-31T08:54:41Z

src/modules/ekf2/EKF/mag_3d_control.cpp

+
+	if (_control_status.flags.mag) {
+		aid_src.timestamp_sample = mag_sample.time_us;
+		aid_src.fusion_enabled = true;


is that one still needed then?

bresch · 2023-07-31T09:03:44Z

src/modules/ekf2/EKF/mag_control.cpp

-		case MagFuseType::AUTO:
-			selectMagAuto();
-			break;
+		if (!_control_status.flags.tilt_align && !_control_status.flags.yaw_align && starting_conditions_passing) {


I know that it was already there before, but why do we want to reset the yaw if tilt is not aligned? What's the benefit?
During leveling, we can just fuse a constant position and wait for it to complete before doing the yaw alignment.

I actually dropped it initially, but kept it for now due to unit test failures (initializeWithTilt, etc). Do you want to take a closer look? Feel free to change the branch directly.

src/modules/ekf2/EKF/mag_control.cpp

bresch · 2023-07-31T09:20:12Z

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -75,7 +75,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 		_fault_status.flags.bad_mag_x = true;

 		// we need to re-initialise covariances and abort this fusion step
-		resetMagRelatedCovariances();
+		if (update_all_states) {
+			resetQuatCov();


Side note: I'm actually wondering what's happening if we reset the quat cov like that in air (tilt cov are also reset)

…t_and_refactor

dagar · 2023-07-31T15:20:19Z

@bresch without yaw reset during tilt alignment.

dagar · 2023-07-31T16:42:28Z

I removed the yaw reset during tilt alignment and did a pass updating EkfInitializationTest a bit more intelligently (we weren't even checking for tilt_align to complete). a81e115

The non-level initialization tests need a closer look.

- avoid multiple re-initializations - fuse zero heading innovation to stabilize the yaw during leveling

bresch · 2023-08-02T13:05:40Z

src/modules/ekf2/test/test_EKF_initialization.cpp

 	{
-		// TODO: precision = 0.0002f is only required for the pitch90 test to pass


mystery solved!

bresch · 2023-08-03T12:09:19Z

src/modules/ekf2/EKF/mag_3d_control.cpp

+
+		if (continuing_conditions_passing && _control_status.flags.yaw_align) {
+
+			if (mag_sample.reset || checkHaglYawResetReq()) {


so no more yaw reset at 1.5m if we're in mag heading fusion mode?

To be honest I've mainly been thinking of mag type AUTO, but I can throw this into mag heading fusion as well to preserve dedicated mag type heading configuration.

bresch · 2023-08-03T13:32:32Z

src/modules/ekf2/EKF/mag_3d_control.cpp

+	_control_status.flags.mag_dec = (_control_status.flags.mag && (not_using_ne_aiding || mag_decl_user_selected));
+
+	if (_control_status.flags.mag) {
+		aid_src.timestamp_sample = mag_sample.time_us;


Setting the timestamp here prevents the publication of the aid source if inactive. Didn't we want to publish it regardless of this?

Yes I'd like to, but in this particular case most of it is computed and populated within fuseMag(). We should do a refactor later with these things in mind, this overlaps with other comments about aid_src_status usage.

bresch

Reviewed again and tested in SITL, I think it's in a good shape now.

dagar · 2023-08-04T14:38:17Z

Thanks @bresch

- split mag_3d into new standalone mag fusion and mag fusion allowed to update all states (full mag_3d) - new dedicated control logic for mag/mag_3d fusion and standalone mag heading fusion - if WMM available use for mag_I and mag_B init - mag states reset if external yaw reset (yaw estimator, GPS yaw, etc) - mag reset if declination changed (eliminate _mag_yaw_reset_req) - mag fusion (but not mag_hdg or mag_3d) can be active during gps_yaw or ev_yaw (if yaw aligned north) Co-authored-by: bresch <brescianimathieu@gmail.com>

dagar added Admin: Enhancement (improvement) 💡 EKF2 labels Feb 28, 2023

dagar changed the base branch from main to pr-ekf2_control_status_mag March 1, 2023 15:49

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch from f1d5ab7 to f2c503a Compare March 1, 2023 15:50

dagar force-pushed the pr-ekf2_control_status_mag branch from 93e3c52 to fceda82 Compare March 6, 2023 16:00

dagar changed the base branch from pr-ekf2_control_status_mag to main March 9, 2023 15:36

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch from f2c503a to b0559f3 Compare March 9, 2023 15:44

dagar requested a review from bresch March 13, 2023 14:01

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch from b0559f3 to 3ea66d0 Compare March 13, 2023 14:01

dagar mentioned this pull request Mar 13, 2023

ekf2: mag control cleanup mag reset #21305

Merged

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch from 24f708e to d091c45 Compare July 24, 2023 16:06

dagar assigned dagar and bresch Jul 24, 2023

dagar mentioned this pull request Jul 24, 2023

ekf2: mag fusion only touch mag states unless allowed to update all states #21878

Closed

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch 2 times, most recently from be5f0ea to 3d88bea Compare July 25, 2023 20:52

dagar changed the title ~~[WIP] ekf2: separate mag and mag heading control logic~~ ekf2: separate mag and mag heading control logic Jul 25, 2023

dagar marked this pull request as ready for review July 25, 2023 21:13

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch 5 times, most recently from ba1ef02 to c2c78ba Compare July 26, 2023 14:09

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch 5 times, most recently from ff45e9a to a978a42 Compare July 26, 2023 16:54

bresch requested changes Jul 28, 2023

View reviewed changes

dagar added 2 commits July 28, 2023 09:28

revert max_var_ratio for mag bias save

d5851cf

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

be0d5d7

…t_and_refactor

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch 2 times, most recently from 4e9e0d3 to b744440 Compare July 28, 2023 19:07

address code review

6eb09a8

- periodically check for WMM changes - cleanup mag declination limiting logic and remove mag_3d arbitrary 5s requirement

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch from b744440 to 6eb09a8 Compare July 28, 2023 19:59

bresch requested changes Jul 31, 2023

View reviewed changes

dagar added 2 commits July 31, 2023 10:55

code review, remove duplicate checks

e45a7c6

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

bfb76e3

…t_and_refactor

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch from 6310d45 to 4f2f7e7 Compare July 31, 2023 16:29

WIP: mag_control remove yaw reset during tilt alignment

a81e115

dagar force-pushed the pr-ekf2_mag_control_split_and_refactor branch from 4f2f7e7 to a81e115 Compare July 31, 2023 16:39

ekf: fix tilt initialization

6a2568f

- avoid multiple re-initializations - fuse zero heading innovation to stabilize the yaw during leveling

bresch force-pushed the pr-ekf2_mag_control_split_and_refactor branch from 2238b34 to 6a2568f Compare August 2, 2023 13:03

bresch reviewed Aug 2, 2023

View reviewed changes

bresch and others added 3 commits August 2, 2023 13:38

[AUTO COMMIT] update change indication

a76a18e

gps rate limit WMM update checks

dfe67b8

fix typo

29db871

bresch reviewed Aug 3, 2023

View reviewed changes

dagar and others added 2 commits August 3, 2023 11:24

mag fusion run once above 1.5m AGL

e5bb37e

ekf2: log ev_yaw_fault

347fc96

bresch approved these changes Aug 4, 2023

View reviewed changes

dagar merged commit d75bb62 into main Aug 4, 2023
84 of 86 checks passed

dagar deleted the pr-ekf2_mag_control_split_and_refactor branch August 4, 2023 14:39

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ekf2: separate mag and mag heading control logic #21212

ekf2: separate mag and mag heading control logic #21212

dagar commented Feb 28, 2023 •

edited

dagar commented May 30, 2023 •

edited

dagar commented Jul 26, 2023

bresch Jul 28, 2023

dagar Jul 28, 2023

dagar Jul 31, 2023

bresch Jul 28, 2023

dagar Jul 28, 2023

bresch Jul 31, 2023

bresch Jul 28, 2023

dagar Jul 28, 2023

bresch Jul 31, 2023

dagar Jul 31, 2023

bresch Jul 28, 2023

bresch Jul 28, 2023

dagar Jul 28, 2023

bresch Jul 28, 2023

dagar Jul 28, 2023

bresch Jul 31, 2023

bresch Jul 31, 2023

bresch Jul 31, 2023

dagar Jul 31, 2023

bresch Jul 31, 2023

dagar commented Jul 31, 2023

dagar commented Jul 31, 2023 •

edited

bresch Aug 2, 2023

bresch Aug 3, 2023

dagar Aug 3, 2023

bresch Aug 3, 2023

dagar Aug 3, 2023

bresch left a comment

dagar commented Aug 4, 2023


		resetEstimatorAidStatus(aid_src);

		const bool wmm_updated = (_wmm_gps_time_last_set > aid_src.time_last_fuse);

		{
		// TODO: precision = 0.0002f is only required for the pitch90 test to pass


		if (continuing_conditions_passing && _control_status.flags.yaw_align) {

		if (mag_sample.reset \|\| checkHaglYawResetReq()) {

ekf2: separate mag and mag heading control logic #21212

ekf2: separate mag and mag heading control logic #21212

Conversation

dagar commented Feb 28, 2023 • edited

dagar commented May 30, 2023 • edited

dagar commented Jul 26, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dagar commented Jul 31, 2023

dagar commented Jul 31, 2023 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bresch left a comment

Choose a reason for hiding this comment

dagar commented Aug 4, 2023

dagar commented Feb 28, 2023 •

edited

dagar commented May 30, 2023 •

edited

dagar commented Jul 31, 2023 •

edited