ekf2: migrate mag declination to SymForce

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -47,6 +47,7 @@
 #include "python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h"
 #include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h"
 #include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h"
+#include "python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h"
 
 #include <mathlib/mathlib.h>
 
@@ -333,63 +334,24 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 
 bool Ekf::fuseDeclination(float decl_sigma)
 {
-	// assign intermediate state variables
-	const float magN = _state.mag_I(0);
-	const float magE = _state.mag_I(1);
-
-	// minimum North field strength before calculation becomes badly conditioned (T)
-	constexpr float N_field_min = 0.001f;
-
 	// observation variance (rad**2)
 	const float R_DECL = sq(decl_sigma);
 
-	// Calculate intermediate variables
-	if (fabsf(magN) < sq(N_field_min)) {
-		// calculation is badly conditioned close to +-90 deg declination
-		return false;
-	}
+	Vector24f H;
+	float innovation;
+	float innovation_variance;
 
-	const float HK0 = ecl::powf(magN, -2);
-	const float HK1 = HK0*ecl::powf(magE, 2) + 1.0F;
-	const float HK2 = 1.0F/HK1;
-	const float HK3 = 1.0F/magN;
-	const float HK4 = HK2*HK3;
-	const float HK5 = HK3*magE;
-	const float HK6 = HK5*P(16,17) - P(17,17);
-	const float HK7 = ecl::powf(HK1, -2);
-	const float HK8 = HK5*P(16,16) - P(16,17);
-	const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/ecl::powf(magN, 3) + R_DECL;
-	float HK9;
+	sym::ComputeMagDeclinationInnovInnovVarAndH(getStateAtFusionHorizonAsVector(), P, getMagDeclination(), R_DECL, FLT_EPSILON, &innovation, &innovation_variance, &H);
 
-	if (innovation_variance > R_DECL) {
-		HK9 = HK4/innovation_variance;
-	} else {
+	if (innovation_variance < R_DECL) {
 		// variance calculation is badly conditioned
 		return false;
 	}
 
-	// Calculate the observation Jacobian
-	// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
-	// Note Hfusion indices do not match state indices
-	SparseVector24f<16,17> Hfusion;
-	Hfusion.at<16>() = -HK0*HK2*magE;
-	Hfusion.at<17>() = HK4;
+	SparseVector24f<16,17> Hfusion(H);
 
 	// Calculate the Kalman gains
-	Vector24f Kfusion;
-
-	for (unsigned row = 0; row <= 15; row++) {
-		Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17));
-	}
-
-	Kfusion(16) = -HK8*HK9;
-	Kfusion(17) = -HK6*HK9;
-
-	for (unsigned row = 18; row <= 23; row++) {
-		Kfusion(row) = -HK9*(HK5*P(16,row) - P(17,row));
-	}
-
-	const float innovation = math::constrain(atan2f(magE, magN) - getMagDeclination(), -0.5f, 0.5f);
+	Vector24f Kfusion = P * Hfusion / innovation_variance;
 
 	const bool is_fused = measurementUpdate(Kfusion, Hfusion, innovation);
 

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -321,6 +321,22 @@ def compute_yaw_312_innov_var_and_h_alternate(
 
     return (innov_var, H.T)
 
+def compute_mag_declination_innov_innov_var_and_h(
+        state: VState,
+        P: MState,
+        meas: sf.Scalar,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.Scalar, VState):
+
+    meas_pred = sf.atan2(state[State.iy], state[State.ix], epsilon=epsilon)
+    innov = meas_pred - meas
+
+    H = sf.V1(meas_pred).jacobian(state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov, innov_var, H.T)
+
 print("Derive EKF2 equations...")
 generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
 generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
@@ -335,3 +351,4 @@ generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var"
 generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"])
 generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"])
 generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"])
+generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"])

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h

@@ -0,0 +1,74 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_mag_declination_innov_innov_var_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     meas: Scalar
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov: Scalar
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeMagDeclinationInnovInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                                            const matrix::Matrix<Scalar, 24, 24>& P,
+                                            const Scalar meas, const Scalar R, const Scalar epsilon,
+                                            Scalar* const innov = nullptr,
+                                            Scalar* const innov_var = nullptr,
+                                            matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 23
+
+  // Input arrays
+
+  // Intermediate terms (4)
+  const Scalar _tmp0 =
+      epsilon * ((((state(16, 0)) > 0) - ((state(16, 0)) < 0)) + Scalar(0.5)) + state(16, 0);
+  const Scalar _tmp1 =
+      Scalar(1.0) / (std::pow(_tmp0, Scalar(2)) + std::pow(state(17, 0), Scalar(2)));
+  const Scalar _tmp2 = _tmp1 * state(17, 0);
+  const Scalar _tmp3 = _tmp0 * _tmp1;
+
+  // Output terms (3)
+  if (innov != nullptr) {
+    Scalar& _innov = (*innov);
+
+    _innov = -meas + std::atan2(state(17, 0), _tmp0);
+  }
+
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = R - _tmp2 * (-P(16, 16) * _tmp2 + P(17, 16) * _tmp3) +
+                 _tmp3 * (-P(16, 17) * _tmp2 + P(17, 17) * _tmp3);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+
+    _H.setZero();
+
+    _H(16, 0) = -_tmp2;
+    _H(17, 0) = _tmp3;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/test/CMakeLists.txt

@@ -50,6 +50,7 @@ px4_add_unit_gtest(SRC test_EKF_height_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_si
 px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_initialization.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_mag_3d_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
+px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_sideslip_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)

src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include "EKF/ekf.h"
+#include "test_helper/comparison_helper.h"
+
+#include "../EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h"
+
+using namespace matrix;
+
+TEST(MagDeclinationGenerated, declination90deg)
+{
+	// GIVEN: an estimated mag declination of 90 degrees
+	Vector24f state_vector{};
+	state_vector(16) = 0.f; // North mag field
+	state_vector(17) = 0.2f; // East mag field
+
+	const float R = sq(radians(sq(0.5f)));
+	SquareMatrix24f P = createRandomCovarianceMatrix24f();
+
+	Vector24f H;
+	float innov;
+	float innov_var;
+
+	const float decl = radians(90.f);
+	sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R, FLT_EPSILON, &innov, &innov_var, &H);
+
+	// THEN: Even at the singularity point, atan2 is still defined
+	EXPECT_TRUE(innov_var < 5000.f && innov_var > R) << "innov_var = " << innov_var;
+	EXPECT_LT(fabsf(innov), 1e-6f);
+}
+
+TEST(MagDeclinationGenerated, declinationUndefined)
+{
+	// GIVEN: an undefined declination
+	Vector24f state_vector{};
+	state_vector(16) = 0.f; // North mag field
+	state_vector(17) = 0.f; // East mag field
+
+	const float R = sq(radians(sq(0.5f)));
+	SquareMatrix24f P = createRandomCovarianceMatrix24f();
+
+	Vector24f H;
+	float innov;
+	float innov_var;
+
+	const float decl = radians(0.f);
+	sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R, FLT_EPSILON, &innov, &innov_var, &H);
+
+	// THEN: the innovation variance is gigantic but finite
+	EXPECT_TRUE(PX4_ISFINITE(innov_var) && innov_var > R && innov_var > 1e9f) << "innov_var = " << innov_var;
+	EXPECT_LT(fabsf(innov), 1e-6f);
+}