ekf2_test: test airspeed symforce vs sympy generated code

2026-10-14 04:53:35 +08:00 · 2022-10-03 10:59:42 -04:00
parent 29ebef1f74
commit 299e6058e3
2 changed files with 204 additions and 0 deletions
@@ -37,6 +37,7 @@ add_subdirectory(sensor_simulator)
 add_subdirectory(test_helper)

 px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
+px4_add_unit_gtest(SRC test_EKF_airspeed_fusion_generated.cpp LINKLIBS ecl_EKF)
 px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
@@ -0,0 +1,203 @@
+/****************************************************************************
+ *
+ *   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 "../EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h"
+#include "../EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h"
+
+using namespace matrix;
+
+typedef matrix::Vector<float, 24> Vector24f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+template<int ... Idxs>
+using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+
+float randf()
+{
+	return (float)rand() / (float)RAND_MAX;
+}
+
+struct DiffRatioReport {
+	float max_diff_fraction;
+	float max_row;
+	float max_v1;
+	float max_v2;
+};
+
+DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &v2)
+{
+	// Find largest observation variance difference as a fraction of v1 or v2
+	DiffRatioReport report = {};
+
+	for (int row = 0; row < 24; row++) {
+		float diff_fraction;
+
+		if (fabsf(v1(row)) > FLT_EPSILON) {
+			diff_fraction = fabsf(v2(row) - v1(row)) / fabsf(v1(row));
+
+		} else if (fabsf(v2(row)) > FLT_EPSILON) {
+			diff_fraction = fabsf(v2(row) - v1(row)) / fabsf(v2(row));
+
+		} else {
+			diff_fraction = 0.0f;
+		}
+
+		if (diff_fraction > report.max_diff_fraction) {
+			report.max_diff_fraction = diff_fraction;
+			report.max_row = row;
+			report.max_v1 = v1(row);
+			report.max_v2 = v2(row);
+		}
+	}
+
+	return report;
+}
+
+TEST(AirspeedFusionGenerated, SympyVsSymforce)
+{
+	// Compare calculation of observation Jacobians and Kalman gains for sympy and symforce generated equations
+	const float R_TAS = sq(1.5f);
+
+	const float vn = 9.0f;
+	const float ve = 12.0f;
+	const float vd = -1.5f;
+
+	const float vwn = -4.0f;
+	const float vwe = 3.0f;
+
+	// Create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
+	SquareMatrix24f P;
+
+	for (int col = 0; col <= 23; col++) {
+		for (int row = 0; row <= col; row++) {
+			if (row == col) {
+				P(row, col) = randf();
+
+			} else {
+				P(col, row) = P(row, col) = 2.0f * (randf() - 0.5f);
+			}
+		}
+	}
+
+	// First calculate observationjacobians and Kalman gains using sympy generated equations
+	Vector24f Hfusion_sympy;
+	Vector24f Kfusion_sympy;
+
+	{
+		// Intermediate variables
+		const float HK0 = vn - vwn;
+		const float HK1 = ve - vwe;
+		const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2);
+		const float v_tas_pred = sqrtf(HK2); // predicted airspeed
+		//const float HK3 = powf(HK2, -1.0F/2.0F);
+		// calculation can be badly conditioned for very low airspeed values so don't fuse this time
+		EXPECT_GT(v_tas_pred, 1.f);
+		const float HK3 = 1.0f / v_tas_pred;
+		const float HK4 = HK0 * HK3;
+		const float HK5 = HK1 * HK3;
+		const float HK6 = 1.0F / HK2;
+		const float HK7 = HK0 * P(4, 6) - HK0 * P(6, 22) + HK1 * P(5, 6) - HK1 * P(6, 23) + P(6, 6) * vd;
+		const float HK8 = HK1 * P(5, 23);
+		const float HK9 = HK0 * P(4, 5) - HK0 * P(5, 22) + HK1 * P(5, 5) - HK8 + P(5, 6) * vd;
+		const float HK10 = HK1 * HK6;
+		const float HK11 = HK0 * P(4, 22);
+		const float HK12 = HK0 * P(4, 4) - HK1 * P(4, 23) + HK1 * P(4, 5) - HK11 + P(4, 6) * vd;
+		const float HK13 = HK0 * HK6;
+		const float HK14 = -HK0 * P(22, 23) + HK0 * P(4, 23) - HK1 * P(23, 23) + HK8 + P(6, 23) * vd;
+		const float HK15 = -HK0 * P(22, 22) - HK1 * P(22, 23) + HK1 * P(5, 22) + HK11 + P(6, 22) * vd;
+		const float inn_var = (-HK10 * HK14 + HK10 * HK9 + HK12 * HK13 - HK13 * HK15 + HK6 * HK7 * vd + R_TAS);
+		const float HK16 = HK3 / inn_var;
+
+		// Observation Jacobians
+		SparseVector24f<4, 5, 6, 22, 23> Hfusion;
+		Hfusion.at<4>() = HK4;
+		Hfusion.at<5>() = HK5;
+		Hfusion.at<6>() = HK3 * vd;
+		Hfusion.at<22>() = -HK4;
+		Hfusion.at<23>() = -HK5;
+
+		Vector24f Kfusion;
+
+		for (unsigned row = 0; row <= 3; row++) {
+			Kfusion(row) = HK16 * (HK0 * P(4, row) - HK0 * P(row, 22) + HK1 * P(5, row) - HK1 * P(row, 23) + P(6, row) * vd);
+		}
+
+		Kfusion(4) = HK12 * HK16;
+		Kfusion(5) = HK16 * HK9;
+		Kfusion(6) = HK16 * HK7;
+
+		for (unsigned row = 7; row <= 21; row++) {
+			Kfusion(row) = HK16 * (HK0 * P(4, row) - HK0 * P(row, 22) + HK1 * P(5, row) - HK1 * P(row, 23) + P(6, row) * vd);
+		}
+
+		Kfusion(22) = HK15 * HK16;
+		Kfusion(23) = HK14 * HK16;
+
+		// save output
+		Hfusion_sympy(4) = Hfusion.at<4>();
+		Hfusion_sympy(5) = Hfusion.at<5>();
+		Hfusion_sympy(6) = Hfusion.at<6>();
+		Hfusion_sympy(22) = Hfusion.at<22>();
+		Hfusion_sympy(23) = Hfusion.at<23>();
+		Kfusion_sympy = Kfusion;
+	}
+
+	// Then calculate observationjacobians and Kalman gains using symforce generated equations
+	Vector24f Hfusion_symforce;
+	Vector24f Kfusion_symforce;
+
+	{
+		Vector24f state_vector{};
+		state_vector(4) = vn;
+		state_vector(5) = ve;
+		state_vector(6) = vd;
+		state_vector(22) = vwn;
+		state_vector(23) = vwe;
+
+		float innov;
+		float innov_var;
+
+		sym::ComputeAirspeedInnovAndInnovVar(state_vector, P, 0.f, R_TAS, FLT_EPSILON, &innov, &innov_var);
+		sym::ComputeAirspeedHAndK(state_vector, P, innov_var, FLT_EPSILON, &Hfusion_symforce, &Kfusion_symforce);
+	}
+
+	DiffRatioReport report = computeDiffRatioVector24f(Hfusion_sympy, Hfusion_symforce);
+	EXPECT_LT(report.max_diff_fraction, 1e-5f) << "Airspeed Hfusion max diff fraction = " << report.max_diff_fraction <<
+			" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
+
+	report = computeDiffRatioVector24f(Kfusion_sympy, Kfusion_symforce);
+	EXPECT_LT(report.max_diff_fraction, 1e-5f) << "Airspeed Kfusion max diff fraction = " << report.max_diff_fraction <<
+			" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
+}