diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 8cf8940ce1..178e6f21d7 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -48,7 +48,7 @@ px4_add_module( 3600 SRCS EKF/airspeed_fusion.cpp - EKF/baro_bias_estimator.cpp + EKF/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 f03d9c4f48..bd04a01dd3 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -33,7 +33,7 @@ add_library(ecl_EKF airspeed_fusion.cpp - baro_bias_estimator.cpp + 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/bias_estimator.cpp similarity index 83% rename from src/modules/ekf2/EKF/baro_bias_estimator.cpp rename to src/modules/ekf2/EKF/bias_estimator.cpp index 26e4a754a5..389b3e9b18 100644 --- a/src/modules/ekf2/EKF/baro_bias_estimator.cpp +++ b/src/modules/ekf2/EKF/bias_estimator.cpp @@ -32,14 +32,14 @@ ****************************************************************************/ /** - * @file baro_bias_estimator.cpp + * @file bias_estimator.cpp * * @author Mathieu Bresciani */ -#include "baro_bias_estimator.hpp" +#include "bias_estimator.hpp" -void BaroBiasEstimator::predict(const float dt) +void BiasEstimator::predict(const float dt) { // State is constant // Predict state covariance only @@ -62,12 +62,12 @@ void BaroBiasEstimator::predict(const float dt) _status.bias_var = _state_var; } -void BaroBiasEstimator::constrainStateVar() +void BiasEstimator::constrainStateVar() { _state_var = math::constrain(_state_var, 1e-8f, _state_var_max); } -void BaroBiasEstimator::fuseBias(const float measurement, const float measurement_var) +void BiasEstimator::fuseBias(const float measurement, const float measurement_var) { const float innov_var = _state_var + measurement_var; const float innov = measurement - _state; @@ -85,28 +85,28 @@ void BaroBiasEstimator::fuseBias(const float measurement, const float measuremen _status = packStatus(innov, innov_var, innov_test_ratio); } -inline float BaroBiasEstimator::computeInnovTestRatio(const float innov, const float innov_var) const +inline float BiasEstimator::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 +inline bool BiasEstimator::isTestRatioPassing(const float innov_test_ratio) const { return innov_test_ratio < 1.f; } -inline void BaroBiasEstimator::updateState(const float K, const float innov) +inline void BiasEstimator::updateState(const float K, const float innov) { _state = math::constrain(_state + K * innov, -_state_max, _state_max); } -inline void BaroBiasEstimator::updateStateCovariance(const float K) +inline void BiasEstimator::updateStateCovariance(const float K) { _state_var -= K * _state_var; constrainStateVar(); } -inline void BaroBiasEstimator::updateOffsetDetection(const float innov, const float innov_test_ratio) +inline void BiasEstimator::updateOffsetDetection(const float innov, const float innov_test_ratio) { 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)); @@ -121,7 +121,7 @@ inline void BaroBiasEstimator::updateOffsetDetection(const float innov, const fl } } -inline bool BaroBiasEstimator::isOffsetDetected() const +inline bool BiasEstimator::isOffsetDetected() const { // There is an offset in the estimate if the average of innovation is statistically too large // or if the sign of the innovation is constantly the same @@ -130,7 +130,7 @@ inline bool BaroBiasEstimator::isOffsetDetected() const || (_time_since_last_negative_innov > _innov_sequence_monitnoring_time_constant); } -inline BaroBiasEstimator::status BaroBiasEstimator::packStatus(const float innov, const float innov_var, +inline BiasEstimator::status BiasEstimator::packStatus(const float innov, const float innov_var, const float innov_test_ratio) const { // Send back status for logging diff --git a/src/modules/ekf2/EKF/baro_bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator.hpp similarity index 90% rename from src/modules/ekf2/EKF/baro_bias_estimator.hpp rename to src/modules/ekf2/EKF/bias_estimator.hpp index e559b0a544..faf8ae720d 100644 --- a/src/modules/ekf2/EKF/baro_bias_estimator.hpp +++ b/src/modules/ekf2/EKF/bias_estimator.hpp @@ -32,32 +32,28 @@ ****************************************************************************/ /** - * @file baro_bias_estimator.hpp - * @brief Implementation of a single-state baro bias estimator + * @file bias_estimator.hpp + * @brief Implementation of a single-state bias estimator * - * state: baro bias (in meters) + * state: bias * * 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 +#pragma once #include #include #include -class BaroBiasEstimator +class BiasEstimator { public: struct status { @@ -68,8 +64,8 @@ public: float innov_test_ratio; }; - BaroBiasEstimator() = default; - ~BaroBiasEstimator() = default; + BiasEstimator() = default; + ~BiasEstimator() = default; void predict(float dt); void fuseBias(float measurement, float measurement_var); @@ -127,4 +123,3 @@ private: static constexpr float _innov_sequence_monitnoring_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/ekf.h b/src/modules/ekf2/EKF/ekf.h index c4e99ff9b9..1c92bd8d24 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -46,7 +46,7 @@ #include "estimator_interface.h" #include "EKFGSF_yaw.h" -#include "baro_bias_estimator.hpp" +#include "bias_estimator.hpp" #include #include @@ -345,7 +345,7 @@ public: // Returns true if the output of the yaw emergency estimator can be used for a reset bool isYawEmergencyEstimateAvailable() const; - const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } + const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } const auto &aid_src_airspeed() const { return _aid_src_airspeed; } @@ -1042,7 +1042,7 @@ private: // yaw estimator instance EKFGSF_yaw _yawEstimator{}; - BaroBiasEstimator _baro_b_est{}; + BiasEstimator _baro_b_est{}; int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) 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/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c98cfe3035..f0778fad8a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -680,7 +680,7 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) void EKF2::PublishBaroBias(const hrt_abstime ×tamp) { if (_device_id_baro != 0) { - const BaroBiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus(); + const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus(); if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) { estimator_baro_bias_s baro_bias{};