EKF: Add function to reset yaw and magnetic field states

EKF/ekf.h

@@ -128,6 +128,8 @@ private:
 	float _mag_innov_var[3]; // earth magnetic field innovation variance
 	float _heading_innov_var; // heading measurement innovation variance
 
+	float _mag_declination = 0.0f; // magnetic declination used by reset and fusion functions (rad)
+
 	// complementary filter states
 	Vector3f _delta_angle_corr;	// delta angle correction vector
 	Vector3f _delta_vel_corr;	// delta velocity correction vector
@@ -195,6 +197,10 @@ private:
 	// reset velocity states of the ekf
 	void resetVelocity();
 
+	// reset the heading and magnetic field states using the declination and magnetometer measurements
+	// return true if successful
+	bool resetMagHeading(Vector3f &mag_init);
+
 	// reset position states of the ekf (only vertical position)
 	void resetPosition();
 

EKF/ekf_helper.cpp

@@ -94,6 +94,36 @@ void Ekf::resetHeight()
 	}
 }
 
+// Reset heading and magnetic field states
+bool Ekf::resetMagHeading(Vector3f &mag_init)
+{
+	// If we don't a tilt estimate then we cannot initialise the yaw
+	if (!_control_status.flags.tilt_align) {
+		return false;
+	}
+
+	// get the roll, pitch, yaw estimates and set the yaw to zero
+	matrix::Quaternion<float> q(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
+				    _state.quat_nominal(3));
+	matrix::Euler<float> euler_init(q);
+	euler_init(2) = 0.0f;
+
+	// rotate the magnetometer measurements into earth axes
+	matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
+	Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
+	euler_init(2) = _mag_declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
+
+	// calculate initial quaternion states
+	_state.quat_nominal = Quaternion(euler_init);
+	_output_new.quat_nominal = _state.quat_nominal;
+
+	// calculate initial earth magnetic field states
+	matrix::Dcm<float> R_to_earth(euler_init);
+	_state.mag_I = R_to_earth * mag_init;
+
+	return true;
+}
+
 // This function forces the covariance matrix to be symmetric
 void Ekf::makeSymmetrical()
 {