ekf2: declination fusion don't use uninitialized parameter (EKF2_MAG_…

…DECL)
PX4 · Feb 21, 2024 · 506c60c · 506c60c
1 parent 643d3e3
commit 506c60c
Show file tree

Hide file tree

Showing 2 changed files with 158 additions and 139 deletions.
diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp
@@ -227,35 +227,52 @@ bool Ekf::fuseDeclination(float decl_sigma)
 		return false;
 	}
 
-	// observation variance (rad**2)
-	const float R_DECL = sq(decl_sigma);
+	float decl_measurement = NAN;
 
-	VectorState H;
-	float decl_pred;
-	float innovation_variance;
+	if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+	    && PX4_ISFINITE(_mag_declination_gps)
+	   ) {
+		decl_measurement = _mag_declination_gps;
 
-	// TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter?
-	sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
+	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+		  ) {
+		decl_measurement = math::radians(_params.mag_declination_deg);
+	}
 
-	const float innovation = wrap_pi(decl_pred - getMagDeclination());
+	if (PX4_ISFINITE(decl_measurement)) {
 
-	if (innovation_variance < R_DECL) {
-		// variance calculation is badly conditioned
-		return false;
-	}
+		// observation variance (rad**2)
+		const float R_DECL = sq(decl_sigma);
+
+		VectorState H;
+		float decl_pred;
+		float innovation_variance;
+
+		sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
 
-	// Calculate the Kalman gains
-	VectorState Kfusion = P * H / innovation_variance;
+		const float innovation = wrap_pi(decl_pred - decl_measurement);
 
-	const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);
+		if (innovation_variance < R_DECL) {
+			// variance calculation is badly conditioned
+			return false;
+		}
+
+		// Calculate the Kalman gains
+		VectorState Kfusion = P * H / innovation_variance;
 
-	_fault_status.flags.bad_mag_decl = !is_fused;
+		const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);
 
-	if (is_fused) {
-		limitDeclination();
+		_fault_status.flags.bad_mag_decl = !is_fused;
+
+		if (is_fused) {
+			limitDeclination();
+		}
+
+		return is_fused;
 	}
 
-	return is_fused;
+	return false;
 }
 
 void Ekf::limitDeclination()
@@ -274,7 +291,9 @@ void Ekf::limitDeclination()
 		// set to 50% of the horizontal strength from geo tables if location is known
 		h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
 
-	} else if (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) {
+	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+		  ) {
 		// use parameter value if GPS isn't available
 		decl_reference = math::radians(_params.mag_declination_deg);
 	}