From 54c7e74de3dedd8ca5bc9b997b4f5aa7d0382e93 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 19 Jul 2021 14:36:40 +0200 Subject: [PATCH] EKF: add baro bias estimator using GNSS altitude --- msg/CMakeLists.txt | 1 + msg/baro_bias_estimate.msg | 10 ++ src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + src/modules/ekf2/EKF/baro_bias_estimator.cpp | 128 ++++++++++++++++++ src/modules/ekf2/EKF/baro_bias_estimator.hpp | 129 +++++++++++++++++++ src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/control.cpp | 4 +- src/modules/ekf2/EKF/ekf.h | 7 + src/modules/ekf2/EKF/ekf_helper.cpp | 36 +++++- src/modules/ekf2/EKF2.cpp | 17 +++ src/modules/ekf2/EKF2.hpp | 3 + src/modules/logger/logged_topics.cpp | 1 + 13 files changed, 337 insertions(+), 2 deletions(-) create mode 100644 msg/baro_bias_estimate.msg create mode 100644 src/modules/ekf2/EKF/baro_bias_estimator.cpp create mode 100644 src/modules/ekf2/EKF/baro_bias_estimator.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ce36da4b6c..518d233099 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -44,6 +44,7 @@ set(msg_files airspeed.msg airspeed_validated.msg airspeed_wind.msg + baro_bias_estimate.msg battery_status.msg camera_capture.msg camera_trigger.msg diff --git a/msg/baro_bias_estimate.msg b/msg/baro_bias_estimate.msg new file mode 100644 index 0000000000..52b807c610 --- /dev/null +++ b/msg/baro_bias_estimate.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 baro_device_id # unique device ID for the sensor that does not change between power cycles +float32 bias # estimated barometric altitude bias (m) +float32 bias_var # estimated barometric altitude bias variance (m^2) + +float32 innov # innovation of the last measurement fusion (m) +float32 innov_var # innovation variance of the last measurement fusion (m^2) +float32 innov_test_ratio # normalized innovation squared test ratio diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 9a54b1cbc2..ecb07d587e 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -45,6 +45,7 @@ px4_add_module( 3500 SRCS EKF/airspeed_fusion.cpp + EKF/baro_bias_estimator.cpp EKF/control.cpp EKF/covariance.cpp EKF/drag_fusion.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 4a47e3d3a0..daeb559a8f 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(ecl_EKF airspeed_fusion.cpp + baro_bias_estimator.cpp control.cpp covariance.cpp drag_fusion.cpp diff --git a/src/modules/ekf2/EKF/baro_bias_estimator.cpp b/src/modules/ekf2/EKF/baro_bias_estimator.cpp new file mode 100644 index 0000000000..f9625ae0d4 --- /dev/null +++ b/src/modules/ekf2/EKF/baro_bias_estimator.cpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2021 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. + * + ****************************************************************************/ + +/** + * @file baro_bias_estimator.cpp + * + * @author Mathieu Bresciani + */ + +#include "baro_bias_estimator.hpp" + +void BaroBiasEstimator::predict(const float dt) +{ + // State is constant + // Predict state covariance only + _state_var += _process_var * dt * dt; + constrainStateVar(); + + if (dt > FLT_EPSILON && fabsf(_dt - dt) > 0.001f) { + _signed_innov_test_ratio_lpf.setParameters(dt, _lpf_time_constant); + _dt = dt; + } +} + +void BaroBiasEstimator::constrainStateVar() +{ + _state_var = math::constrain(_state_var, 1e-8f, _state_var_max); +} + +void BaroBiasEstimator::fuseBias(const float measurement, const float measurement_var) +{ + const float innov_var = _state_var + measurement_var; + const float innov = measurement - _state; + const float K = _state_var / innov_var; + const float innov_test_ratio = computeInnovTestRatio(innov, innov_var); + + if (isTestRatioPassing(innov_test_ratio)) { + updateState(K, innov); + updateStateCovariance(K); + + } + + if (isLargeOffsetDetected()) { + // A bias in the state has been detected by the innovation + // sequence check. + bumpStateVariance(); + } + + const float signed_innov_test_ratio = matrix::sign(innov) * innov_test_ratio; + _signed_innov_test_ratio_lpf.update(math::constrain(signed_innov_test_ratio, -1.f, 1.f)); + + _status = packStatus(innov, innov_var, innov_test_ratio); +} + +inline float BaroBiasEstimator::computeInnovTestRatio(const float innov, const float innov_var) const +{ + return innov * innov / (_gate_size * _gate_size * innov_var); +} + +inline bool BaroBiasEstimator::isTestRatioPassing(const float innov_test_ratio) const +{ + return innov_test_ratio < 1.f; +} + +inline void BaroBiasEstimator::updateState(const float K, const float innov) +{ + _state = math::constrain(_state + K * innov, -_state_max, _state_max); +} + +inline void BaroBiasEstimator::updateStateCovariance(const float K) +{ + _state_var -= K * _state_var; + constrainStateVar(); +} + +inline bool BaroBiasEstimator::isLargeOffsetDetected() const +{ + return fabsf(_signed_innov_test_ratio_lpf.getState()) > 0.2f; +} + +inline void BaroBiasEstimator::bumpStateVariance() +{ + _state_var += _process_var_boost_gain * _process_var * _dt * _dt; +} + +inline BaroBiasEstimator::status BaroBiasEstimator::packStatus(const float innov, const float innov_var, + const float innov_test_ratio) const +{ + // Send back status for logging + status ret{}; + ret.bias = _state; + ret.bias_var = _state_var; + ret.innov = innov; + ret.innov_var = innov_var; + ret.innov_test_ratio = innov_test_ratio; + + return ret; +} diff --git a/src/modules/ekf2/EKF/baro_bias_estimator.hpp b/src/modules/ekf2/EKF/baro_bias_estimator.hpp new file mode 100644 index 0000000000..5280b542d0 --- /dev/null +++ b/src/modules/ekf2/EKF/baro_bias_estimator.hpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2021 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. + * + ****************************************************************************/ + +/** + * @file baro_bias_estimator.hpp + * @brief Implementation of a single-state baro bias estimator + * + * state: baro bias (in meters) + * + * The state is noise driven: Transition matrix A = 1 + * x[k+1] = Ax[k] + v with v ~ N(0, Q) + * y[k] = x[k] + w with w ~ N(0, R) + * + * The difference between the current barometric altitude and another absolute + * reference (e.g.: GNSS altitude) is used as a bias measurement. + * + * During the measurment update step, the Normalized Innovation Squared (NIS) is checked + * and the measurement is rejected if larger than the gate size. + * + * @author Mathieu Bresciani + */ + +#ifndef EKF_BARO_BIAS_ESTIMATOR_HPP +#define EKF_BARO_BIAS_ESTIMATOR_HPP + +#include +#include +#include + +class BaroBiasEstimator +{ +public: + struct status { + float bias; + float bias_var; + float innov; + float innov_var; + float innov_test_ratio; + }; + + BaroBiasEstimator() = default; + ~BaroBiasEstimator() = default; + + void predict(float dt); + void fuseBias(float measurement, float measurement_var); + + void setBias(float bias) { _state = bias; } + void setProcessNoiseStdDev(float process_noise) + { + _process_var = process_noise * process_noise; + _state_drift_rate = 3.f * process_noise; + } + void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; } + void setInnovGate(float gate_size) { _gate_size = gate_size; } + + void setMaxStateNoise(float max_noise) { _state_var_max = max_noise * max_noise; } + + float getBias() const { return _state; } + float getBiasVar() const { return _state_var; } + const status &getStatus() const { return _status; } + +private: + float _state{0.f}; + float _state_max{100.f}; + float _state_drift_rate{0.005f}; ///< in m/s + float _dt{0.01f}; + + float _gate_size{3.f}; ///< Used for innovation filtering (innovation test ratio) + float _state_var{0.1f}; ///< Initial state uncertainty variance (m^2) + float _process_var{25.0e-6f}; ///< State process noise variance (m^2/s^2) + float _state_var_max{2.f}; ///< Used to constrain the state variance (m^2) + + AlphaFilter _signed_innov_test_ratio_lpf; ///< innovation sequence monitoring; used to detect a bias in the state + + status _status; + + void constrainStateVar(); + float computeKalmanGain(float innov_var) const; + + /* + * Compute the ratio between the Normalized Innovation Squared (NIS) + * and its maximum gate size. Use isTestRatioPassing to know if the + * measurement should be fused or not. + */ + float computeInnovTestRatio(float innov, float innov_var) const; + bool isTestRatioPassing(float innov_test_ratio) const; + + void updateState(float K, float innov); + void updateStateCovariance(float K); + + bool isLargeOffsetDetected() const; + void bumpStateVariance(); + + status packStatus(float innov, float innov_var, float innov_test_ratio) const; + + static constexpr float _lpf_time_constant{10.f}; ///< in seconds + static constexpr float _process_var_boost_gain{1.0e3f}; +}; +#endif // !EKF_BARO_BIAS_ESTIMATOR_HPP diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 5b18bafc7d..a70bc9bc8c 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -257,6 +257,7 @@ struct parameters { float gps_pos_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m) float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m) float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) + float baro_drift_rate{0.005f}; ///< process noise for barometric height bias estimation (m/s) float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) float gps_pos_innov_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD) float gps_vel_innov_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index a74cfbebe5..3413991f41 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1161,6 +1161,7 @@ void Ekf::controlHeightFusion() break; } + updateBaroHgtBias(); updateBaroHgtOffset(); if (_control_status.flags.rng_hgt @@ -1184,7 +1185,8 @@ void Ekf::controlHeightFusion() Vector3f baro_hgt_obs_var; // vertical position innovation - baro measurement has opposite sign to earth z axis - _baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset; + const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias(); + _baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset; // observation variance - user parameter defined baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f)); // innovation gate size diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b89699ab23..ed050448fc 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -46,6 +46,7 @@ #include "estimator_interface.h" #include "EKFGSF_yaw.h" +#include "baro_bias_estimator.hpp" class Ekf final : public EstimatorInterface { @@ -297,6 +298,7 @@ public: // returns false when data is not available bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); + void getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio); private: @@ -500,6 +502,8 @@ private: AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m) float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m) + float _baro_hgt_bias{0.0f}; + float _baro_hgt_bias_var{1.f}; // Variables used to control activation of post takeoff functionality float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) @@ -890,6 +894,7 @@ private: void startGpsHgtFusion(); void updateBaroHgtOffset(); + void updateBaroHgtBias(); // return an estimation of the GPS altitude variance float getGpsHeightVariance(); @@ -983,6 +988,8 @@ private: // yaw estimator instance EKFGSF_yaw _yawEstimator{}; + BaroBiasEstimator _baro_b_est{}; + int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested uint8_t _ekfgsf_yaw_reset_count{0}; // number of times the yaw has been reset to the EKF-GSF estimate diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index dfb513396f..cac4dccfe4 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1319,7 +1319,10 @@ void Ekf::updateBaroHgtOffset() const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f); // apply a 10 second first order low pass filter to baro offset - const float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(2) - _baro_hgt_offset); + const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias(); +; + const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - + _baro_hgt_offset); _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); } } @@ -1334,6 +1337,37 @@ float Ekf::getGpsHeightVariance() return gps_alt_var; } +void Ekf::updateBaroHgtBias() +{ + // Baro bias estimation using GPS altitude + if (_baro_data_ready) { + const float dt = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f); + _baro_b_est.setMaxStateNoise(_params.baro_noise); + _baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate); + _baro_b_est.predict(dt); + } + + if (_gps_data_ready && !_gps_hgt_intermittent + && _gps_checks_passed &&_NED_origin_initialised + && !_baro_hgt_faulty) { + // Use GPS altitude as a reference to compute the baro bias measurement + const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset) + - (_gps_sample_delayed.hgt - _gps_alt_ref); + const float baro_bias_var = getGpsHeightVariance() + sq(_params.baro_noise); + _baro_b_est.fuseBias(baro_bias, baro_bias_var); + } +} + +void Ekf::getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio) +{ + BaroBiasEstimator::status status = _baro_b_est.getStatus(); + bias = status.bias; + bias_var = status.bias_var; + innov = status.innov; + innov_var = status.innov_var; + innov_test_ratio = status.innov_test_ratio; +} + Vector3f Ekf::getVisionVelocityInEkfFrame() const { Vector3f vel; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 96d96380d7..3402612212 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -202,6 +202,7 @@ bool EKF2::multi_init(int imu, int mag) _estimator_status_flags_pub.advertise(); _estimator_visual_odometry_aligned_pub.advertised(); _yaw_est_pub.advertise(); + _baro_bias_estimate_pub.advertise(); bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag); @@ -505,6 +506,7 @@ void EKF2::Run() PublishInnovationTestRatios(now); PublishInnovationVariances(now); PublishYawEstimatorStatus(now); + PublishBaroBiasEstimate(now); UpdateMagCalibration(now); @@ -548,6 +550,21 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) } } +void EKF2::PublishBaroBiasEstimate(const hrt_abstime ×tamp) +{ + + baro_bias_estimate_s bbe{}; + bbe.timestamp = timestamp; + bbe.timestamp_sample = timestamp; + bbe.baro_device_id = _device_id_baro; + _ekf.getBaroBiasEstimatorStatus(bbe.bias, + bbe.bias_var, + bbe.innov, + bbe.innov_var, + bbe.innov_test_ratio); + _baro_bias_estimate_pub.publish(bbe); +} + void EKF2::PublishEkfDriftMetrics(const hrt_abstime ×tamp) { // publish GPS drift data only when updated to minimise overhead diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 8859c2d830..d926ac9ce4 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -62,6 +62,7 @@ #include #include #include +#include #include #include #include @@ -129,6 +130,7 @@ private: void Run() override; void PublishAttitude(const hrt_abstime ×tamp); + void PublishBaroBiasEstimate(const hrt_abstime ×tamp); void PublishEkfDriftMetrics(const hrt_abstime ×tamp); void PublishEventFlags(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); @@ -256,6 +258,7 @@ private: uint32_t _filter_warning_event_changes{0}; uint32_t _filter_information_event_changes{0}; + uORB::PublicationMulti _baro_bias_estimate_pub{ORB_ID(baro_bias_estimate)}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index cd1f071c32..4ef39092e5 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -137,6 +137,7 @@ void LoggedTopics::add_default_topics() #else static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed add_topic("estimator_selector_status"); + add_topic_multi("baro_bias_estimate", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES);