ekf2_test: compare mag decl fusion sympy vs symforce

src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp

@@ -82,3 +82,104 @@ TEST(MagDeclinationGenerated, declinationUndefined)
 	EXPECT_TRUE(PX4_ISFINITE(innov_var) && innov_var > R && innov_var > 1e9f) << "innov_var = " << innov_var;
 	EXPECT_LT(fabsf(innov), 1e-6f);
 }
+
+void sympyMagDeclInnovVarHAndK(float magN, float magE, const SquareMatrix24f &P, float R_DECL,
+			       float &innovation_variance, Vector24f &H, Vector24f &Kfusion)
+{
+	const float h_field_min = 1e-3f;
+	const float magN_sq = sq(magN);
+
+	if (magN_sq < sq(h_field_min)) {
+		printf("bad numerical conditioning\n");
+		return;
+	}
+
+	const float HK0 = 1.0F / magN_sq;
+	const float HK1 = HK0 * sq(magE) + 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 = 1.0F / sq(HK1);
+	const float HK8 = HK5 * P(16, 16) - P(16, 17);
+	innovation_variance = -HK0 * HK6 * HK7 + HK7 * HK8 * magE / (magN * magN_sq) + R_DECL;
+	float HK9;
+
+	if (innovation_variance > R_DECL) {
+		HK9 = HK4 / innovation_variance;
+
+	} else {
+		printf("bad numerical conditioning\n");
+		return;
+	}
+
+	// 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
+	float Hfusion[24] = {};
+	Hfusion[16] = -HK0 * HK2 * magE;
+	Hfusion[17] = HK4;
+
+	// Calculate the Kalman gains
+	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));
+	}
+
+	for (int row = 0; row < 24; row++) {
+		H(row) = Hfusion[row];
+	}
+}
+
+TEST(MagDeclinationGenerated, SympyVsSymforce)
+{
+	const float R_DECL = sq(0.3f);
+	const float mag_n = 0.08f;
+	const float mag_e = -0.06f;
+
+	Vector24f state_vector{};
+	state_vector(16) = mag_n;
+	state_vector(17) = mag_e;
+
+	const float decl = M_PI_F;
+
+	SquareMatrix24f P = createRandomCovarianceMatrix24f();
+
+	Vector24f H_sympy;
+	Vector24f H_symforce;
+	Vector24f K_sympy;
+	Vector24f K_symforce;
+	float innov_sympy;
+	float innov_symforce;
+	float innov_var_sympy;
+	float innov_var_symforce;
+
+	sympyMagDeclInnovVarHAndK(mag_n, mag_e, P, R_DECL, innov_var_sympy, H_sympy, K_sympy);
+	innov_sympy = std::atan2(mag_e, mag_n) - decl;
+	sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R_DECL, FLT_EPSILON, &innov_symforce,
+			&innov_var_symforce, &H_symforce);
+	K_symforce = P * H_symforce / innov_var_symforce;
+
+	EXPECT_NEAR(innov_sympy, innov_symforce, 1e-5f);
+	EXPECT_NEAR(innov_var_sympy, innov_var_symforce, 1e-2f); // Slightly different because of epsilon
+
+	DiffRatioReport report = computeDiffRatioVector24f(H_sympy, H_symforce);
+	EXPECT_LT(report.max_diff_fraction, 2e-4f)
+			<< "Max diff fraction = " << report.max_diff_fraction
+			<< " location index = " << report.max_row
+			<< " sympy = " << report.max_v1
+			<< " symforce = " << report.max_v2;
+
+	report = computeDiffRatioVector24f(K_sympy, K_symforce);
+	EXPECT_LT(report.max_diff_fraction, 2e-4f)
+			<< "Max diff fraction = " << report.max_diff_fraction
+			<< " location index = " << report.max_row
+			<< " sympy = " << report.max_v1
+			<< " symforce = " << report.max_v2;
+}