ekf2: fuseDeclination respect mag update_all_states

- when both mag_hdg/mag_3d are inactive we should be able to continue
   updating mag without any possible impact on other states

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -237,13 +237,13 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 						if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
 						    && PX4_ISFINITE(_wmm_declination_rad)
 						   ) {
-							fuseDeclination(_wmm_declination_rad, 0.5f);
+							fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states);
 
 						} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
 							   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
 							  ) {
 
-							fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f);
+							fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f, update_all_states);
 
 						} else {
 							_control_status.flags.mag_dec = false;

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -151,7 +151,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	return false;
 }
 
-bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma)
+bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma, bool update_all_states)
 {
 	// observation variance (rad**2)
 	const float R_DECL = sq(decl_sigma);
@@ -174,6 +174,19 @@ bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma)
 	// Calculate the Kalman gains
 	VectorState Kfusion = P * H / innovation_variance;
 
+	if (!update_all_states) {
+		// zero non-mag Kalman gains if not updating all states
+
+		// copy mag_I and mag_B Kalman gains
+		const Vector3f K_mag_I = Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0);
+		const Vector3f K_mag_B = Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0);
+
+		// zero all Kalman gains, then restore mag
+		Kfusion.setZero();
+		Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = K_mag_I;
+		Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
+	}
+
 	const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
 
 	_fault_status.flags.bad_mag_decl = !is_fused;

src/modules/ekf2/EKF/ekf.h

@@ -773,7 +773,7 @@ private:
 
 	// fuse magnetometer declination measurement
 	//  declination uncertainty in radians
-	bool fuseDeclination(float decl_measurement_rad, float decl_sigma);
+	bool fuseDeclination(float decl_measurement_rad, float decl_sigma, bool update_all_states = false);
 
 #endif // CONFIG_EKF2_MAGNETOMETER