diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 0d1b02d8f0..e74d968934 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/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) diff --git a/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp b/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp index 8e3cf500ef..9d4ac39012 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp @@ -33,58 +33,13 @@ #include #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 Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; -template -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; - - 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); - } - } - } + SquareMatrix24f P = createRandomCovarianceMatrix24f(); // First calculate observationjacobians and Kalman gains using sympy generated equations Vector24f Hfusion_sympy; diff --git a/src/modules/ekf2/test/test_helper/CMakeLists.txt b/src/modules/ekf2/test/test_helper/CMakeLists.txt index 3c5a89b97e..e5ac7d9a62 100644 --- a/src/modules/ekf2/test/test_helper/CMakeLists.txt +++ b/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}) diff --git a/src/modules/ekf2/test/test_helper/comparison_helper.cpp b/src/modules/ekf2/test/test_helper/comparison_helper.cpp new file mode 100644 index 0000000000..eb428e00f0 --- /dev/null +++ b/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; +} diff --git a/src/modules/ekf2/test/test_helper/comparison_helper.h b/src/modules/ekf2/test/test_helper/comparison_helper.h new file mode 100644 index 0000000000..d290885895 --- /dev/null +++ b/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 Vector24f; +typedef matrix::SquareMatrix SquareMatrix24f; +template +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