[ekf] controlMagFusion refactor and mag field strength check #662

bresch · 2019-10-29T16:56:02Z

I basically tried to abstract the overall mag fusion type switching from the implementation details and get rid of the multiple checks for the same flags (e.g.: in_air).

I still need to test a lot and maybe I'll move the whole controlMagFusion and sub-functions to a new file.

Feature added: the mag fusion is inhibited when the norm of the magnetic field exceeds X times the expected strength (from WMM table).

fixes #525

mhkabir · 2019-10-29T18:49:08Z

Revolutionary!

bresch · 2019-10-31T16:03:20Z

Test of the EKF2_MAG_TYPE parameter:

Initial value: 5: None
t = 50s: 2: 3-axis, immediately switch to 3-axis fusion, yaw is reset, yaw_aligned flag gets true
t = 59s: vehicle reaches 1.5m AGL, yaw reset
t = 92s: 1: Magnetic heading, immediately switch to heading fusion
t = 102: 2: 3-axis, switches again to 3D fusion
t = 130: 5: None, fusion stops
t = 139: 0: Automatic, enough motion, 3D fusion
t = 149: still Automatic but not enough motion, heading fusion

Also, the mag covariances are properly saved and loaded every time it switches in-and-out of 3D fusion:

Log: https://logs.px4.io/plot_app?log=e08a96ab-c5bd-4b4a-86c0-d8a73a14009b

jkflying

I haven't gone through the entire logic in detail, I'll try to do that tomorrow. Some small things i noticed:

jkflying · 2019-10-31T15:12:46Z

EKF/mag_control.cpp

+
+bool Ekf::isStrongMagneticDisturbance() const
+{
+	return _mag_sample_delayed.mag.length() > 2.f * _mag_strength_gps;


Do we want to check for the length being too small as well? Or does that never really happen?

That's a good question. I think it's really rare but it might be worth adding for completeness. I'll add a check for _mag_sample_delayed.mag.length() < .5f * _mag_strength_gps

jkflying · 2019-10-31T15:40:53Z

EKF/mag_control.cpp

+bool Ekf::isStrongMagneticDisturbance() const
+{
+	return _mag_sample_delayed.mag.length() > 2.f * _mag_strength_gps;
+}


I guess we need to check that _mag_strength_gps is valid? I assume we drop back to 2D fusion if we don't have GPS... but what happens to this check?

Right _mag_strength_gps = 0.f when there is no GPS fix, I can protect it with _NED_origin_initialised and compare with an average value and increased margins if false

kamilritz

This is a very nice refactor. Great Work!

kamilritz · 2019-10-31T16:29:58Z

EKF/mag_control.cpp

+
+	if ((_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE)
+	    || _control_status.flags.mag_fault
+	    || !_control_status.flags.tilt_align) {


So we do not want to fuse mag before we are tilt aligned? Was this also like this before?

We already denied 3D fusion before tilt alignment: https://github.com/PX4/ecl/blob/7c1e38d3adc79db7586f86dba64f7b15635792b2/EKF/control.cpp#L1412
And it doesn't make sense to me to fuse magnetic heading if the tilt isn't aligned. Does that help to align tilt? I don't think so.
What do you think @priseborough ?

We shouldn't be using mag data before tilt is know because regardless of the method used, it requires comparison with earth frame field data.

The _control_status.flags.yaw_align flag should be true before fusing magnetic data and the _control_status.flags.tilt_align flag should be true before aligning the yaw using magnetic data.

The tilt_align flag is set true on initial filter alignment and ever goes false unless the EKF is restarted. It would make more sense to use _control_status.flags.yaw_align as a requirement for fusing mag data.

kamilritz · 2019-11-01T07:13:58Z

EKF/mag_control.cpp

+		if (_control_status.flags.in_air) {
+			checkHaglYawResetReq();
+			runInAirYawReset();
+			runVelPosReset();


Previously a velocity or position reset was only called in the fixed wing loop. Is this functional change intended?

The _velpos_reset_request flag is currently only set in realignYawGPS which is only called -as you said- for fixed-wings. However, I don't see why this flag should be reserved for fixed-wings; the logic isn't there yet, but it would make sense to me that a large heading realignment on a multicopter triggers a vel/pos reset. Don't you think so?

Independent of platform type it could be conditional on the level of velocity and position innovation test ratios. If the vehicle is hovering or on ground and has small velocities, then a velocity and position reset is probably unnecessary. When doing a reset to GPS there is always a possibility of resetting to a noise spike.

kamilritz · 2019-11-01T07:17:44Z

EKF/mag_control.cpp

+			controlMagStateOnlyFusion();
+			break;
+
+		case MAG_FUSE_TYPE_HEADING:


Not sure if it would be better to add here the repeated function call and a break, such that it is harder to make an unintentional change to the MAG_FUSE_TYPE_HEADING.

I'll swap them and add a fall-through comment

kamilritz · 2019-11-01T07:41:12Z

EKF/mag_control.cpp

+{
+	// Use of 3D fusion requires an in-air heading alignment but it should not
+	// be used when the heading and mag biases are not observable for more than 2 seconds
+	return _control_status.flags.mag_aligned_in_flight


Previously we checked also for in_air and tilt align. Is this not necessary?

mag_aligned_in_flight cannot be true if not in_air and we don't run mag fusion at all if the tilt isn't aligned

kamilritz · 2019-11-01T07:55:39Z

EKF/mag_control.cpp

+	const bool not_using_gps = !(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.gps;
+	const bool not_using_evpos = !(_params.fusion_mode & MASK_USE_EVPOS) || !_control_status.flags.ev_pos;
+	const bool not_using_evvel = !(_params.fusion_mode & MASK_USE_EVVEL) || !_control_status.flags.ev_vel;
+	const bool not_selected_evyaw =  !(_params.fusion_mode & MASK_USE_EVYAW);


I think that the EVYAW should not belong here, since when we fuse EVYAW we do not use MAG anyway.

MaEtUgR · 2019-11-04T12:42:52Z

EKF/control.cpp

 			// Check if height has increased sufficiently to be away from ground magnetic anomalies
 			// and request a yaw reset if not already requested.
 			float terrain_vpos_estimate = isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD;
-			_mag_yaw_reset_req |= (terrain_vpos_estimate - _state.pos(2)) > 1.5f;
+			static constexpr float mag_reset_altitude = 1.5f;


maybe put this at the top of the file to have an overview of hardcoded values?

MaEtUgR · 2019-11-04T12:46:38Z

EKF/control.cpp

+		if (_mag_yaw_reset_req && !_mag_use_inhibit
+		    && _control_status.flags.tilt_align
+		    && _control_status.flags.in_air) {
+			const bool has_realigned_yaw = canRealignYawUsingGps()


I would avoid complicated ternary conditionals and can not accept nested ones.

MaEtUgR · 2019-11-04T13:13:27Z

EKF/control.cpp

+bool Ekf::isStrongMagneticDisturbance() const
+{
+	static constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
+	return _mag_sample_delayed.mag.length() > 3.f * average_earth_mag_field_strength;


It would probably also be reasonable to check for not enough field strength because the direction does not make sense anymore in that case.

bresch · 2019-11-04T16:42:32Z

@jkflying @kamilritz @MaEtUgR Thanks for the review guys, I made the modifications.

I also made a mission comparison before and after to show that the mag switch is correct.
Before:
https://logs.px4.io/plot_app?log=754be585-9ff7-4a20-b38e-aea2ca140528

After:
https://logs.px4.io/plot_app?log=aff79634-e523-4523-8dbe-2cdb9fcab90d

You can see in the second log that there is one additional switch to heading fusion at 65s. This makes sense to me as it is during a straight line; I don't understand why it was not switching before.

CarlOlsson

Nice refactoring!!
I added some comments. This is the largest change in a long time to the EKF so please be very careful before merging this

CarlOlsson · 2019-11-05T08:27:55Z

EKF/mag_control.cpp

+bool Ekf::isMeasuredMatchingAverageMagStrength() const
+{
+	constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
+	constexpr float average_earth_mag_gate_size = 2.5f;
+	return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(),
+					  average_earth_mag_field_strength,
+					  average_earth_mag_gate_size);
+}


This check is too tight for places with a low magnetic field strength.
E.g. in south america there are places with a mag strength of 0.18 Gauss ->
0.18 * 2.5 = 0.45, and this is with a perfect magnetometer calibration.

@CarlOlsson Good catch. which gate would you suggest?

0.18 * 3.0 = 0.54 0.18 * 3.5 = 0.63 0.18 * 4.0 = 0.72

I'm changing to a expected +/- gate check

CarlOlsson · 2019-11-05T08:44:50Z

EKF/mag_control.cpp

+		bool has_realigned_yaw = false;
+
+		if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); }
+		else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_sample_delayed.mag); }


This issue was there before but since you anyway change a lot of things regarding mag fusion you could consider low pass filtering the mag sample going into resetMagHeading(). As noted in the linked issue, there is no consistency check or outlier detection at all on this sample, so if this one sample is bad it have critical consequences.

Fixed in 9937d67

CarlOlsson · 2019-11-05T08:51:52Z

EKF/mag_control.cpp

+			selectMagAuto();
+			break;
+
+		case MAG_FUSE_TYPE_AUTOFW:


This mode was developed by us to prevent a badly aligned magnetometer to deteriorate the accelerometer biases leading to bad velocity tracking after a large attitude change (transition to hover mode).
However, many flights have shown that it is unsafe for a fixed wing to fly without some sort of sensor fusion (mag or synthetic etc) stabilizing the heading estimate. We hence stopped using this mode more than a year ago and I would strongly suggest no one to use it. I.e. I would vote for removing it

Perfect, less code is better :) I'll remove it.

CarlOlsson · 2019-11-05T08:55:42Z

EKF/mag_control.cpp

+{
+	if (!_mag_decl_cov_reset) {
+		// After any magnetic field covariance reset event the earth field state
+		// covariances need to be corrected to incorporate knowedge of the declination


Suggested change

// covariances need to be corrected to incorporate knowedge of the declination

// covariances need to be corrected to incorporate knowledge of the declination

bresch · 2019-11-06T17:40:18Z

Okay, I think I'm done here. The mag field strength check is disabled by default and will be enabled later from the firmware side via a parameter.

bresch · 2019-11-06T18:35:25Z

I made a mistake on the unit test, will fix that

mhkabir · 2019-11-06T19:31:04Z

Not a part of this PR directly, but what do people think of moving every sensor into its own source file? e.g Everything related to magnetometer fusion and outlier checking lives it in it's own file.

Move mag inhibition check in separate function

- yaw abd mag bias observability checks - mag 3D conditions - load mag covariances - set and clear mag control modes

…tom) Also split inAirYawReset from onGroundYawReset

- transform if-else into switch-case for parameter fusion type selection

flag naming in Test script

Reset some variables on ground even if mag fusion is not running yet. It could be that it runs later so we need to make sure that those variables are properly set.

…ol.cpp

to average earth mag field with larger gate if not valid

0.1/0.9 type low-pass filters are commonly used to smooth data, this class is meant to abstract the computation of this filter

…n outlier fixes ecl issue #525

add parameter to enable or disable mag field strength check

…lemented for VTOL but did not solve the problem and should not be used anymore

bresch · 2019-11-06T20:18:31Z

Not a part of this PR directly, but what do people think of moving every sensor into its own source file? e.g Everything related to magnetometer fusion and outlier checking lives it in it's own file.

@mhkabir I think this is necessary, yes. At least own file (as we refactor each component) and maybe even its own class to avoid having a massive class with 1 trillion global variables.

mhkabir · 2019-11-06T21:50:03Z

maybe even its own class to avoid having a massive class with 1 trillion global variables.

Yes, exactly 😆

…ng the flag

as there is no reason to fuse mag when the ekf isn't aligned

bresch · 2019-11-08T09:45:46Z

This PR has been tested by the Dronecode test team: PX4/PX4-Autopilot#13380
Everything seems good, I checked in the logs that:

The automatic mag fusion type switches correctly from 3D to heading
When in 3D, the bias are learned, when switching to heading, the covariance is saved and restored when switching again to 3D
The yaw is realigned in air at 1.5m, the yaw is reset (quat reset counter increases) and the flags is set

I'm quite confident that this PR is now ready to be merged. I don't think that waiting more will help.
Can one of you approve the PR please?

RomanBapst

Nice cleanup!

bresch · 2019-11-08T15:03:16Z

Youhou, thanks for your reviews guys.

dropped during the mag fusion refactor PR #662

bresch added the ekf label Oct 29, 2019

bresch self-assigned this Oct 29, 2019

bresch requested review from jkflying, priseborough, kamilritz and MaEtUgR and removed request for jkflying October 31, 2019 15:04

bresch force-pushed the pr-ekf-mag-control-refactor branch from f1d7748 to c8fd26a Compare October 31, 2019 15:23

bresch marked this pull request as ready for review October 31, 2019 15:50

jkflying reviewed Oct 31, 2019

View reviewed changes

kamilritz reviewed Nov 1, 2019

View reviewed changes

MaEtUgR reviewed Nov 4, 2019

View reviewed changes

bresch force-pushed the pr-ekf-mag-control-refactor branch 3 times, most recently from eb8f7bc to 0890824 Compare November 4, 2019 15:27

bresch mentioned this pull request Nov 4, 2019

[Ekf] controlMagFusion refactor and mag field strength check PX4/PX4-Autopilot#13380

Closed

bresch force-pushed the pr-ekf-mag-control-refactor branch from 0890824 to 2a52d12 Compare November 5, 2019 08:00

CarlOlsson reviewed Nov 5, 2019

View reviewed changes

bresch force-pushed the pr-ekf-mag-control-refactor branch 2 times, most recently from 3907dd6 to 9937d67 Compare November 6, 2019 16:24

bresch force-pushed the pr-ekf-mag-control-refactor branch 2 times, most recently from 790fa31 to 4797df7 Compare November 6, 2019 20:09

bresch added 2 commits November 6, 2019 21:12

ekf_control: Inhibit mag fusion when field magnitude is large

c773401

Move mag inhibition check in separate function

ekf_control: pull out of functionalities out of controlMagFusion

b5d9c68

- yaw abd mag bias observability checks - mag 3D conditions - load mag covariances - set and clear mag control modes

bresch added 12 commits November 6, 2019 21:12

ekf_control: extract mag_state_only functionality of AUTOFW (VTOL cus…

70d29a7

…tom) Also split inAirYawReset from onGroundYawReset

ekf_control: extract mag automatic selection

6bf34ab

- transform if-else into switch-case for parameter fusion type selection

ekf_control: extract run3DMagAndDeclFusion, reorganize functions, fix

cd8f2e7

flag naming in Test script

ekf_control: do not run mag fusion if tilt is not aligned.

714f8a1

Reset some variables on ground even if mag fusion is not running yet. It could be that it runs later so we need to make sure that those variables are properly set.

ekf_control: move controlMagFusion and related functions to mag_contr…

769a543

…ol.cpp

ekf control: check for validity of mag strength from WMM and falls back

4845e49

to average earth mag field with larger gate if not valid

ekf control: remove evyaw check for mag inhibition

1658eb8

ekf control: change nested ternary operator into if-else if

823af18

Ekf: create AlphaFilter template class for simple low-pass filtering

bec6a2e

0.1/0.9 type low-pass filters are commonly used to smooth data, this class is meant to abstract the computation of this filter

ekf control: reset heading using mag_lpf data to avoid resetting on a…

0b788cf

…n outlier fixes ecl issue #525

ekf control: replace mag_states_only flag with mag_field_disturbed and

57ad284

add parameter to enable or disable mag field strength check

ekf control: remove AUTOFW mag fusion type as not needed This was imp…

b806b8b

…lemented for VTOL but did not solve the problem and should not be used anymore

bresch force-pushed the pr-ekf-mag-control-refactor branch from 4797df7 to b806b8b Compare November 6, 2019 20:15

bresch added 3 commits November 7, 2019 13:06

ekf control: use start/stop mag functions everywhere instead of setti…

29e3c20

…ng the flag

ekf control: Run mag fusion depending on yaw_align instead of tilt_align

f528df6

as there is no reason to fuse mag when the ekf isn't aligned

AlphaFilter: add test for float and Vector3f

d349676

bresch force-pushed the pr-ekf-mag-control-refactor branch from 69a1c6c to d349676 Compare November 7, 2019 13:58

jkflying approved these changes Nov 8, 2019

View reviewed changes

RomanBapst approved these changes Nov 8, 2019

View reviewed changes

bresch merged commit c7bdf25 into master Nov 8, 2019

bresch deleted the pr-ekf-mag-control-refactor branch November 8, 2019 15:03

bresch added a commit that referenced this pull request Dec 19, 2019

mag_fusion: re-introduce delay between mag reset and fusion, this was

fd2d779

dropped during the mag fusion refactor PR #662

bresch mentioned this pull request Dec 19, 2019

mag_fusion: re-introduce delay between mag reset and fusion #692

Merged

priseborough pushed a commit that referenced this pull request Jan 15, 2020

mag_fusion: re-introduce delay between mag reset and fusion, this was

d0fe1f0

dropped during the mag fusion refactor PR #662

bresch mentioned this pull request Jun 25, 2020

Check magnetic field vector norm PX4/PX4-Autopilot#15107

Closed

priseborough added the enhancement label Jun 27, 2020

	// covariances need to be corrected to incorporate knowedge of the declination
	// covariances need to be corrected to incorporate knowledge of the declination

[ekf] controlMagFusion refactor and mag field strength check #662

[ekf] controlMagFusion refactor and mag field strength check #662

Conversation

bresch commented Oct 29, 2019 • edited Loading

mhkabir commented Oct 29, 2019

bresch commented Oct 31, 2019 • edited Loading

jkflying left a 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

kamilritz left a 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

bresch commented Nov 4, 2019

CarlOlsson left a 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

bresch Nov 6, 2019 • edited Loading

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 commented Nov 6, 2019

bresch commented Nov 6, 2019

mhkabir commented Nov 6, 2019

bresch commented Nov 6, 2019

mhkabir commented Nov 6, 2019 • edited Loading

bresch commented Nov 8, 2019

RomanBapst left a comment

Choose a reason for hiding this comment

bresch commented Nov 8, 2019 • edited Loading

bresch commented Oct 29, 2019 •

edited

Loading

bresch commented Oct 31, 2019 •

edited

Loading

bresch Nov 6, 2019 •

edited

Loading

mhkabir commented Nov 6, 2019 •

edited

Loading

bresch commented Nov 8, 2019 •

edited

Loading