From 363edf5eb9124777b510e601874b907a7ef30b13 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 10:16:48 +1100 Subject: [PATCH 01/12] EKF: Fix yaw reset for fixed wing Ensures that a complete reset of velocity and position states will always be performed if yaw has had to be reset using GPS velocity. Ensures that the yaw_align status cannot be set to false once the filter has aligned. --- EKF/control.cpp | 24 +++++++++++++----------- EKF/ekf.h | 1 + EKF/ekf_helper.cpp | 5 +++-- 3 files changed, 17 insertions(+), 13 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index f386e1a04c..759468659c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -485,14 +485,12 @@ void Ekf::controlGpsFusion() if (do_reset) { // Reset states to the last GPS measurement if (_control_status.flags.fixed_wing) { - // if flying a fixed wing aircraft, do a complete reset that includes yaw, velocity and position + // if flying a fixed wing aircraft, do a complete reset that includes yaw realignYawGPS(); - resetVelocity(); - resetPosition(); - } else { - resetVelocity(); - resetPosition(); } + resetVelocity(); + resetPosition(); + _velpos_reset_request = false; ECL_WARN("EKF GPS fusion timeout - reset to GPS"); // Reset the timeout counters @@ -1200,12 +1198,16 @@ void Ekf::controlMagFusion() if (!_flt_mag_align_complete) { // If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { - _control_status.flags.yaw_align = realignYawGPS(); - _flt_mag_align_complete = _control_status.flags.yaw_align; + _flt_mag_align_complete = realignYawGPS(); + if (_velpos_reset_request) { + resetVelocity(); + resetPosition(); + _velpos_reset_request = false; + } } else { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); - _flt_mag_align_complete = _control_status.flags.yaw_align; + _flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag); } + _control_status.flags.yaw_align = _control_status.flags.yaw_align || _flt_mag_align_complete; } else { // reset the mag field covariances zeroRows(P, 16, 21); @@ -1261,7 +1263,7 @@ void Ekf::controlMagFusion() } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states if (!_control_status.flags.mag_3D) { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag) || _control_status.flags.yaw_align; } // always use 3-axis mag fusion diff --git a/EKF/ekf.h b/EKF/ekf.h index 2309748ff4..db130e2bea 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -380,6 +380,7 @@ private: bool _flt_mag_align_complete{true}; ///< true when the in-flight mag field alignment has been completed uint64_t _time_last_movement{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) float _saved_mag_variance[6] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) + bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required gps_check_fail_status_u _gps_check_fail_status{}; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 65ddc59ade..5b9a9c0371 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -411,6 +411,9 @@ bool Ekf::realignYawGPS() // calculate new filter quaternion states using corected yaw angle _state.quat_nominal = Quatf(euler321); + // If heading was bad, then we alos need to reset the velocity and position states + _velpos_reset_request = badMagYaw; + // update transformation matrix from body to world frame using the current state estimate _R_to_earth = quat_to_invrotmat(_state.quat_nominal); @@ -453,8 +456,6 @@ bool Ekf::realignYawGPS() // capture the reset event _state_reset_status.quat_counter++; - // the alignment using GPS has been successful - _control_status.flags.yaw_align = true; return true; } else { From 29d383edbf1082e99115522b66624d74ab7f36ac Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 10:29:28 +1100 Subject: [PATCH 02/12] EKF: Allow mag field estimates to stabilise before use --- EKF/control.cpp | 9 +++++++++ EKF/ekf.h | 4 +++- EKF/ekf_helper.cpp | 5 +++++ EKF/mag_fusion.cpp | 8 +++++--- 4 files changed, 22 insertions(+), 4 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 759468659c..dee0f44360 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1244,6 +1244,9 @@ void Ekf::controlMagFusion() _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) && _control_status.flags.fixed_wing; + // For the first 10 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise + _flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)1e7); + 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 @@ -1293,6 +1296,12 @@ void Ekf::controlMagFusion() fuseDeclination(); } + if (_flt_mag_align_converging) { + _control_status.flags.mag_hdg = true; + fuseHeading(); + _control_status.flags.mag_hdg = false; + } + } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { // fusion of an Euler yaw angle from either a 321 or 312 rotation sequence fuseHeading(); diff --git a/EKF/ekf.h b/EKF/ekf.h index db130e2bea..cc4a469d99 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -377,7 +377,9 @@ private: // Variables used to control activation of post takeoff functionality float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) - bool _flt_mag_align_complete{true}; ///< true when the in-flight mag field alignment has been completed + bool _flt_mag_align_complete{false}; ///< true when the in-flight mag field alignment has been completed + bool _flt_mag_align_converging{false}; ///< true when the in-flight mag field post alignment convergence is being performd + uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) uint64_t _time_last_movement{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) float _saved_mag_variance[6] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5b9a9c0371..4159a104b4 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -437,6 +437,8 @@ bool Ekf::realignYawGPS() P[index][index] = sq(_params.mag_noise); } + // record the start time for the magnetic field alignment + _flt_mag_align_start_time = _imu_sample_delayed.time_us; // calculate the amount that the quaternion has changed by _state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; @@ -637,6 +639,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) P[index][index] = sq(_params.mag_noise); } + // record the start time for the magnetic field alignment + _flt_mag_align_start_time = _imu_sample_delayed.time_us; + // calculate the amount that the quaternion has changed by _state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 2294d1a5b0..1c0740b05a 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -163,6 +163,8 @@ void Ekf::fuseMag() return; } + bool update_all_states = !_control_status.flags.update_mag_states_only && !_flt_mag_align_converging; + // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { @@ -187,7 +189,7 @@ void Ekf::fuseMag() SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3; SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2; - if (!_control_status.flags.update_mag_states_only) { + if (update_all_states) { Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]); Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]); Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]); @@ -241,7 +243,7 @@ void Ekf::fuseMag() SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2; SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3; - if (!_control_status.flags.update_mag_states_only) { + if (update_all_states) { Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); @@ -295,7 +297,7 @@ void Ekf::fuseMag() SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3; SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3; - if (!_control_status.flags.update_mag_states_only) { + if (update_all_states) { Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]); Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]); Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]); From 9e47b6e1b618f94481133b85bddc2ee65a1bb53d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 10:32:35 +1100 Subject: [PATCH 03/12] EKF: don't reset quaternions unnecessarily When performing the initial in-flight magnetic field reset for fixed wing vehicles, resetting the quaternion states and their corresponding covariances should be avoided unless yaw errors are large, because state estimates are vulnerable to transient sensor errors immediately following a reset. --- EKF/ekf_helper.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 4159a104b4..792b98fdf9 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -461,9 +461,23 @@ bool Ekf::realignYawGPS() return true; } else { - // attempt a normal alignment using the magnetometer - return resetMagHeading(_mag_sample_delayed.mag); + // align mag states only + // calculate initial earth magnetic field states + _state.mag_I = _R_to_earth * _mag_sample_delayed.mag; + + // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance + zeroRows(P, 16, 21); + zeroCols(P, 16, 21); + + for (uint8_t index = 16; index <= 21; index ++) { + P[index][index] = sq(_params.mag_noise); + } + + // record the start time for the magnetic field alignment + _flt_mag_align_start_time = _imu_sample_delayed.time_us; + + return true; } } else { From 8f27d3fc548385136c9780fda6d7eee2350cd0e8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 11:32:57 +1100 Subject: [PATCH 04/12] EKF: don't reset quaternion states unnecessarily When performing the initial in-flight mag yaw reset for RW vehicle, do not reset the quaternion states and corresponding variances unless there has been a change in yaw angle large enough to cause problems with navigation. This is because the state estimates after a reset are more vulnerable to transient sensor errors, so a reset should be avoided if possible. --- EKF/ekf_helper.cpp | 126 ++++++++++++++++++++++++--------------------- 1 file changed, 68 insertions(+), 58 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 792b98fdf9..dda53d73f1 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -492,6 +492,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) { // save a copy of the quaternion state for later use in calculating the amount of reset change Quatf quat_before_reset = _state.quat_nominal; + Quatf quat_after_reset = _state.quat_nominal; // calculate the variance for the rotation estimate expressed as a rotation vector // this will be used later to reset the quaternion state covariances @@ -532,26 +533,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // calculate initial quaternion states for the ekf // we don't change the output attitude to avoid jumps - _state.quat_nominal = Quatf(euler321); - - // calculate the amount that the quaternion has changed by - _state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; - - // add the reset amount to the output observer buffered data - outputSample output_states; - unsigned output_length = _output_buffer.get_length(); - for (unsigned i = 0; i < output_length; i++) { - output_states = _output_buffer.get_from_index(i); - output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal; - _output_buffer.push_to_index(i, output_states); - } - - // apply the change in attitude quaternion to our newest quaternion estimate - // which was already taken out from the output buffer - _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; - - // capture the reset event - _state_reset_status.quat_counter++; + quat_after_reset = Quatf(euler321); } else { // use a 312 sequence @@ -618,32 +600,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // calculate initial quaternion states for the ekf // we don't change the output attitude to avoid jumps - _state.quat_nominal = Quatf(R_to_earth); + quat_after_reset = Quatf(R_to_earth); } - // update transformation matrix from body to world frame using the current estimate - _R_to_earth = quat_to_invrotmat(_state.quat_nominal); - - // reset the rotation from the EV to EKF frame of reference if it is being used - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) { - resetExtVisRotMat(); - } - - // update the yaw angle variance using the variance of the measurement - if (_params.fusion_mode & MASK_USE_EVYAW) { - // 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_AUTOFW) { - // using magnetic heading tuning parameter - angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); - } - - // reset the quaternion covariances using the rotation vector variances - initialiseQuatCovariances(angle_err_var_vec); - - // calculate initial earth magnetic field states - _state.mag_I = _R_to_earth * mag_init; + // set the earth magnetic field states using the updated rotation + Dcmf _R_to_earth_after = quat_to_invrotmat(quat_after_reset); + _state.mag_I = _R_to_earth_after * mag_init; // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance zeroRows(P, 16, 21); @@ -653,28 +615,76 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) P[index][index] = sq(_params.mag_noise); } - // record the start time for the magnetic field alignment + // record the time for the magnetic field alignment event _flt_mag_align_start_time = _imu_sample_delayed.time_us; // calculate the amount that the quaternion has changed by - _state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; + Quatf q_error = quat_before_reset.inversed() * quat_after_reset; + q_error.normalize(); - // add the reset amount to the output observer buffered data - outputSample output_states = {}; - unsigned output_length = _output_buffer.get_length(); + // convert the quaternion delta to a delta angle + Vector3f delta_ang_error; + float scalar; - for (unsigned i = 0; i < output_length; i++) { - output_states = _output_buffer.get_from_index(i); - output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal; - _output_buffer.push_to_index(i, output_states); + if (q_error(0) >= 0.0f) { + scalar = -2.0f; + + } else { + scalar = 2.0f; } - // apply the change in attitude quaternion to our newest quaternion estimate - // which was already taken out from the output buffer - _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + delta_ang_error(0) = scalar * q_error(1); + delta_ang_error(1) = scalar * q_error(2); + delta_ang_error(2) = scalar * q_error(3); - // capture the reset event - _state_reset_status.quat_counter++; + + // update the quaternion state estimates and corresponding covariances only if the change in angle has been large + if (delta_ang_error.norm() > math::radians(15.0f)) { + // update quaternion states + _state.quat_nominal = quat_after_reset; + + // record the state change + _state_reset_status.quat_change = q_error; + + // update transformation matrix from body to world frame using the current estimate + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + + // reset the rotation from the EV to EKF frame of reference if it is being used + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) { + resetExtVisRotMat(); + } + + // update the yaw angle variance using the variance of the measurement + if (_params.fusion_mode & MASK_USE_EVYAW) { + // 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_AUTOFW) { + // using magnetic heading tuning parameter + angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); + } + + // reset the quaternion covariances using the rotation vector variances + initialiseQuatCovariances(angle_err_var_vec); + + // add the reset amount to the output observer buffered data + outputSample output_states = {}; + unsigned output_length = _output_buffer.get_length(); + + for (unsigned i = 0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal; + _output_buffer.push_to_index(i, output_states); + } + + // apply the change in attitude quaternion to our newest quaternion estimate + // which was already taken out from the output buffer + _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + + // capture the reset event + _state_reset_status.quat_counter++; + + } return true; } From f3e34eddc9727d4f676233452d3a00c7a96d0c11 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 17:39:56 +1100 Subject: [PATCH 05/12] EKF: do not attempt to align FW yaw using GPS method if on ground --- EKF/control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index dee0f44360..3cfa2311d8 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -483,8 +483,8 @@ void Ekf::controlGpsFusion() do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max); if (do_reset) { - // Reset states to the last GPS measurement - if (_control_status.flags.fixed_wing) { + // use GPS velocity data to cehck and correct yaw angle if a FW vehicle + if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { // if flying a fixed wing aircraft, do a complete reset that includes yaw realignYawGPS(); } From 32de90b9ef3ac1cb87625fc8a412d82895a31bc8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 17:37:45 +1100 Subject: [PATCH 06/12] EKF: Add method to zero covariance terms --- EKF/ekf.h | 3 +++ EKF/ekf_helper.cpp | 20 ++++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index cc4a469d99..c75f6e4d23 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -596,6 +596,9 @@ private: // zero the specified range of columns in the state covariance matrix void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); + // zero the specified range of off diagonals the state covariance matrix + void zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); + // calculate the measurement variance for the optical flow sensor float calcOptFlowMeasVar(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index dda53d73f1..6e4b372694 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1207,6 +1207,26 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first } } +void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last) +{ + // store diagonals + uint8_t row; + float variances[last-first+1]; + for (row = first; row <= last; row++) { + variances[row-first] = cov_mat[row][row]; + } + + // zero rows and columns + zeroRows(cov_mat, first, last); + zeroCols(cov_mat, first, last); + + // restore diagonals + for (row = first; row <= last; row++) { + cov_mat[row][row] = variances[row-first]; + } + +} + bool Ekf::global_position_is_valid() { // return true if we are not doing unconstrained free inertial navigation and the origin is set From 141264fe636daf91ef3fa96e79dec81957d48fc1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 17:49:32 +1100 Subject: [PATCH 07/12] EKF: Add method to set diagonals in covariance matrix --- EKF/ekf.h | 6 +++++- EKF/ekf_helper.cpp | 14 ++++++++++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index c75f6e4d23..272f85e29e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -596,9 +596,13 @@ private: // zero the specified range of columns in the state covariance matrix void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); - // zero the specified range of off diagonals the state covariance matrix + // zero the specified range of off diagonals in the state covariance matrix void zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); + // zero the specified range of off diagonals in the state covariance matrix + // set the diagonals to the supplied value + void setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance); + // calculate the measurement variance for the optical flow sensor float calcOptFlowMeasVar(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 6e4b372694..6b36b4fed6 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1227,6 +1227,20 @@ void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t fi } +void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance) +{ + // zero rows and columns + zeroRows(cov_mat, first, last); + zeroCols(cov_mat, first, last); + + // set diagonals + uint8_t row; + for (row = first; row <= last; row++) { + cov_mat[row][row] = variance; + } + +} + bool Ekf::global_position_is_valid() { // return true if we are not doing unconstrained free inertial navigation and the origin is set From 279fc836f7a74f68225467abfa91545a13ac27c7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 18:02:29 +1100 Subject: [PATCH 08/12] EKF: Always reset covariance matrix terms when doing vel pos state reset If state errors were large before the reset, then failure to reset the covariance matrix terms can result in incorrect fusion of position and velocity measurements after the reset due to inconsistencies in the covariance matrix. --- EKF/ekf_helper.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 6b36b4fed6..db95716d80 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -56,8 +56,12 @@ bool Ekf::resetVelocity() // this reset is only called if we have new gps data at the fusion time horizon _state.vel = _gps_sample_delayed.vel; + // use GPS accuracy to reset variances + setDiag(P,4,6,sq(_gps_sample_delayed.sacc)); + } else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) { _state.vel.setZero(); + zeroOffDiag(P,4,6); } else { return false; @@ -106,15 +110,22 @@ bool Ekf::resetPosition() _state.pos(0) = _gps_sample_delayed.pos(0); _state.pos(1) = _gps_sample_delayed.pos(1); + // use GPS accuracy to reset variances + setDiag(P,7,8,sq(_gps_sample_delayed.hacc)); + } else if (_control_status.flags.opt_flow) { _state.pos(0) = 0.0f; _state.pos(1) = 0.0f; + zeroOffDiag(P,7,8); } else if (_control_status.flags.ev_pos) { // this reset is only called if we have new ev data at the fusion time horizon _state.pos(0) = _ev_sample_delayed.posNED(0); _state.pos(1) = _ev_sample_delayed.posNED(1); + // use EV accuracy to reset variances + setDiag(P,7,8,sq(_ev_sample_delayed.posErr)); + } else { return false; } From c70363c5013b97055ae6908764fe9d9c21ebd0bf Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 18:48:48 +1100 Subject: [PATCH 09/12] EKF: Don't fuse heading if FW and waiting for mag states to stabilise Doing so is a bad idea because bad mag data can drag the yaw angle away from the reset value and lead to rejection of GPS. --- EKF/control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 3cfa2311d8..47d74ff260 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -483,7 +483,7 @@ void Ekf::controlGpsFusion() do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max); if (do_reset) { - // use GPS velocity data to cehck and correct yaw angle if a FW vehicle + // use GPS velocity data to check and correct yaw angle if a FW vehicle if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { // if flying a fixed wing aircraft, do a complete reset that includes yaw realignYawGPS(); @@ -1296,7 +1296,7 @@ void Ekf::controlMagFusion() fuseDeclination(); } - if (_flt_mag_align_converging) { + if (_flt_mag_align_converging && !_control_status.flags.fixed_wing) { _control_status.flags.mag_hdg = true; fuseHeading(); _control_status.flags.mag_hdg = false; From 44c50ab2dfa6b362bb52546109797a0272d61b9a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 22:22:08 +1100 Subject: [PATCH 10/12] EKF: Correct magnetic yaw measurement using learned mag biases Don't apply bias corrections when biases are being learned to avoid possible circular data dependency. --- EKF/mag_fusion.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 1c0740b05a..632bda0c67 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -469,7 +469,12 @@ void Ekf::fuseHeading() Dcmf R_to_earth(euler321); // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + if (_control_status.flags.mag_3D) { + // don't apply bias corrections if we are learning them + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + } else { + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); + } // the angle of the projection onto the horizontal gives the yaw angle measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; @@ -556,7 +561,12 @@ void Ekf::fuseHeading() R_to_earth(2,2) = cp*cr; // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + if (_control_status.flags.mag_3D) { + // don't apply bias corrections if we are learning them + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + } else { + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); + } // the angle of the projection onto the horizontal gives the yaw angle measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; From df9f48d2d35edb5048ae8a1c71555fed45934b41 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 10 Nov 2017 07:15:03 +1100 Subject: [PATCH 11/12] EKF: Fix build error --- EKF/ekf_helper.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index db95716d80..6a928c9f11 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1220,11 +1220,11 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last) { - // store diagonals + // save diagonal elements uint8_t row; - float variances[last-first+1]; + float variances[_k_num_states]; for (row = first; row <= last; row++) { - variances[row-first] = cov_mat[row][row]; + variances[row] = cov_mat[row][row]; } // zero rows and columns @@ -1233,9 +1233,8 @@ void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t fi // restore diagonals for (row = first; row <= last; row++) { - cov_mat[row][row] = variances[row-first]; + cov_mat[row][row] = variances[row]; } - } void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance) From 72a7ab2c25e0078734b358bf3615ea22408664be Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 10 Nov 2017 10:32:49 +1100 Subject: [PATCH 12/12] EKF: Improve resistance to bad initial mag offset When magnetic field states have been reset in-flight using a single sample, the magnetic field states are not used to constrain heading drift for a period after the reset. This period has been shortened from 10 to 5 seconds which is enough time to average out the effects of measurement noise (the original concern). The shorter time has enabled the previous practice for RW vehicles of using magnetic heading in that time period to constrain yaw drift to be discontinued. This is necessary becasue while magnetic heading is being used, it fights the yaw corrections obtained from GPs observations and lengthens the time required to recover from a bad mag calibration. --- EKF/control.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 47d74ff260..5185239f77 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1244,8 +1244,9 @@ void Ekf::controlMagFusion() _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) && _control_status.flags.fixed_wing; - // For the first 10 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise - _flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)1e7); + // For the first 5 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise + // before they are used to constrain heading drift + _flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)5e6); 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 @@ -1296,12 +1297,6 @@ void Ekf::controlMagFusion() fuseDeclination(); } - if (_flt_mag_align_converging && !_control_status.flags.fixed_wing) { - _control_status.flags.mag_hdg = true; - fuseHeading(); - _control_status.flags.mag_hdg = false; - } - } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { // fusion of an Euler yaw angle from either a 321 or 312 rotation sequence fuseHeading();