From 1a0f97ebbd67870d4686ee7ebb6d38e11662eabc Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 19 Aug 2024 15:36:04 +0200 Subject: [PATCH] ekf2-fake pos: add valid fake position fusion This is similar to fake pos but is only used when the ekf has an external information telling it that the vehicle is not changing position. This information can then be used to keep a valid local position even when the vehicle isn't exactly at rest. --- msg/EstimatorStatusFlags.msg | 2 + .../ekf2/EKF/aid_sources/fake_pos_control.cpp | 98 +++++++------- src/modules/ekf2/EKF/common.h | 3 + src/modules/ekf2/EKF/control.cpp | 2 + src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF/ekf_helper.cpp | 10 +- src/modules/ekf2/EKF/estimator_interface.h | 2 + src/modules/ekf2/EKF2.cpp | 11 ++ src/modules/ekf2/EKF2.hpp | 2 + src/modules/ekf2/test/CMakeLists.txt | 1 + .../ekf2/test/sensor_simulator/airspeed.cpp | 17 ++- src/modules/ekf2/test/test_EKF_airspeed.cpp | 4 +- src/modules/ekf2/test/test_EKF_fake_pos.cpp | 121 ++++++++++++++++++ 13 files changed, 215 insertions(+), 60 deletions(-) create mode 100644 src/modules/ekf2/test/test_EKF_fake_pos.cpp diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 23e6e15fd4..79b900f6a8 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -45,6 +45,8 @@ bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag d bool cs_aux_gpos # 38 - true if auxiliary global position measurement fusion is intended bool cs_rng_terrain # 39 - true if we are fusing range finder data for terrain bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terrain +bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused +bool cs_constant_pos # 42 - true if the vehicle is at a constant position # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index df7257d9b0..b87e14654f 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -75,55 +75,20 @@ void Ekf::controlFakePosFusion() Vector2f(getStateVariance()) + obs_var, // innovation variance innov_gate); // innovation gate - const bool enable_conditions_passing = !isHorizontalAidingActive() - && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest) - && (!(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest) - && _horizontal_deadreckon_time_exceeded; + const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest; + const bool enable_fake_pos = !enable_valid_fake_pos + && (getTiltVariance() > sq(math::radians(3.f))) + && !(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) + && _horizontal_deadreckon_time_exceeded; - if (_control_status.flags.fake_pos) { - if (enable_conditions_passing) { + _control_status.flags.fake_pos = runFakePosStateMachine(enable_fake_pos, _control_status.flags.fake_pos, aid_src); + _control_status.flags.valid_fake_pos = runFakePosStateMachine(enable_valid_fake_pos, + _control_status.flags.valid_fake_pos, aid_src); - // always protect against extreme values that could result in a NaN - if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate)) - && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)) - ) { - if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], - State::pos.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], - State::pos.idx + 1) - ) { - aid_src.fused = true; - aid_src.time_last_fuse = _time_delayed_us; - } - } - - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); - - if (is_fusion_failing) { - ECL_WARN("fake position fusion failing, resetting"); - resetFakePosFusion(); - } - - } else { - stopFakePosFusion(); - } - - } else { - if (enable_conditions_passing) { - ECL_INFO("start fake position fusion"); - _control_status.flags.fake_pos = true; - resetFakePosFusion(); - - if (_control_status.flags.tilt_align) { - // The fake position fusion is not started for initial alignement - ECL_WARN("stopping navigation"); - } - } - } - - } else if (_control_status.flags.fake_pos && isHorizontalAidingActive()) { - stopFakePosFusion(); + } else if ((_control_status.flags.fake_pos || _control_status.flags.valid_fake_pos) && isHorizontalAidingActive()) { + ECL_INFO("stop fake position fusion"); + _control_status.flags.fake_pos = false; + _control_status.flags.valid_fake_pos = false; } } @@ -138,10 +103,41 @@ void Ekf::resetFakePosFusion() _aid_src_fake_pos.time_last_fuse = _time_delayed_us; } -void Ekf::stopFakePosFusion() +bool Ekf::runFakePosStateMachine(const bool enable_conditions_passing, bool status_flag, + estimator_aid_source2d_s &aid_src) { - if (_control_status.flags.fake_pos) { - ECL_INFO("stop fake position fusion"); - _control_status.flags.fake_pos = false; + if (status_flag) { + if (enable_conditions_passing) { + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], + State::pos.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], + State::pos.idx + 1) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); + + if (is_fusion_failing) { + ECL_WARN("fake position fusion failing, resetting"); + resetFakePosFusion(); + } + + } else { + ECL_INFO("stop fake position fusion"); + status_flag = false; + } + + } else { + if (enable_conditions_passing) { + ECL_INFO("start fake position fusion"); + status_flag = true; + + resetFakePosFusion(); + } } + + return status_flag; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index af03d2a8db..13c2767f75 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -261,6 +261,7 @@ struct systemFlagUpdate { bool in_air{true}; bool is_fixed_wing{false}; bool gnd_effect{false}; + bool constant_pos{false}; }; struct parameters { @@ -614,6 +615,8 @@ uint64_t mag_heading_consistent : uint64_t aux_gpos : 1; ///< 38 - true if auxiliary global position measurement fusion is intended uint64_t rng_terrain : 1; ///< 39 - true if we are fusing range finder data for terrain uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain + uint64_t valid_fake_pos : 1; ///< 41 - true if a valid constant position is being fused + uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position } flags; uint64_t value; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index b8aca027e7..ceefc01f6f 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -62,6 +62,8 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) if (system_flags_delayed.gnd_effect) { set_gnd_effect(); } + + set_constant_pos(system_flags_delayed.constant_pos); } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index d9ff978e20..e6c182171a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1132,7 +1132,7 @@ private: } void resetFakePosFusion(); - void stopFakePosFusion(); + bool runFakePosStateMachine(bool enable_condition_passing, bool status_flag, estimator_aid_source2d_s &aid_src); // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch // yaw : Euler yaw angle (rad) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ae3e9e355e..e88d3b483a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -642,7 +642,8 @@ uint16_t Ekf::get_ekf_soln_status() const #endif // CONFIG_EKF2_TERRAIN // 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) - soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.vehicle_at_rest; + soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.valid_fake_pos + || _control_status.flags.vehicle_at_rest; // 256 ESTIMATOR_PRED_POS_HORIZ_REL True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive(); @@ -793,6 +794,13 @@ void Ekf::updateHorizontalDeadReckoningstatus() } } + if (_control_status.flags.valid_fake_pos && isRecent(_aid_src_fake_pos.time_last_fuse, _params.no_aid_timeout_max)) { + // only respect as a valid aiding source now if we expect to have another valid source once in air + if (aiding_expected_in_air) { + inertial_dead_reckoning = false; + } + } + if (inertial_dead_reckoning) { if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) { // deadreckon time exceeded diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 75e6526830..57e6fc06b0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -179,6 +179,8 @@ public: _control_status.flags.vehicle_at_rest = at_rest; } + void set_constant_pos(bool constant_pos) { _control_status.flags.constant_pos = constant_pos; } + // return true if the attitude is usable bool attitude_valid() const { return _control_status.flags.tilt_align; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index af2eb6615f..bb986208de 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1925,6 +1925,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_aux_gpos = _ekf.control_status_flags().aux_gpos; status_flags.cs_rng_terrain = _ekf.control_status_flags().rng_terrain; status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain; + status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos; + status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; @@ -2592,6 +2594,15 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) flags.gnd_effect = vehicle_land_detected.in_ground_effect; } + launch_detection_status_s launch_detection_status; + + if (_launch_detection_status_sub.copy(&launch_detection_status) + && (ekf2_timestamps.timestamp < launch_detection_status.timestamp + 3_s)) { + + flags.constant_pos = (launch_detection_status.launch_detection_state == + launch_detection_status_s::STATE_WAITING_FOR_LAUNCH); + } + _ekf.setSystemFlagData(flags); } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index ef7280e88c..867cf9ae57 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -388,6 +389,7 @@ private: uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 800b568d96..c1ecb992b9 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_si 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) +px4_add_unit_gtest(SRC test_EKF_fake_pos.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_flow_generated.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/sensor_simulator/airspeed.cpp b/src/modules/ekf2/test/sensor_simulator/airspeed.cpp index 520aca024e..67d89224d2 100644 --- a/src/modules/ekf2/test/sensor_simulator/airspeed.cpp +++ b/src/modules/ekf2/test/sensor_simulator/airspeed.cpp @@ -15,13 +15,18 @@ Airspeed::~Airspeed() void Airspeed::send(uint64_t time) { - if (_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON) { - airspeedSample airspeed_sample; - airspeed_sample.time_us = time; - airspeed_sample.eas2tas = _true_airspeed_data / _indicated_airspeed_data; - airspeed_sample.true_airspeed = _true_airspeed_data; - _ekf->setAirspeedData(airspeed_sample); + float ias2tas = 1.f; + + if (PX4_ISFINITE(_indicated_airspeed_data) + && (_indicated_airspeed_data > FLT_EPSILON)) { + ias2tas = _true_airspeed_data / _indicated_airspeed_data; } + + airspeedSample airspeed_sample; + airspeed_sample.time_us = time; + airspeed_sample.eas2tas = ias2tas; + airspeed_sample.true_airspeed = _true_airspeed_data; + _ekf->setAirspeedData(airspeed_sample); } void Airspeed::setData(float true_airspeed, float indicated_airspeed) diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index d284b959e3..61865dfc7a 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -78,7 +78,6 @@ public: TEST_F(EkfAirspeedTest, testWindVelocityEstimation) { - const Vector3f simulated_velocity_earth(0.0f, 1.5f, 0.0f); const Vector2f airspeed_body(2.4f, 0.0f); _ekf_wrapper.enableExternalVisionVelocityFusion(); @@ -86,6 +85,9 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation) _sensor_simulator._vio.setVelocityFrameToLocalNED(); _sensor_simulator.startExternalVision(); + // Let the EV fusion start first to reset the velocity estimate + _sensor_simulator.runSeconds(0.5); + _ekf->set_in_air_status(true); _ekf->set_vehicle_at_rest(false); _ekf->set_is_fixed_wing(true); diff --git a/src/modules/ekf2/test/test_EKF_fake_pos.cpp b/src/modules/ekf2/test/test_EKF_fake_pos.cpp new file mode 100644 index 0000000000..3cd755a3a5 --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_fake_pos.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" + + +class EkfFakePosTest : public ::testing::Test +{ +public: + + EkfFakePosTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf), + _quat_sim(Eulerf(0.0f, 0.0f, math::radians(45.0f))) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + const Quatf _quat_sim; + + // Setup the Ekf with synthetic measurements + void SetUp() override + { + // run briefly to init, then manually set in air and at rest (default for a real vehicle) + _ekf->init(0); + _sensor_simulator.runSeconds(0.1); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); + + _sensor_simulator.simulateOrientation(_quat_sim); + _sensor_simulator.runSeconds(7); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } +}; + +TEST_F(EkfFakePosTest, testValidFakePos) +{ + _ekf->set_vehicle_at_rest(false); + _ekf->set_constant_pos(true); + _sensor_simulator.runSeconds(1); + + EXPECT_EQ(1, (int) _ekf->control_status_flags().constant_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fake_pos); + EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos); +} + +TEST_F(EkfFakePosTest, testFakePosStopGnss) +{ + _ekf->set_vehicle_at_rest(false); + _ekf->set_constant_pos(true); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(12); + + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + EXPECT_EQ(1, (int) _ekf->control_status_flags().constant_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fake_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().valid_fake_pos); +} + +TEST_F(EkfFakePosTest, testValidFakePosValidLocalPos) +{ + _ekf->set_is_fixed_wing(true); + _sensor_simulator.startAirspeedSensor(); + _sensor_simulator._airspeed.setData(-0.01f, 0.f); // airspeed close to 0 + _ekf_wrapper.enableBetaFusion(); + + // WHEN: the vehicle is not as rest but is known to be at a constant position + _ekf->set_vehicle_at_rest(false); + _ekf->set_constant_pos(true); + _sensor_simulator.runSeconds(1); + + // THEN: the valid fake position is fused + EXPECT_EQ(1, (int) _ekf->control_status_flags().constant_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fake_pos); + EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos); + + // AND: since airspeed is expected to provide wind-relative dead-reckoning after takeoff + // the local position is considered valid + _sensor_simulator.runSeconds(60); + + EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos); + EXPECT_TRUE(_ekf->local_position_is_valid()); +}