From 07e8e88e88fd19428772d4f09e1e8765b6210557 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Mon, 9 Dec 2019 14:27:22 +0100 Subject: [PATCH] Support full vision variance --- EKF/common.h | 14 ++++++-------- EKF/control.cpp | 24 +++++++++++++++--------- EKF/ekf_helper.cpp | 6 +++--- EKF/estimator_interface.cpp | 7 +++---- EKF/mag_fusion.cpp | 2 +- 5 files changed, 28 insertions(+), 25 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 0c80069769..8fe107f047 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -83,10 +83,9 @@ struct ext_vision_message { Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis Quatf quat; ///< quaternion defining rotation from body to earth frame - float posErr; ///< 1-Sigma horizontal position accuracy (m) - float hgtErr; ///< 1-Sigma height accuracy (m) - float velErr; ///< 1-Sigma velocity accuracy (m/sec) - float angErr; ///< 1-Sigma angular error (rad) + Vector3f posVar; ///< XYZ position variances (m**2) + Vector3f velVar; ///< XYZ velocity variances ((m/sec)**2) + float angVar; ///< angular heading variance (rad**2) }; struct outputSample { @@ -156,10 +155,9 @@ struct extVisionSample { Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis Quatf quat; ///< quaternion defining rotation from body to earth frame - float posErr; ///< 1-Sigma horizontal position accuracy (m) - float hgtErr; ///< 1-Sigma height accuracy (m) - float velErr; ///< 1-Sigma velocity accuracy (m/sec) - float angErr; ///< 1-Sigma angular error (rad) + Vector3f posVar; ///< XYZ position variances (m**2) + Vector3f velVar; ///< XYZ velocity variances ((m/sec)**2) + float angVar; ///< angular heading variance (rad**2) uint64_t time_us; ///< timestamp of the measurement (uSec) }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 79112911db..415500e640 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -238,7 +238,7 @@ void Ekf::controlExternalVisionFusion() uncorrelateQuatStates(); // adjust the quaternion covariances estimated yaw error - increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); + increaseQuatYawErrVariance(fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f))); // calculate the amount that the quaternion has changed by _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); @@ -307,7 +307,10 @@ void Ekf::controlExternalVisionFusion() _ev_pos_innov(1) = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); // observation 1-STD error, incremental pos observation is expected to have more uncertainty - ev_pos_obs_var(0) = ev_pos_obs_var(1) = sq(fmaxf(_ev_sample_delayed.posErr, 0.5f)); + Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar); + ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose(); + ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f)); + ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f)); } // record observation and estimate for use next time @@ -318,13 +321,16 @@ void Ekf::controlExternalVisionFusion() } else { // use the absolute position Vector3f ev_pos_meas = _ev_sample_delayed.pos; + Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar); if (_params.fusion_mode & MASK_ROTATE_EV) { ev_pos_meas = _R_ev_to_ekf * ev_pos_meas; + ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose(); } _ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0); _ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1); - // observation 1-STD error - ev_pos_obs_var(0) = ev_pos_obs_var(1) = sq(fmaxf(_ev_sample_delayed.posErr, 0.01f)); + + ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.01f)); + ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 0), sq(0.01f)); // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_hor_pos_fuse) > _params.reset_timeout_max) { @@ -349,11 +355,13 @@ void Ekf::controlExternalVisionFusion() Vector3f ev_vel_obs_var; Vector2f ev_vel_innov_gates; - Vector3f vel_aligned(_ev_sample_delayed.vel); + Vector3f vel_aligned{_ev_sample_delayed.vel}; + Matrix3f ev_vel_var = matrix::diag(_ev_sample_delayed.velVar); // rotate measurement into correct earth frame if required if (_params.fusion_mode & MASK_ROTATE_EV) { vel_aligned = _R_ev_to_ekf * _ev_sample_delayed.vel; + ev_vel_var = _R_ev_to_ekf * ev_vel_var * _R_ev_to_ekf.transpose(); } // correct velocity for offset relative to IMU @@ -373,10 +381,8 @@ void Ekf::controlExternalVisionFusion() } } - // observation 1-STD error - ev_vel_obs_var(0) = ev_vel_obs_var(1) = ev_vel_obs_var(2) = sq(fmaxf(_ev_sample_delayed.velErr, 0.01f)); + ev_vel_obs_var = matrix::max(ev_vel_var.diag(), sq(0.01f)); - // innovation gate size ev_vel_innov_gates(0) = ev_vel_innov_gates(1) = fmaxf(_params.ev_vel_innov_gate, 1.0f); fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); @@ -1180,7 +1186,7 @@ void Ekf::controlHeightFusion() // 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) = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f)); + 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); // fuse height information diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 3a320b1d2d..375d25baf7 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -100,7 +100,7 @@ bool Ekf::resetVelocity() _ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel; } _state.vel = _ev_vel; - P.uncorrelateCovarianceSetVariance<3>(4, sq(_ev_sample_delayed.velErr)); + P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar); } else { ECL_INFO_TIMESTAMPED("reset velocity to zero"); // Used when falling back to non-aiding mode of operation @@ -165,7 +165,7 @@ bool Ekf::resetPosition() _state.pos(1) = _ev_pos(1); // use EV accuracy to reset variances - P.uncorrelateCovarianceSetVariance<2>(7, sq(_ev_sample_delayed.posErr)); + P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0)); } else if (_control_status.flags.opt_flow) { ECL_INFO_TIMESTAMPED("reset position to last known position"); @@ -705,7 +705,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool // update the yaw angle variance using the variance of the measurement if (_control_status.flags.ev_yaw) { // using error estimate from external vision data - increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); + increaseQuatYawErrVariance(fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f))); } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // using magnetic heading tuning parameter increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 8f324d5719..590c6e9bcc 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -434,10 +434,9 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message // calculate the system time-stamp for the mid point of the integration period ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000; - ev_sample_new.angErr = evdata->angErr; - ev_sample_new.posErr = evdata->posErr; - ev_sample_new.velErr = evdata->velErr; - ev_sample_new.hgtErr = evdata->hgtErr; + ev_sample_new.angVar = evdata->angVar; + ev_sample_new.posVar = evdata->posVar; + ev_sample_new.velVar = evdata->velVar; ev_sample_new.quat = evdata->quat; ev_sample_new.pos = evdata->pos; ev_sample_new.vel = evdata->vel; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 744825acf6..8d8bf90d12 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -626,7 +626,7 @@ void Ekf::fuseHeading() } else if (_control_status.flags.ev_yaw) { // using error estimate from external vision data - R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)); + R_YAW = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)); } else { // default value