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

…n outlier

fixes ecl issue #525

EKF/control.cpp

@@ -57,7 +57,7 @@ void Ekf::controlFusionModes()
 		// and declare the tilt alignment complete
 		if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) {
 			_control_status.flags.tilt_align = true;
-			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+			_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
 
 			// send alignment status message to the console
 			if (_control_status.flags.baro_hgt) {
@@ -506,7 +506,7 @@ void Ekf::controlOpticalFlowFusion()
 		{
 			// If the heading is not aligned, reset the yaw and magnetic field states
 			if (!_control_status.flags.yaw_align) {
-				_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+				_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
 			}
 
 			// If the heading is valid and use is not inhibited , start using optical flow aiding
@@ -628,7 +628,7 @@ void Ekf::controlGpsFusion()
 								       _mag_inhibit_yaw_reset_req;
 				if (want_to_reset_mag_heading && canResetMagHeading()) {
 					_control_status.flags.ev_yaw = false;
-					_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+					_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
 					// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
 					// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
 					if (_mag_inhibit_yaw_reset_req) {

EKF/ekf.h

@@ -621,6 +621,7 @@ class Ekf : public EstimatorInterface
 
 	// control fusion of magnetometer observations
 	void controlMagFusion();
+	void updateMagFilter();
 
 	bool canRunMagFusion() const;
 

EKF/ekf_helper.cpp

@@ -545,7 +545,7 @@ bool Ekf::realignYawGPS()
 
 	} else {
 		// attempt a normal alignment using the magnetometer
-		return resetMagHeading(_mag_sample_delayed.mag);
+		return resetMagHeading(_mag_lpf.getState());
 
 	}
 }

EKF/mag_control.cpp

@@ -41,6 +41,8 @@
 
 void Ekf::controlMagFusion()
 {
+	updateMagFilter();
+
 	// If we are on ground, store the local position and time to use as a reference
 	// Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude
 	if (!_control_status.flags.in_air) {
@@ -102,6 +104,13 @@ void Ekf::controlMagFusion()
 	}
 }
 
+void Ekf::updateMagFilter()
+{
+	if (_mag_data_ready) {
+		_mag_lpf.update(_mag_sample_delayed.mag);
+	}
+}
+
 bool Ekf::canRunMagFusion() const
 {
 	// check for new magnetometer data that has fallen behind the fusion time horizon
@@ -131,7 +140,7 @@ void Ekf::runOnGroundYawReset()
 {
 	if (_mag_yaw_reset_req && isYawResetAuthorized()) {
 		const bool has_realigned_yaw = canResetMagHeading()
-					       ? resetMagHeading(_mag_sample_delayed.mag)
+					       ? resetMagHeading(_mag_lpf.getState())
 					       : false;
 
 		_control_status.flags.yaw_align = _control_status.flags.yaw_align || has_realigned_yaw;
@@ -155,7 +164,7 @@ void Ekf::runInAirYawReset()
 		bool has_realigned_yaw = false;
 
 		if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); }
-		else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_sample_delayed.mag); }
+		else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); }
 
 		_control_status.flags.yaw_align = _control_status.flags.yaw_align || has_realigned_yaw;
 		_mag_yaw_reset_req = !has_realigned_yaw;
@@ -354,7 +363,7 @@ void Ekf::run3DMagAndDeclFusions()
 {
 	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
+		// covariances need to be corrected to incorporate knowledge of the declination
 		// before fusing magnetomer data to prevent rapid rotation of the earth field
 		// states for the first few observations.
 		fuseDeclination(0.02f);