ekf2_test: create helper functions for auto-generated code diff

src/modules/ekf2/test/CMakeLists.txt

@@ -37,7 +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_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 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)

src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp

@@ -33,58 +33,13 @@
 
 #include <gtest/gtest.h>
 #include "EKF/ekf.h"
+#include "test_helper/comparison_helper.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
@@ -97,19 +52,7 @@ TEST(AirspeedFusionGenerated, SympyVsSymforce)
 	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 = createRandomCovarianceMatrix24f();
-	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;

src/modules/ekf2/test/test_helper/CMakeLists.txt

@@ -34,6 +34,7 @@
 
 set(SRCS
 	reset_logging_checker.cpp
+	comparison_helper.cpp
    )
 
 add_library(ecl_test_helper ${SRCS})

src/modules/ekf2/test/test_helper/comparison_helper.cpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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 "comparison_helper.h"
+
+float randf()
+{
+	return (float)rand() / (float)RAND_MAX;
+}
+
+SquareMatrix24f createRandomCovarianceMatrix24f()
+{
+	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);
+			}
+		}
+	}
+
+	return P;
+}
+
+DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &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;
+}

src/modules/ekf2/test/test_helper/comparison_helper.h

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef EKF_COMPARISON_HELPER
+#define EKF_COMPARISON_HELPER
+
+#include "EKF/ekf.h"
+
+typedef matrix::Vector<float, 24> Vector24f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+template<int ... Idxs>
+using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+
+struct DiffRatioReport {
+	float max_diff_fraction;
+	float max_row;
+	float max_v1;
+	float max_v2;
+};
+
+float randf();
+
+// Create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
+SquareMatrix24f createRandomCovarianceMatrix24f();
+
+// Find largest element-wise difference as a fraction of v1 or v2
+DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &v2);
+#endif