From 52f726c5b75a7004c6583812037adf7a6df039b2 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 5 Aug 2022 10:21:00 +0200 Subject: [PATCH] ekf2: in bias estimator, use psd instead of var for prediction PSD is independent from the sampling time while variance isn't --- src/modules/ekf2/EKF/bias_estimator.cpp | 2 +- src/modules/ekf2/EKF/bias_estimator.hpp | 6 +++--- src/modules/ekf2/EKF/common.h | 5 ++++- src/modules/ekf2/EKF/gps_fusion.cpp | 18 ++++++++---------- src/modules/ekf2/EKF/height_fusion.cpp | 9 ++++----- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/modules/ekf2/EKF/bias_estimator.cpp b/src/modules/ekf2/EKF/bias_estimator.cpp index 545b969d16..a691121b28 100644 --- a/src/modules/ekf2/EKF/bias_estimator.cpp +++ b/src/modules/ekf2/EKF/bias_estimator.cpp @@ -43,7 +43,7 @@ void BiasEstimator::predict(const float dt) { // State is constant // Predict state covariance only - float delta_state_var = _process_var * dt * dt; + float delta_state_var = _process_psd * dt; if (isOffsetDetected()) { // A bias in the state has been detected by the innovation sequence check diff --git a/src/modules/ekf2/EKF/bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator.hpp index 637efc14a8..b534a67a11 100644 --- a/src/modules/ekf2/EKF/bias_estimator.hpp +++ b/src/modules/ekf2/EKF/bias_estimator.hpp @@ -80,9 +80,9 @@ public: virtual void fuseBias(float measurement, float measurement_var); void setBias(float bias) { _state = bias; } - void setProcessNoiseStdDev(float process_noise) + void setProcessNoiseSpectralDensity(float nsd) { - _process_var = process_noise * process_noise; + _process_psd = nsd * nsd; } void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; } void setInnovGate(float gate_size) { _gate_size = gate_size; } @@ -99,7 +99,7 @@ private: 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 _process_psd{1.25e-6f}; ///< State process power spectral density (m^2/s^2/Hz) float _state_var_max{2.f}; ///< Used to constrain the state variance (m^2) // Innovation sequence monitoring; used to detect a bias in the state diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 6a11cbbe05..75a67b6017 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -305,9 +305,10 @@ struct parameters { // position and velocity fusion float gps_vel_noise{5.0e-1f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec) float gps_pos_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m) + float gps_hgt_bias_nsd{0.13f}; ///< process noise for gnss height bias estimation (m/s/sqrt(Hz)) 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_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) 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) @@ -342,6 +343,7 @@ struct parameters { // range finder fusion float range_noise{0.1f}; ///< observation noise for range finder measurements (m) float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) + float rng_hgt_bias_nsd{0.13f}; ///< process noise for range height bias estimation (m/s/sqrt(Hz)) float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m) float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m) @@ -356,6 +358,7 @@ struct parameters { // vision position fusion float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) + float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz)) // optical flow fusion float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp index 33538096b0..1497713bb9 100644 --- a/src/modules/ekf2/EKF/gps_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_fusion.cpp @@ -105,20 +105,12 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample) const float height_measurement = gps_sample.hgt - getEkfGlobalOriginAltitude(); const float height_measurement_var = getGpsHeightVariance(); - // save current bias and update bias estimator - const float bias = _gps_hgt_b_est.getBias(); - const float bias_var = _gps_hgt_b_est.getBiasVar(); - - _gps_hgt_b_est.setMaxStateNoise(height_measurement_var); - _gps_hgt_b_est.setProcessNoiseStdDev(height_measurement_var); //TODO: update this - _gps_hgt_b_est.fuseBias(height_measurement - (-_state.pos(2)), height_measurement_var + P(9, 9)); - Vector3f position; position(0) = gps_sample.pos(0); position(1) = gps_sample.pos(1); // vertical position - gps measurement has opposite sign to earth z axis - position(2) = -(height_measurement - bias); + position(2) = -(height_measurement - _gps_hgt_b_est.getBias()); const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); @@ -136,7 +128,7 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample) obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit)); } - obs_var(2) = height_measurement_var + bias_var; + obs_var(2) = height_measurement_var + _gps_hgt_b_est.getBiasVar(); // innovation gate size float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f); @@ -160,6 +152,12 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample) } gps_pos.timestamp_sample = gps_sample.time_us; + + // update the bias estimator before updating the main filter but after + // using its current state to compute the vertical position innovation + _gps_hgt_b_est.setMaxStateNoise(height_measurement_var); + _gps_hgt_b_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd); + _gps_hgt_b_est.fuseBias(height_measurement - (-_state.pos(2)), height_measurement_var + P(9, 9)); } void Ekf::fuseGpsVel() diff --git a/src/modules/ekf2/EKF/height_fusion.cpp b/src/modules/ekf2/EKF/height_fusion.cpp index 40dcfc8e99..d83232d73d 100644 --- a/src/modules/ekf2/EKF/height_fusion.cpp +++ b/src/modules/ekf2/EKF/height_fusion.cpp @@ -91,7 +91,7 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation _baro_b_est.setMaxStateNoise(_params.baro_noise); - _baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate); + _baro_b_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd); _baro_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9)); } @@ -143,7 +143,7 @@ void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt) // using its current state to compute the vertical position innovation const float rng_noise = sqrtf(measurement_var); _rng_hgt_b_est.setMaxStateNoise(rng_noise); - _rng_hgt_b_est.setProcessNoiseStdDev(rng_noise); // TODO: fix + _rng_hgt_b_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd); _rng_hgt_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9)); } @@ -167,9 +167,8 @@ void Ekf::fuseEvHgt() const float bias = _ev_hgt_b_est.getBias(); const float bias_var = _ev_hgt_b_est.getBiasVar(); - const float ev_noise = sqrtf(measurement_var); - _ev_hgt_b_est.setMaxStateNoise(ev_noise); - _ev_hgt_b_est.setProcessNoiseStdDev(ev_noise); // TODO: fix + _ev_hgt_b_est.setMaxStateNoise(sqrtf(measurement_var)); + _ev_hgt_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); _ev_hgt_b_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9)); // calculate the innovation assuming the external vision observation is in local NED frame