forked from Archive/PX4-Autopilot
ekf2: rename BaroBiasEstimator -> BiasEstimator
This commit is contained in:
parent
39453405a0
commit
f962399ba1
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -32,14 +32,14 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file baro_bias_estimator.cpp
|
||||
* @file bias_estimator.cpp
|
||||
*
|
||||
* @author Mathieu Bresciani <mathieu@auterion.com>
|
||||
*/
|
||||
|
||||
#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
|
|
@ -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 <mathieu@auterion.com>
|
||||
*/
|
||||
|
||||
#ifndef EKF_BARO_BIAS_ESTIMATOR_HPP
|
||||
#define EKF_BARO_BIAS_ESTIMATOR_HPP
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mathlib/math/filter/AlphaFilter.hpp>
|
||||
|
||||
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
|
|
@ -46,7 +46,7 @@
|
|||
#include "estimator_interface.h"
|
||||
|
||||
#include "EKFGSF_yaw.h"
|
||||
#include "baro_bias_estimator.hpp"
|
||||
#include "bias_estimator.hpp"
|
||||
|
||||
#include <uORB/topics/estimator_aid_source_1d.h>
|
||||
#include <uORB/topics/estimator_aid_source_2d.h>
|
||||
|
@ -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
|
||||
|
|
|
@ -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{};
|
||||
|
|
Loading…
Reference in New Issue