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

@@ -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) {

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()

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();

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;
 

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;