diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 20e8d7c3a8..ce8f969c4e 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -993,8 +993,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) && ((down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps) || (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel)) && ((down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt) - || (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt) - || (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt) + || (down_dvel_bias * _baro_hgt_innov < 0.0f && _control_status.flags.baro_hgt) + || (down_dvel_bias * _rng_hgt_innov < 0.0f && _control_status.flags.rng_hgt) || (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt))); // record the pass/fail diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 7ca45805c2..f1234617de 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -78,13 +78,13 @@ public: void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; - void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov(2); } - void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var(2); } - void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); } + void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov; } + void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var; } + void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio; } - void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov(2); } - void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var(2); } - void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); } + void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov; } + void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var; } + void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio; } void getAuxVelInnov(float aux_vel_innov[2]) const; void getAuxVelInnovVar(float aux_vel_innov[2]) const; @@ -440,11 +440,11 @@ 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) + float _baro_hgt_innov{}; ///< baro hgt innovations (m) + float _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) + float _rng_hgt_innov{}; ///< range hgt innovations (m) + float _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) @@ -672,8 +672,8 @@ private: bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false); - bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, - Vector3f &innov_var, Vector2f &test_ratio); + bool fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var, + float &innov_var, float &test_ratio); void fuseGpsVelPos(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 2992f1de81..ff2cb4f6ed 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -965,13 +965,13 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f // return the vertical position innovation test ratio if (_control_status.flags.baro_hgt) { - hgt = math::max(sqrtf(_baro_hgt_test_ratio(1)), FLT_MIN); + hgt = math::max(sqrtf(_baro_hgt_test_ratio), FLT_MIN); } else if (_control_status.flags.gps_hgt) { hgt = math::max(sqrtf(_gps_pos_test_ratio(1)), FLT_MIN); } else if (_control_status.flags.rng_hgt) { - hgt = math::max(sqrtf(_rng_hgt_test_ratio(1)), FLT_MIN); + hgt = math::max(sqrtf(_rng_hgt_test_ratio), FLT_MIN); } else if (_control_status.flags.ev_hgt) { hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index fe04e5e293..bfd23da1ce 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -338,8 +338,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{}; // Auxiliary 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 _baro_hgt_test_ratio{}; // baro height innovation consistency check ratios + float _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 diff --git a/src/modules/ekf2/EKF/height_fusion.cpp b/src/modules/ekf2/EKF/height_fusion.cpp index f6332958f4..bc33085669 100644 --- a/src/modules/ekf2/EKF/height_fusion.cpp +++ b/src/modules/ekf2/EKF/height_fusion.cpp @@ -40,16 +40,10 @@ void Ekf::fuseBaroHgt() { - Vector2f baro_hgt_innov_gate; - Vector3f baro_hgt_obs_var; - // vertical position innovation - baro measurement has opposite sign to earth z axis 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 - baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f); + + _baro_hgt_innov = _state.pos(2) + unbiased_baro - _baro_hgt_offset; // Compensate for positive static pressure transients (negative vertical position innovations) // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. @@ -57,62 +51,69 @@ void Ekf::fuseBaroHgt() const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone; if (_control_status.flags.gnd_effect) { - if (_baro_hgt_innov(2) < -deadzone_start) { - if (_baro_hgt_innov(2) <= -deadzone_end) { - _baro_hgt_innov(2) += deadzone_end; + if (_baro_hgt_innov < -deadzone_start) { + if (_baro_hgt_innov <= -deadzone_end) { + _baro_hgt_innov += deadzone_end; } else { - _baro_hgt_innov(2) = -deadzone_start; + _baro_hgt_innov = -deadzone_start; } } } - fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate, - baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio); + // innovation gate size + float innov_gate = fmaxf(_params.baro_innov_gate, 1.f); + + // observation variance - user parameter defined + float obs_var = sq(fmaxf(_params.baro_noise, 0.01f)); + + fuseVerticalPosition(_baro_hgt_innov, innov_gate, obs_var, + _baro_hgt_innov_var, _baro_hgt_test_ratio); } void Ekf::fuseGpsHgt() { - Vector2f gps_hgt_innov_gate; - Vector3f gps_hgt_obs_var; // vertical position innovation - gps measurement has opposite sign to earth z axis _gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset; - gps_hgt_obs_var(2) = getGpsHeightVariance(); - // innovation gate size - gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f); - fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate, - gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); + // innovation gate size + float innov_gate = fmaxf(_params.baro_innov_gate, 1.f); + + float obs_var = getGpsHeightVariance(); + + // _gps_pos_test_ratio(1) is the vertical test ratio + fuseVerticalPosition(_gps_pos_innov(2), innov_gate, obs_var, + _gps_pos_innov_var(2), _gps_pos_test_ratio(1)); } void Ekf::fuseRngHgt() { - Vector2f rng_hgt_innov_gate; - Vector3f rng_hgt_obs_var; // use range finder with tilt correction - _rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(), - _params.rng_gnd_clearance)) - _hgt_sensor_offset; - // observation variance - user parameter defined - rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f); - // innovation gate size - rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f); + _rng_hgt_innov = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(), + _params.rng_gnd_clearance)) - _hgt_sensor_offset; - fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate, - rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio); + // innovation gate size + float innov_gate = fmaxf(_params.range_innov_gate, 1.f); + + // observation variance - user parameter defined + float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f); + + fuseVerticalPosition(_rng_hgt_innov, innov_gate, obs_var, + _rng_hgt_innov_var, _rng_hgt_test_ratio); } void Ekf::fuseEvHgt() { - Vector2f ev_hgt_innov_gate; - Vector3f ev_hgt_obs_var; // calculate the innovation assuming the external vision observation is in local NED frame _ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2); - // observation variance - defined externally - ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)); - // innovation gate size - ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f); - fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate, - ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio); + // innovation gate size + float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f); + + // observation variance - defined externally + float obs_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)); + + // _ev_pos_test_ratio(1) is the vertical test ratio + fuseVerticalPosition(_ev_pos_innov(2), innov_gate, obs_var, + _ev_pos_innov_var(2), _ev_pos_test_ratio(1)); } diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 4b1163d303..4e8e846c96 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -146,33 +146,32 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga } } -bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, - Vector3f &innov_var, Vector2f &test_ratio) +bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var, + float &innov_var, float &test_ratio) { - - innov_var(2) = P(9, 9) + obs_var(2); - test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); - _vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2)); + innov_var = P(9, 9) + obs_var; + test_ratio = sq(innov) / (sq(innov_gate) * innov_var); + _vert_pos_innov_ratio = innov / sqrtf(innov_var); _vert_pos_fuse_attempt_time_us = _time_last_imu; - bool innov_check_pass = test_ratio(1) <= 1.0f; + bool innov_check_pass = test_ratio <= 1.0f; // if there is bad vertical acceleration data, then don't reject measurement, // but limit innovation to prevent spikes that could destabilise the filter float innovation; if (_fault_status.flags.bad_acc_vertical && !innov_check_pass) { - const float innov_limit = innov_gate(1) * sqrtf(innov_var(2)); - innovation = math::constrain(innov(2), -innov_limit, innov_limit); + const float innov_limit = innov_gate * sqrtf(innov_var); + innovation = math::constrain(innov, -innov_limit, innov_limit); innov_check_pass = true; } else { - innovation = innov(2); + innovation = innov; } if (innov_check_pass) { _time_last_hgt_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_pos = false; - fuseVelPosHeight(innovation, innov_var(2), 5); + fuseVelPosHeight(innovation, innov_var, 5); return true;