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

@@ -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,9 +600,52 @@ 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);
+	}
+
+	// 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);
+	zeroCols(P, 16, 21);
+
+	for (uint8_t index = 16; index <= 21; index ++) {
+		P[index][index] = sq(_params.mag_noise);
+	}
+
+	// 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
+	Quatf q_error =  quat_before_reset.inversed() * quat_after_reset;
+	q_error.normalize();
+
+	// convert the quaternion delta to a delta angle
+	Vector3f delta_ang_error;
+	float scalar;
+
+	if (q_error(0) >= 0.0f) {
+		scalar = -2.0f;
+
+	} else {
+		scalar = 2.0f;
 	}
 
+	delta_ang_error(0) = scalar * q_error(1);
+	delta_ang_error(1) = scalar * q_error(2);
+	delta_ang_error(2) = scalar * q_error(3);
+
+
+	// 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);
 
@@ -642,23 +667,6 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		// 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;
-
-	// 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;
-
-	// 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();
@@ -676,6 +684,8 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		// capture the reset event
 		_state_reset_status.quat_counter++;
 
+	}
+
 	return true;
 }