From 25148e1b45d3cf8af4fe8ece6eaf5997d916503b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 17 Jan 2019 13:51:54 +1100
Subject: [PATCH] EKF: Prevent rapid changes in declination estimate after a
 reset

Ensures that each time the earth field covariance and variance data is reset, that the off-diagonal elements containing earth field declination angle certainty is restored.
---
 EKF/control.cpp    |  5 ++++-
 EKF/covariance.cpp |  3 +++
 EKF/ekf.h          |  3 ++-
 EKF/ekf_helper.cpp | 10 ++++++++++
 EKF/mag_fusion.cpp |  4 ++--
 5 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index 489e17b900..735de164e3 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -1510,6 +1510,9 @@ void Ekf::controlMagFusion()
 				for (uint8_t index = 0; index <= 5; index ++) {
 					P[index + 16][index + 16] = sq(_params.mag_noise);
 				}
+
+				// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
+				fuseDeclination(0.02f);
 			}
 
 		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
@@ -1567,7 +1570,7 @@ void Ekf::controlMagFusion()
 			fuseMag();
 
 			if (_control_status.flags.mag_dec) {
-				fuseDeclination();
+				fuseDeclination(0.5f);
 			}
 
 		} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index 6e6b0497d8..fc365f408e 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -889,6 +889,9 @@ void Ekf::resetMagCovariance()
 	for (uint8_t rc_index = 16; rc_index <= 21; rc_index ++) {
 		P[rc_index][rc_index] = sq(_params.mag_noise);
 	}
+
+	// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
+	fuseDeclination(0.02f);
 }
 
 void Ekf::resetWindCovariance()
diff --git a/EKF/ekf.h b/EKF/ekf.h
index cb5d375ba5..f57890a4a9 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -489,7 +489,8 @@ private:
 	bool resetGpsAntYaw();
 
 	// fuse magnetometer declination measurement
-	void fuseDeclination();
+	// argument passed in is the declination uncertainty in radians
+	void fuseDeclination(float decl_sigma);
 
 	// fuse airspeed measurement
 	void fuseAirspeed();
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index c0055954bf..bc59009535 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -491,8 +491,12 @@ bool Ekf::realignYawGPS()
 				P[index][index] = sq(_params.mag_noise);
 			}
 
+			// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
+			fuseDeclination(0.02f);
+
 			// 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;
 
@@ -525,6 +529,9 @@ bool Ekf::realignYawGPS()
 				P[index][index] = sq(_params.mag_noise);
 			}
 
+			// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
+			fuseDeclination(0.02f);
+
 			// record the start time for the magnetic field alignment
 			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 
@@ -684,6 +691,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		P[index][index] = sq(_params.mag_noise);
 	}
 
+	// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
+	fuseDeclination(0.02f);
+
 	// record the time for the magnetic field alignment event
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 
diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index 3814c7e706..f2793deefc 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -791,13 +791,13 @@ void Ekf::fuseHeading()
 	}
 }
 
-void Ekf::fuseDeclination()
+void Ekf::fuseDeclination(float decl_sigma)
 {
 	// assign intermediate state variables
 	float magN = _state.mag_I(0);
 	float magE = _state.mag_I(1);
 
-	float R_DECL = sq(0.5f);
+	float R_DECL = sq(decl_sigma);
 
 	// Calculate intermediate variables
 	float t2 = magE*magE;