From 299e6058e3f12e0392ce2b7899bbe93c1a24af52 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 3 Oct 2022 13:34:45 +0200 Subject: [PATCH] ekf2_test: test airspeed symforce vs sympy generated code --- src/modules/ekf2/test/CMakeLists.txt | 1 + .../test_EKF_airspeed_fusion_generated.cpp | 203 ++++++++++++++++++ 2 files changed, 204 insertions(+) create mode 100644 src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index a565d96d25..0d1b02d8f0 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -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) diff --git a/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp b/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp new file mode 100644 index 0000000000..8e3cf500ef --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp @@ -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 +#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 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 + 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; +}