From 181303488cfc4d2ae34f61f0ad9a34b49a2501f7 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 3 Dec 2019 12:58:24 +0100 Subject: [PATCH] Log height innovation sensor specific --- EKF/control.cpp | 70 ++++++++++++++++++++++----------------- EKF/ekf.h | 18 ++++++++++ EKF/ekf_helper.cpp | 30 +++++++++++++++++ EKF/estimator_interface.h | 10 ++++++ 4 files changed, 98 insertions(+), 30 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 3781d5eae0..cf2c40b1af 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1163,20 +1163,18 @@ void Ekf::controlHeightFusion() if (_fuse_height) { - Vector2f height_innov_gate{}; - Vector2f height_test_ratio{}; - Vector3f height_obs_var{}; - Vector3f height_innov_var{}; - Vector3f height_innov{}; + if (_control_status.flags.baro_hgt) { - // vertical position innovation - baro measurement has opposite sign to earth z axis - height_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset; - // observation variance - user parameter defined - height_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f)); + Vector2f baro_hgt_innov_gate{}; + 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 - _hgt_sensor_offset; + // observation variance - user parameter defined + baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f)); // innovation gate size - height_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f); + baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f); // Compensate for positive static pressure transients (negative vertical position innovations) // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. @@ -1184,52 +1182,64 @@ void Ekf::controlHeightFusion() float deadzone_end = deadzone_start + _params.gnd_effect_deadzone; if (_control_status.flags.gnd_effect) { - if (height_innov(2) < -deadzone_start) { - if (height_innov(2) <= -deadzone_end) { - height_innov(2) += deadzone_end; + if (_baro_hgt_innov(2) < -deadzone_start) { + if (_baro_hgt_innov(2) <= -deadzone_end) { + _baro_hgt_innov(2) += deadzone_end; } else { - height_innov(2) = -deadzone_start; + _baro_hgt_innov(2) = -deadzone_start; } } } + // fuse height information + fuseVerticalPosition(_baro_hgt_innov,baro_hgt_innov_gate, + baro_hgt_obs_var, _baro_hgt_innov_var,_baro_hgt_test_ratio); } else if (_control_status.flags.gps_hgt) { + Vector2f gps_hgt_innov_gate{}; + Vector3f gps_hgt_obs_var{}; // vertical position innovation - gps measurement has opposite sign to earth z axis - height_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset; + _gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset; // observation variance - receiver defined and parameter limited // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); - height_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit)); + gps_hgt_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit)); // innovation gate size - height_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f); + gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f); + // fuse height information + fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate, + gps_hgt_obs_var, _gps_pos_innov_var,_gps_pos_test_ratio); } else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) { + // TODO: Tilt check does not belong here, should not set fuse height to true if tilted + Vector2f rng_hgt_innov_gate{}; + Vector3f rng_hgt_obs_var{}; // use range finder with tilt correction - height_innov(2) = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, + _rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, _params.rng_gnd_clearance)) - _hgt_sensor_offset; // observation variance - user parameter defined - height_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); + rng_hgt_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); // innovation gate size - height_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f); + rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f); + // fuse height information + fuseVerticalPosition(_rng_hgt_innov,rng_hgt_innov_gate, + rng_hgt_obs_var, _rng_hgt_innov_var,_rng_hgt_test_ratio); } else if (_control_status.flags.ev_hgt) { + Vector2f ev_hgt_innov_gate{}; + Vector3f ev_hgt_obs_var{}; // calculate the innovation assuming the external vision observation is in local NED frame - height_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2); + _ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2); // observation variance - defined externally - height_obs_var(2) = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f)); + ev_hgt_obs_var(2) = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f)); // innovation gate size - height_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f); + ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f); + // fuse height information + fuseVerticalPosition(_ev_pos_innov,ev_hgt_innov_gate, + ev_hgt_obs_var, _ev_pos_innov_var,_ev_pos_test_ratio); } - // fuse height inforamtion - fuseVerticalPosition(height_innov,height_innov_gate, - height_obs_var, height_innov_var,height_test_ratio); - // This is a temporary hack until we do proper height sensor fusion - _gps_pos_innov(2) = height_innov(2); - _gps_pos_innov_var(2) = height_innov_var(2); - _gps_pos_test_ratio(1) = height_test_ratio(1); } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 6ff9ccf2aa..37183e5c77 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -75,6 +75,18 @@ public: void getEvVelPosInnovRatio(float& hvel, float &vvel, float& hpos, float &vpos) override; + void getBaroHgtInnov(float &baro_hgt_innov) override; + + void getBaroHgtInnovVar(float &baro_hgt_innov_var) override; + + void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) override; + + void getRngHgtInnov(float &rng_hgt_innov) override; + + void getRngHgtInnovVar(float &rng_hgt_innov_var) override; + + void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) override; + void getAuxVelInnov(float aux_vel_innov[2]) override; void getAuxVelInnovVar(float aux_vel_innov[2]) override; @@ -387,6 +399,12 @@ private: Vector3f _ev_pos_innov {}; ///< external vision position innovations (m) Vector3f _ev_pos_innov_var {}; ///< external vision position innovation variances (m**2) + Vector3f _baro_hgt_innov {}; ///< baro hgt innovations (m) + Vector3f _baro_hgt_innov_var {}; ///< baro hgt innovation variances (m**2) + + Vector3f _rng_hgt_innov {}; ///< range hgt innovations (m) + Vector3f _rng_hgt_innov_var {}; ///< range hgt innovation variances (m**2) + Vector3f _aux_vel_innov {}; ///< horizontal auxiliary velocity innovations: (m/sec) Vector3f _aux_vel_innov_var {}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 9a05fee22b..03b5f48bca 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -889,6 +889,36 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp vpos = _ev_pos_test_ratio(1); } +void Ekf::getBaroHgtInnov(float &baro_hgt_innov) +{ + baro_hgt_innov = _baro_hgt_innov(2); +} + +void Ekf::getBaroHgtInnovVar(float &baro_hgt_innov_var) +{ + baro_hgt_innov_var = _baro_hgt_innov_var(2); +} + +void Ekf::getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) +{ + baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); +} + +void Ekf::getRngHgtInnov(float &rng_hgt_innov) +{ + rng_hgt_innov = _rng_hgt_innov(2); +} + +void Ekf::getRngHgtInnovVar(float &rng_hgt_innov_var) +{ + rng_hgt_innov_var = _rng_hgt_innov_var(2); +} + +void Ekf::getRngHgtInnovRatio(float &rng_hgt_innov_ratio) +{ + rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); +} + void Ekf::getAuxVelInnov(float aux_vel_innov[2]) { aux_vel_innov[0] = _aux_vel_innov(0); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 6f236113c1..0adc97ec7f 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -75,6 +75,14 @@ public: virtual void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; virtual void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 0; + virtual void getBaroHgtInnov(float &baro_hgt_innov) = 0; + virtual void getBaroHgtInnovVar(float &baro_hgt_innov_var) = 0; + virtual void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) = 0; + + virtual void getRngHgtInnov(float &rng_hgt_innov) = 0; + virtual void getRngHgtInnovVar(float &rng_hgt_innov_var) = 0; + virtual void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) = 0; + virtual void getAuxVelInnov(float aux_vel_innov[2]) = 0; virtual void getAuxVelInnovVar(float aux_vel_innov[2]) = 0; virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) = 0; @@ -479,6 +487,8 @@ protected: Vector2f _ev_vel_test_ratio {}; // EV velocity innovation consistency check ratios Vector2f _ev_pos_test_ratio {}; // EV position innovation consistency check ratios Vector2f _aux_vel_test_ratio{}; // Auxiliray horizontal velocity innovation consistency check ratio + Vector2f _baro_hgt_test_ratio {}; // baro height innovation consistency check ratios + Vector2f _rng_hgt_test_ratio {}; // range finder height innovation consistency check ratios float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio float _tas_test_ratio{}; // tas innovation consistency check ratio float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio