From f7ff0a996102ed020d0cbf53b530b6ba5a2cd721 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 13 Jul 2022 17:22:56 +0200 Subject: [PATCH] WindEstimator: add test case for airspeed fusion singularity --- src/lib/wind_estimator/CMakeLists.txt | 2 + src/lib/wind_estimator/test/CMakeLists.txt | 36 ++++++++ .../test/test_airspeed_fusion.cpp | 86 +++++++++++++++++++ 3 files changed, 124 insertions(+) create mode 100644 src/lib/wind_estimator/test/CMakeLists.txt create mode 100644 src/lib/wind_estimator/test/test_airspeed_fusion.cpp diff --git a/src/lib/wind_estimator/CMakeLists.txt b/src/lib/wind_estimator/CMakeLists.txt index 8c47f08c2d..b97e4245de 100644 --- a/src/lib/wind_estimator/CMakeLists.txt +++ b/src/lib/wind_estimator/CMakeLists.txt @@ -37,6 +37,8 @@ add_library(wind_estimator ) add_dependencies(wind_estimator prebuild_targets) +add_subdirectory(test) + add_custom_target(wind_estimator_generate_airspeed_fusion COMMAND ${PYTHON_EXECUTABLE} derivation.py #DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/python/derivation.py diff --git a/src/lib/wind_estimator/test/CMakeLists.txt b/src/lib/wind_estimator/test/CMakeLists.txt new file mode 100644 index 0000000000..1db8713406 --- /dev/null +++ b/src/lib/wind_estimator/test/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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_directories(${CMAKE_CURRENT_SOURCE_DIR}/..) + +px4_add_unit_gtest(SRC test_airspeed_fusion.cpp) diff --git a/src/lib/wind_estimator/test/test_airspeed_fusion.cpp b/src/lib/wind_estimator/test/test_airspeed_fusion.cpp new file mode 100644 index 0000000000..c1d90d1d85 --- /dev/null +++ b/src/lib/wind_estimator/test/test_airspeed_fusion.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Run this test only using make tests TESTFILTER=unit-test_airspeed_fusion + */ + +#include +#include +#include +#include "../python/generated/fuse_airspeed.h" + +using namespace matrix; + +TEST(TestAirspeedFusion, SingularityHandling) +{ + // GIVEN: all inputs set to zero (singularity point) + const float true_airspeed = 0.f; + const Vector3f vel_i{0.f, 0.f, 0.f}; + const Vector3f state{0.f, 0.f, 0.f}; + const Matrix P; + const float tas_var = 0.f; + + matrix::Matrix H; + matrix::Matrix K; + float innov_var; + float innov; + + // WHEN: no singularity handling is done + float epsilon = 0.f; + sym::FuseAirspeed(vel_i, state, P, true_airspeed, tas_var, epsilon, + &H, &K, &innov_var, &innov); + + // THEN: some of the returned values are not finite + EXPECT_TRUE(std::isnan(H(0, 0))) << H(0, 0); + EXPECT_TRUE(std::isnan(H(0, 1))) << H(0, 1); + EXPECT_FLOAT_EQ(H(0, 2), 0.f) << H(0, 2); + EXPECT_TRUE(std::isnan(K(0, 0))) << K(0, 0); + EXPECT_TRUE(std::isnan(K(1, 0))) << K(1, 0); + EXPECT_TRUE(std::isnan(K(2, 0))) << K(2, 0); + EXPECT_TRUE(std::isnan(innov_var)); + EXPECT_FLOAT_EQ(innov, 0.f); + + // BUT WHEN: singularity is avoided by addind a small constant + epsilon = FLT_EPSILON; + sym::FuseAirspeed(vel_i, state, P, true_airspeed, tas_var, epsilon, + &H, &K, &innov_var, &innov); + + // THEN: the returned values are finite and the Kalman gain is null + EXPECT_FLOAT_EQ(H(0, 0), 0.f) << H(0, 0); + EXPECT_FLOAT_EQ(H(0, 1), 0.f) << H(0, 1); + EXPECT_FLOAT_EQ(H(0, 2), std::sqrt(FLT_EPSILON)) << H(0, 2); + EXPECT_TRUE(isEqual(K, Vector3f())); + EXPECT_FLOAT_EQ(innov_var, tas_var); + EXPECT_FLOAT_EQ(innov, 0.f); +}