From 821e1fa8fc31df46e0e764cbab44a9422cc1d6f2 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 3 Jun 2020 11:24:26 +0200 Subject: [PATCH] terrain_estimator: add unit tests --- test/CMakeLists.txt | 1 + test/sensor_simulator/ekf_wrapper.cpp | 34 +++++ test/sensor_simulator/ekf_wrapper.h | 8 ++ test/sensor_simulator/flow.cpp | 2 +- test/test_EKF_flow.cpp | 6 +- test/test_EKF_terrain_estimator.cpp | 176 ++++++++++++++++++++++++++ 6 files changed, 223 insertions(+), 4 deletions(-) create mode 100644 test/test_EKF_terrain_estimator.cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index ae0203bc30..4920e7b446 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -51,6 +51,7 @@ set(SRCS test_EKF_airspeed.cpp test_EKF_withReplayData.cpp test_EKF_flow.cpp + test_EKF_terrain_estimator.cpp test_SensorRangeFinder.cpp test_geo.cpp ) diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index c6ac9b6762..1d56c799c3 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -177,6 +177,40 @@ bool EkfWrapper::isWindVelocityEstimated() const return control_status.flags.wind; } +void EkfWrapper::enableTerrainRngFusion() +{ + _ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseRangeFinder; +} + +void EkfWrapper::disableTerrainRngFusion() +{ + _ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseRangeFinder; +} + +bool EkfWrapper::isIntendingTerrainRngFusion() const +{ + terrain_fusion_status_u terrain_status; + terrain_status.value = _ekf->getTerrainEstimateSensorBitfield(); + return terrain_status.flags.range_finder; +} + +void EkfWrapper::enableTerrainFlowFusion() +{ + _ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseOpticalFlow; +} + +void EkfWrapper::disableTerrainFlowFusion() +{ + _ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseOpticalFlow; +} + +bool EkfWrapper::isIntendingTerrainFlowFusion() const +{ + terrain_fusion_status_u terrain_status; + terrain_status.value = _ekf->getTerrainEstimateSensorBitfield(); + return terrain_status.flags.flow; +} + Eulerf EkfWrapper::getEulerAngles() const { return Eulerf(_ekf->getQuaternion()); diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index 104424ce69..ca3128f1ac 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -89,6 +89,14 @@ public: bool isWindVelocityEstimated() const; + void enableTerrainRngFusion(); + void disableTerrainRngFusion(); + bool isIntendingTerrainRngFusion() const; + + void enableTerrainFlowFusion(); + void disableTerrainFlowFusion(); + bool isIntendingTerrainFlowFusion() const; + Eulerf getEulerAngles() const; float getYawAngle() const; matrix::Vector getQuaternionVariance() const; diff --git a/test/sensor_simulator/flow.cpp b/test/sensor_simulator/flow.cpp index e9e4544297..5026b6db81 100644 --- a/test/sensor_simulator/flow.cpp +++ b/test/sensor_simulator/flow.cpp @@ -15,7 +15,7 @@ Flow::~Flow() void Flow::send(uint64_t time) { - _flow_data.dt = time - _time_last_data_sent; + _flow_data.dt = static_cast(time - _time_last_data_sent) * 1e-6f; _flow_data.time_us = time; _ekf->setOpticalFlowData(_flow_data); } diff --git a/test/test_EKF_flow.cpp b/test/test_EKF_flow.cpp index d48cbd73dd..f3684e641a 100644 --- a/test/test_EKF_flow.cpp +++ b/test/test_EKF_flow.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * Test the gps fusion + * Test the flow fusion * @author Kamil Ritz */ @@ -89,8 +89,8 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir) const Vector2f simulated_horz_velocity(0.5f, -0.2f); flowSample flow_sample = _sensor_simulator._flow.dataAtRest(); flow_sample.flow_xy_rad = - Vector2f(- simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground, - simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground); + Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground, + -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground); _sensor_simulator._flow.setData(flow_sample); _ekf_wrapper.enableFlowFusion(); const float max_flow_rate = 5.f; diff --git a/test/test_EKF_terrain_estimator.cpp b/test/test_EKF_terrain_estimator.cpp new file mode 100644 index 0000000000..6bc6b22134 --- /dev/null +++ b/test/test_EKF_terrain_estimator.cpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2020 ECL 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. + * + ****************************************************************************/ + +/** + * Test the terrain estimator + * @author Mathieu Bresciani + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" + +class EkfTerrainTest : public ::testing::Test { + public: + + EkfTerrainTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + // Setup the Ekf with synthetic measurements + void SetUp() override + { + _ekf->init(0); + _sensor_simulator.runSeconds(2); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } + + void runFlowAndRngScenario(const float rng_height, const float flow_height) + { + _sensor_simulator.startGps(); + _ekf->set_min_required_gps_health_time(1e6); + _ekf_wrapper.enableGpsFusion(); + _ekf_wrapper.setBaroHeight(); + _sensor_simulator.runSeconds(2); // Run to pass the GPS checks + + const Vector3f simulated_velocity(0.5f, -1.0f, 0.f); + + // Configure GPS simulator data + _sensor_simulator._gps.setVelocity(simulated_velocity); + _sensor_simulator._gps.setPositionRateNED(simulated_velocity); + + // Configure range finder simulator data + _sensor_simulator._rng.setData(rng_height, 100); + _sensor_simulator._rng.setLimits(0.1f, 20.f); + _sensor_simulator.startRangeFinder(); + + // Configure optical flow simulator data + flowSample flow_sample = _sensor_simulator._flow.dataAtRest(); + flow_sample.flow_xy_rad = + Vector2f( simulated_velocity(1) * flow_sample.dt / flow_height, + -simulated_velocity(0) * flow_sample.dt / flow_height); + _sensor_simulator._flow.setData(flow_sample); + const float max_flow_rate = 5.f; + const float min_ground_distance = 0.f; + const float max_ground_distance = 50.f; + _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); + _sensor_simulator.startFlow(); + + _ekf->set_in_air_status(true); + _sensor_simulator.runSeconds(8); + } +}; + +TEST_F(EkfTerrainTest, setFlowAndRangeTerrainFusion) +{ + // WHEN: simulate being 5m above ground + // By default, both rng and flow aiding are active + const float simulated_distance_to_ground = 1.f; + _sensor_simulator._rng.setData(simulated_distance_to_ground, 100); + _sensor_simulator._rng.setLimits(0.1f, 9.f); + _sensor_simulator.startRangeFinder(); + _ekf->set_in_air_status(true); + _sensor_simulator.runSeconds(1.5f); + + // THEN: By default, both rng and flow aiding are active + EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainRngFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion()); + const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); + EXPECT_FLOAT_EQ(estimated_distance_to_ground, simulated_distance_to_ground); + + // WHEN: rng fusion is disabled + _ekf_wrapper.disableTerrainRngFusion(); + _sensor_simulator.runSeconds(0.2); + + // THEN: only rng fusion should be disabled + EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainRngFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion()); + + // WHEN: flow is now diabled + _ekf_wrapper.disableTerrainFlowFusion(); + _sensor_simulator.runSeconds(0.2); + + // THEN: flow is now also disabled + EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainRngFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainFlowFusion()); +} + +TEST_F(EkfTerrainTest, testFlowForTerrainFusion) +{ + // GIVEN: flow for terrain enabled but not range finder + _ekf_wrapper.enableTerrainFlowFusion(); + _ekf_wrapper.disableTerrainRngFusion(); + + // WHEN: the sensors do not agree + const float rng_height = 1.f; + const float flow_height = 5.f; + runFlowAndRngScenario(rng_height, flow_height); + + // THEN: the estimator should use flow for terrain and the estimated terrain height + // should converge to the simulated height + EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainRngFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion()); + + const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); + EXPECT_NEAR(estimated_distance_to_ground, flow_height, 0.5f); +} + +TEST_F(EkfTerrainTest, testRngForTerrainFusion) +{ + // GIVEN: rng for terrain but not flow + _ekf_wrapper.disableTerrainFlowFusion(); + _ekf_wrapper.enableTerrainRngFusion(); + + // WHEN: the sensors do not agree + const float rng_height = 1.f; + const float flow_height = 5.f; + runFlowAndRngScenario(rng_height, flow_height); + + // THEN: the estimator should use rng for terrain and the estimated terrain height + // should converge to the simulated height + EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainRngFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainFlowFusion()); + + const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); + EXPECT_NEAR(estimated_distance_to_ground, rng_height, 0.01f); +}