From 7b4c957ad4c9943d4d88dd0635c34211f603d07d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 3 Apr 2017 10:42:26 +1000 Subject: [PATCH 1/4] ekf2: Add new mag fusion mode Adds a mode where mag fusion is only used update the field estimates --- EKF/common.h | 1 + EKF/control.cpp | 2 +- EKF/mag_fusion.cpp | 9 +++++++++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/EKF/common.h b/EKF/common.h index 643ccecdb5..70cb67cf49 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -179,6 +179,7 @@ struct dragSample { #define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic #define MAG_FUSE_TYPE_HEADING 1 ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg #define MAG_FUSE_TYPE_3D 2 ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions +#define MAG_FUSE_TYPE_AUTOFW 3 ///< The same as option 0, but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. // Maximum sensor intervals in usec #define GPS_MAX_INTERVAL 5e5 ///< Maximum allowable time interval between GPS measurements (uSec) diff --git a/EKF/control.cpp b/EKF/control.cpp index e8c1673d1f..f2b555d4b8 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1105,7 +1105,7 @@ void Ekf::controlMagFusion() // do no magnetometer fusion at all _control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = false; - } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) { // Check if height has increased sufficiently to be away from ground magnetic anomalies bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 2294d1a5b0..7581901000 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -163,6 +163,15 @@ void Ekf::fuseMag() return; } + /* + When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates. + When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing as detected by the use of wind estimation + and current airspeed estimates, then magnetometer fusion is only allowed to access the magnetic field states. + */ + _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) + && _control_status.flags.wind + && (_time_last_imu - _time_last_airspeed < 1E7); + // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { From e834522f62ad644f1469605293ba399cc1d091c2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 13 Sep 2017 09:34:36 +1000 Subject: [PATCH 2/4] EKF: Use fixed wing status flag in MAG_FUSE_TYPE_AUTOFW logic --- EKF/mag_fusion.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 7581901000..1290ff5c0f 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -165,12 +165,11 @@ void Ekf::fuseMag() /* When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates. - When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing as detected by the use of wind estimation - and current airspeed estimates, then magnetometer fusion is only allowed to access the magnetic field states. + When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing, then magnetometer fusion is only allowed + to access the magnetic field states. */ _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) - && _control_status.flags.wind - && (_time_last_imu - _time_last_airspeed < 1E7); + && _control_status.flags.fixed_wing; // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { From 6f7f05fdc0b9c20dd0dd26e0477e06bf9c88c413 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 13 Sep 2017 10:17:15 +1000 Subject: [PATCH 3/4] EKF: Move MAG_FUSE_TYPE_AUTOFW control to the expected place --- EKF/control.cpp | 10 +++++++++- EKF/mag_fusion.cpp | 8 -------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index f2b555d4b8..d4690753e3 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1188,7 +1188,15 @@ void Ekf::controlMagFusion() _control_status.flags.mag_hdg = true; } - // perform switch-over from only updating the mag states to updating all states + /* + Control switch-over between only updating the mag states to updating all states + When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates. + When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing, then magnetometer fusion is only allowed + to access the magnetic field states. + */ + _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) + && _control_status.flags.fixed_wing; + if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) { // When re-commencing use of magnetometer to correct vehicle states // set the field state variance to the observation variance and zero diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 1290ff5c0f..2294d1a5b0 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -163,14 +163,6 @@ void Ekf::fuseMag() return; } - /* - When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates. - When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing, then magnetometer fusion is only allowed - to access the magnetic field states. - */ - _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) - && _control_status.flags.fixed_wing; - // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { From dbff89fbcb63728b3d6e7c97245c113cb3610923 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 12 Oct 2017 19:54:35 +1100 Subject: [PATCH 4/4] EKF: Fix error preventing selection of MAG_FUSE_TYPE_AUTOFW --- EKF/ekf_helper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 835ed6a44b..8277ba0fab 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -506,7 +506,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // calculate the yaw angle for a 312 sequence euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0)); - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; // the angle of the projection onto the horizontal gives the yaw angle @@ -579,7 +579,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // calculate the yaw angle for a 312 sequence euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1)); - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; // the angle of the projection onto the horizontal gives the yaw angle @@ -616,7 +616,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // using error estimate from external vision data angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)); - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // using magnetic heading tuning parameter angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); }