From e79737a38da1b1293a6fcc4da6193f72874b5e66 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Oct 2023 14:19:48 -0400 Subject: [PATCH] ekf2: create simple estimator aid source base class and extract zero velocity update --- src/modules/ekf2/CMakeLists.txt | 3 +- src/modules/ekf2/EKF/CMakeLists.txt | 3 +- .../EKF/aid_sources/EstimatorAidSource.hpp | 64 +++++++++++++++++++ .../ZeroVelocityUpdate.cpp} | 47 +++++++++----- .../EKF/aid_sources/ZeroVelocityUpdate.hpp | 54 ++++++++++++++++ src/modules/ekf2/EKF/control.cpp | 3 +- src/modules/ekf2/EKF/ekf.cpp | 3 +- src/modules/ekf2/EKF/ekf.h | 14 ++-- 8 files changed, 165 insertions(+), 26 deletions(-) create mode 100644 src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp rename src/modules/ekf2/EKF/{zero_velocity_update.cpp => aid_sources/ZeroVelocityUpdate.cpp} (63%) create mode 100644 src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 37fc6c005b..30118b3672 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -131,7 +131,7 @@ list(APPEND EKF_SRCS EKF/yaw_fusion.cpp EKF/zero_innovation_heading_update.cpp EKF/zero_gyro_update.cpp - EKF/zero_velocity_update.cpp + EKF/aid_sources/ZeroVelocityUpdate.cpp ) if(CONFIG_EKF2_AIRSPEED) @@ -220,6 +220,7 @@ px4_add_module( #-O0 INCLUDES EKF + EKF/aid_sources ${EKF_GENERATED_DERIVATION_INCLUDE_PATH} PRIORITY "SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 60f9d10c5d..6af5b891c7 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -48,7 +48,8 @@ list(APPEND EKF_SRCS yaw_fusion.cpp zero_innovation_heading_update.cpp zero_gyro_update.cpp - zero_velocity_update.cpp + + aid_sources/ZeroVelocityUpdate.cpp ) if(CONFIG_EKF2_AIRSPEED) diff --git a/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp b/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp new file mode 100644 index 0000000000..6d6c9e8ec2 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2023 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_ESTIMATOR_AID_SOURCE_HPP +#define EKF_ESTIMATOR_AID_SOURCE_HPP + +#include + +#include + +// forward declarations +class Ekf; + +namespace estimator +{ +struct imuSample; +}; + +class EstimatorAidSource +{ +public: + EstimatorAidSource() = default; + virtual ~EstimatorAidSource() = default; + + virtual bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) = 0; + + virtual void reset() = 0; + +private: + + +}; + +#endif // !EKF_ESTIMATOR_AID_SOURCE_HPP diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp similarity index 63% rename from src/modules/ekf2/EKF/zero_velocity_update.cpp rename to src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp index f78dc28cc2..d091e5aadb 100644 --- a/src/modules/ekf2/EKF/zero_velocity_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 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 @@ -31,37 +31,50 @@ * ****************************************************************************/ -/** - * @file zero_velocity_update.cpp - * Control function for ekf zero velocity update - */ +#include "ZeroVelocityUpdate.hpp" -#include "ekf.h" +#include "../ekf.h" -void Ekf::controlZeroVelocityUpdate() +ZeroVelocityUpdate::ZeroVelocityUpdate() +{ + reset(); +} + +void ZeroVelocityUpdate::reset() +{ + _time_last_zero_velocity_fuse = 0; +} + +bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { // Fuse zero velocity at a limited rate (every 200 milliseconds) - const bool zero_velocity_update_data_ready = isTimedOut(_time_last_zero_velocity_fuse, (uint64_t)2e5); + const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us); if (zero_velocity_update_data_ready) { - const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest - && _control_status_prev.flags.vehicle_at_rest - && (!isVerticalVelocityAidingActive() || !_control_status.flags.tilt_align); // otherwise the filter is "too rigid" to follow a position drift + const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest + && ekf.control_status_prev_flags().vehicle_at_rest + && (!ekf.isVerticalVelocityAidingActive() + || !ekf.control_status_flags().tilt_align); // otherwise the filter is "too rigid" to follow a position drift if (continuing_conditions_passing) { - Vector3f vel_obs{0, 0, 0}; - Vector3f innovation = _state.vel - vel_obs; + Vector3f vel_obs{0.f, 0.f, 0.f}; + Vector3f innovation = ekf.state().vel - vel_obs; // Set a low variance initially for faster leveling and higher // later to let the states follow the measurements - const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f); - Vector3f innov_var = getVelocityVariance() + obs_var; + + const float obs_var = ekf.control_status_flags().tilt_align ? sq(0.2f) : sq(0.001f); + Vector3f innov_var = ekf.getVelocityVariance() + obs_var; for (unsigned i = 0; i < 3; i++) { - fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); + ekf.fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); } - _time_last_zero_velocity_fuse = _time_delayed_us; + _time_last_zero_velocity_fuse = imu_delayed.time_us; + + return true; } } + + return false; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp new file mode 100644 index 0000000000..0016c584c6 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2022-2023 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_ZERO_VELOCITY_UPDATE_HPP +#define EKF_ZERO_VELOCITY_UPDATE_HPP + +#include "EstimatorAidSource.hpp" + +class ZeroVelocityUpdate : public EstimatorAidSource +{ +public: + ZeroVelocityUpdate(); + virtual ~ZeroVelocityUpdate() = default; + + void reset() override; + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + +private: + + uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) + +}; + +#endif // !EKF_ZERO_VELOCITY_UPDATE_HPP diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 91acf6660b..d55ef16c2e 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -145,7 +145,8 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlZeroInnovationHeadingUpdate(); - controlZeroVelocityUpdate(); + _zero_velocity_update.update(*this, imu_delayed); + controlZeroGyroUpdate(imu_delayed); // Fake position measurement for constraining drift when no other velocity or position measurements diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f1f5204e98..454b29eb70 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -110,7 +110,6 @@ void Ekf::reset() _time_last_hor_vel_fuse = 0; _time_last_ver_vel_fuse = 0; _time_last_heading_fuse = 0; - _time_last_zero_velocity_fuse = 0; _last_known_pos.setZero(); @@ -175,6 +174,8 @@ void Ekf::reset() #if defined(CONFIG_EKF2_RANGE_FINDER) resetEstimatorAidStatus(_aid_src_rng_hgt); #endif // CONFIG_EKF2_RANGE_FINDER + + _zero_velocity_update.reset(); } bool Ekf::update() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index fbe782f22e..9f6e3828ea 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -59,6 +59,8 @@ #include #include +#include "aid_sources/ZeroVelocityUpdate.hpp" + enum class Likelihood { LOW, MEDIUM, HIGH }; class Ekf final : public EstimatorInterface @@ -83,6 +85,8 @@ public: static uint8_t getNumberOfStates() { return State::size; } + const StateSample &state() const { return _state; } + #if defined(CONFIG_EKF2_BAROMETER) const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } @@ -317,6 +321,9 @@ public: #endif } + // fuse single velocity and position measurement + bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); + // gyro bias const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s Vector3f getGyroBiasVariance() const { return getStateVariance(); } // get the gyroscope bias variance in rad/s @@ -514,7 +521,6 @@ private: uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) uint64_t _time_last_heading_fuse{0}; - uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) Vector3f _last_known_pos{}; ///< last known local position vector (m) @@ -774,9 +780,6 @@ private: void fuseDrag(const dragSample &drag_sample); #endif // CONFIG_EKF2_DRAG_FUSION - // fuse single velocity and position measurement - bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); - void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var); void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var); @@ -1070,7 +1073,6 @@ private: void resetHeightToLastKnown(); void stopFakeHgtFusion(); - void controlZeroVelocityUpdate(); void controlZeroGyroUpdate(const imuSample &imu_delayed); void fuseDeltaAngBias(float innov, float innov_var, int obs_index); @@ -1239,6 +1241,8 @@ private: // if any of the innovations are rejected, then the overall innovation is rejected status.innovation_rejected = innovation_rejected; } + + ZeroVelocityUpdate _zero_velocity_update{}; }; #endif // !EKF_EKF_H