From 27f9b1b65a3acd876a091dd0169ac10cd67c067a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Oct 2023 14:47:27 -0400 Subject: [PATCH] ekf2: move zero gyro update to aid source class --- src/modules/ekf2/CMakeLists.txt | 3 +- src/modules/ekf2/EKF/CMakeLists.txt | 2 +- .../ZeroGyroUpdate.cpp} | 60 ++++++++-------- .../ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp | 54 ++++++++++++++ src/modules/ekf2/EKF/control.cpp | 4 +- src/modules/ekf2/EKF/ekf.h | 72 +++++++++---------- 6 files changed, 127 insertions(+), 68 deletions(-) rename src/modules/ekf2/EKF/{zero_gyro_update.cpp => aid_sources/ZeroGyroUpdate.cpp} (70%) create mode 100644 src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 30118b3672..2854fc3b0d 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -130,7 +130,8 @@ list(APPEND EKF_SRCS EKF/vel_pos_fusion.cpp EKF/yaw_fusion.cpp EKF/zero_innovation_heading_update.cpp - EKF/zero_gyro_update.cpp + + EKF/aid_sources/ZeroGyroUpdate.cpp EKF/aid_sources/ZeroVelocityUpdate.cpp ) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 6af5b891c7..cdb0a56ba5 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -47,8 +47,8 @@ list(APPEND EKF_SRCS vel_pos_fusion.cpp yaw_fusion.cpp zero_innovation_heading_update.cpp - zero_gyro_update.cpp + aid_sources/ZeroGyroUpdate.cpp aid_sources/ZeroVelocityUpdate.cpp ) diff --git a/src/modules/ekf2/EKF/zero_gyro_update.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp similarity index 70% rename from src/modules/ekf2/EKF/zero_gyro_update.cpp rename to src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp index 571bde209a..ea9d3f11a8 100644 --- a/src/modules/ekf2/EKF/zero_gyro_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp @@ -31,21 +31,25 @@ * ****************************************************************************/ -/** - * @file zero_gyro_update.cpp - * Control function for ekf zero gyro update - */ +#include "ZeroGyroUpdate.hpp" -#include "ekf.h" +#include "../ekf.h" -void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) +ZeroGyroUpdate::ZeroGyroUpdate() { - if (!(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias))) { - return; - } + reset(); +} +void ZeroGyroUpdate::reset() +{ + _zgup_delta_ang.setZero(); + _zgup_delta_ang_dt = 0.f; +} + +bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) +{ // When at rest, fuse the gyro data as a direct observation of the gyro bias - if (_control_status.flags.vehicle_at_rest) { + if (ekf.control_status_flags().vehicle_at_rest) { // Downsample gyro data to run the fusion at a lower rate _zgup_delta_ang += imu_delayed.delta_ang; _zgup_delta_ang_dt += imu_delayed.delta_ang_dt; @@ -54,38 +58,38 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) const bool zero_gyro_update_data_ready = _zgup_delta_ang_dt >= zgup_dt; if (zero_gyro_update_data_ready) { + Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt; - Vector3f innovation = _state.gyro_bias - gyro_bias; + Vector3f innovation = ekf.state().gyro_bias - gyro_bias; - const float obs_var = sq(math::constrain(_params.gyro_noise, 0.f, 1.f)); + const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f)); - const Vector3f innov_var = getGyroBiasVariance() + obs_var; + const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var; for (int i = 0; i < 3; i++) { - fuseDeltaAngBias(innovation(i), innov_var(i), i); + Ekf::VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. + const unsigned state_index = State::gyro_bias.idx + i; + + // calculate kalman gain K = PHS, where S = 1/innovation variance + for (int row = 0; row < State::size; row++) { + K(row) = ekf.stateCovariance(row, state_index) / innov_var(i); + } + + ekf.measurementUpdate(K, innov_var(i), innovation(i)); } // Reset the integrators _zgup_delta_ang.setZero(); _zgup_delta_ang_dt = 0.f; + + return true; } - } else if (_control_status_prev.flags.vehicle_at_rest) { + } else if (ekf.control_status_prev_flags().vehicle_at_rest) { // Reset the integrators _zgup_delta_ang.setZero(); _zgup_delta_ang_dt = 0.f; } -} - -void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index) -{ - VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = obs_index + State::gyro_bias.idx; - - // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < State::size; row++) { - K(row) = P(row, state_index) / innov_var; - } - - measurementUpdate(K, innov_var, innov); + + return false; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp new file mode 100644 index 0000000000..1ad488e539 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 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_GYRO_UPDATE_HPP +#define EKF_ZERO_GYRO_UPDATE_HPP + +#include "EstimatorAidSource.hpp" + +class ZeroGyroUpdate : public EstimatorAidSource +{ +public: + ZeroGyroUpdate(); + virtual ~ZeroGyroUpdate() = default; + + void reset() override; + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + +private: + + matrix::Vector3f _zgup_delta_ang{}; + float _zgup_delta_ang_dt{0.f}; +}; + +#endif // !EKF_ZERO_GYRO_UPDATE_HPP diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index d55ef16c2e..5c7aa5790b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -147,7 +147,9 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) _zero_velocity_update.update(*this, imu_delayed); - controlZeroGyroUpdate(imu_delayed); + if (_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)) { + _zero_gyro_update.update(*this, imu_delayed); + } // Fake position measurement for constraining drift when no other velocity or position measurements controlFakePosFusion(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 9f6e3828ea..4311417d36 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -59,6 +59,7 @@ #include #include +#include "aid_sources/ZeroGyroUpdate.hpp" #include "aid_sources/ZeroVelocityUpdate.hpp" enum class Likelihood { LOW, MEDIUM, HIGH }; @@ -249,6 +250,7 @@ public: // get the full covariance matrix const matrix::SquareMatrix &covariances() const { return P; } + float stateCovariance(unsigned r, unsigned c) const { return P(r, c); } // get the diagonal elements of the covariance matrix matrix::Vector covariances_diagonal() const { return P.diag(); } @@ -328,6 +330,7 @@ public: 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 float getGyroBiasLimit() const { return _params.gyro_bias_lim; } + float getGyroNoise() const { return _params.gyro_noise; } // accel bias const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2 @@ -473,6 +476,37 @@ public: const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL + + bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) + { + clearInhibitedStateKalmanGains(K); + + const VectorState KS = K * innovation_variance; + SquareMatrixState KHP; + + for (unsigned row = 0; row < State::size; row++) { + for (unsigned col = 0; col < State::size; col++) { + // Instad of literally computing KHP, use an equvalent + // equation involving less mathematical operations + KHP(row, col) = KS(row) * K(col); + } + } + + const bool is_healthy = checkAndFixCovarianceUpdate(KHP); + + if (is_healthy) { + // apply the covariance corrections + P -= KHP; + + fixCovarianceErrors(true); + + // apply the state corrections + fuse(K, innovation); + } + + return is_healthy; + } + private: // set the internal states and status to their default value @@ -530,10 +564,6 @@ private: Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction - // zero gyro update - Vector3f _zgup_delta_ang{}; - float _zgup_delta_ang_dt{0.f}; - Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) float _height_rate_lpf{0.0f}; float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad) @@ -917,36 +947,6 @@ private: #endif // CONFIG_EKF2_WIND } - bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) - { - clearInhibitedStateKalmanGains(K); - - const VectorState KS = K * innovation_variance; - SquareMatrixState KHP; - - for (unsigned row = 0; row < State::size; row++) { - for (unsigned col = 0; col < State::size; col++) { - // Instad of literally computing KHP, use an equvalent - // equation involving less mathematical operations - KHP(row, col) = KS(row) * K(col); - } - } - - const bool is_healthy = checkAndFixCovarianceUpdate(KHP); - - if (is_healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(K, innovation); - } - - return is_healthy; - } - // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP); @@ -1073,9 +1073,6 @@ private: void resetHeightToLastKnown(); void stopFakeHgtFusion(); - void controlZeroGyroUpdate(const imuSample &imu_delayed); - void fuseDeltaAngBias(float innov, float innov_var, int obs_index); - void controlZeroInnovationHeadingUpdate(); #if defined(CONFIG_EKF2_AUXVEL) @@ -1242,6 +1239,7 @@ private: status.innovation_rejected = innovation_rejected; } + ZeroGyroUpdate _zero_gyro_update{}; ZeroVelocityUpdate _zero_velocity_update{}; };