From 5356077a3244a9a29adfae4aeaaab900cd28e9e8 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sun, 21 Jun 2020 14:24:55 +0200 Subject: [PATCH] Make flow_innov/-var a matrix Vector2f --- EKF/ekf.h | 4 ++-- EKF/ekf_helper.cpp | 12 ++++++------ EKF/optflow_fusion.cpp | 14 +++++++------- EKF/terrain_estimator.cpp | 20 ++++++++++---------- 4 files changed, 25 insertions(+), 25 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 6e86463f35..c495f4034c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -428,8 +428,8 @@ private: float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) // optical flow processing - float _flow_innov[2] {}; ///< flow measurement innovation (rad/sec) - float _flow_innov_var[2] {}; ///< flow innovation variance ((rad/sec)**2) + Vector2f _flow_innov; ///< flow measurement innovation (rad/sec) + Vector2f _flow_innov_var; ///< flow innovation variance ((rad/sec)**2) Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad) float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index dfed56ac00..464cddb888 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -727,12 +727,12 @@ void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio) const void Ekf::getFlowInnov(float flow_innov[2]) const { - memcpy(flow_innov, _flow_innov, sizeof(_flow_innov)); + _flow_innov.copyTo(flow_innov); } void Ekf::getFlowInnovVar(float flow_innov_var[2]) const { - memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var)); + _flow_innov_var.copyTo(flow_innov_var); } void Ekf::getFlowInnovRatio(float &flow_innov_ratio) const @@ -983,7 +983,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) if (_control_status.flags.opt_flow) { float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f); - vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(sq(_flow_innov[0]) + sq(_flow_innov[1])); + vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * _flow_innov.norm(); } if (_control_status.flags.gps) { @@ -1667,9 +1667,9 @@ void Ekf::stopAuxVelFusion() void Ekf::stopFlowFusion() { _control_status.flags.opt_flow = false; - memset(_flow_innov,0.0f,sizeof(_flow_innov)); - memset(_flow_innov_var,0.0f,sizeof(_flow_innov_var)); - memset(&_optflow_test_ratio,0.0f,sizeof(_optflow_test_ratio)); + _flow_innov.setZero(); + _flow_innov_var.setZero(); + _optflow_test_ratio = 0.0f; } void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 6d30790107..757dbe52d3 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -104,8 +104,8 @@ void Ekf::fuseOptFlow() const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias); if (opt_flow_rate.norm() < _flow_max_rate) { - _flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis - _flow_innov[1] = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis + _flow_innov(0) = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis + _flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis } else { return; @@ -225,7 +225,7 @@ void Ekf::fuseOptFlow() // calculate innovation variance for X axis observation and protect against a badly conditioned calculation if (t77 >= R_LOS) { t78 = 1.0f / t77; - _flow_innov_var[0] = t77; + _flow_innov_var(0) = t77; } else { // we need to reinitialise the covariance matrix and abort this fusion step @@ -367,7 +367,7 @@ void Ekf::fuseOptFlow() // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation if (t77 >= R_LOS) { t78 = 1.0f / t77; - _flow_innov_var[1] = t77; + _flow_innov_var(1) = t77; } else { // we need to reinitialise the covariance matrix and abort this fusion step @@ -407,8 +407,8 @@ void Ekf::fuseOptFlow() // run the innovation consistency check and record result bool flow_fail = false; float test_ratio[2]; - test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]); - test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]); + test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0)); + test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1)); _optflow_test_ratio = math::max(test_ratio[0],test_ratio[1]); for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { @@ -493,7 +493,7 @@ void Ekf::fuseOptFlow() fixCovarianceErrors(true); // apply the state corrections - fuse(gain, _flow_innov[obs_index]); + fuse(gain, _flow_innov(obs_index)); _time_last_of_fuse = _time_last_imu; } diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 2d12177b50..3a54fa6a04 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -231,27 +231,27 @@ void Ekf::fuseFlowForTerrain() _terrain_var = fmaxf(_terrain_var, 0.0f); // Cacluate innovation variance - _flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS; + _flow_innov_var(0) = Hx * Hx * _terrain_var + R_LOS; // calculate the kalman gain for the flow x measurement - const float Kx = _terrain_var * Hx / _flow_innov_var[0]; + const float Kx = _terrain_var * Hx / _flow_innov_var(0); // calculate prediced optical flow about x axis const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv; // calculate flow innovation (x axis) - _flow_innov[0] = pred_flow_x - opt_flow_rate(0); + _flow_innov(0) = pred_flow_x - opt_flow_rate(0); // calculate correction term for terrain variance const float KxHxP = Kx * Hx * _terrain_var; // innovation consistency check const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f); - float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]); + float flow_test_ratio = sq(_flow_innov(0)) / (sq(gate_size) * _flow_innov_var(0)); // do not perform measurement update if badly conditioned if (flow_test_ratio <= 1.0f) { - _terrain_vpos += Kx * _flow_innov[0]; + _terrain_vpos += Kx * _flow_innov(0); // guard against negative variance _terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f); _time_last_of_fuse = _time_last_imu; @@ -261,25 +261,25 @@ void Ekf::fuseFlowForTerrain() const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv; // Calculuate innovation variance - _flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS; + _flow_innov_var(1) = Hy * Hy * _terrain_var + R_LOS; // calculate the kalman gain for the flow y measurement - const float Ky = _terrain_var * Hy / _flow_innov_var[1]; + const float Ky = _terrain_var * Hy / _flow_innov_var(1); // calculate prediced optical flow about y axis const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv; // calculate flow innovation (y axis) - _flow_innov[1] = pred_flow_y - opt_flow_rate(1); + _flow_innov(1) = pred_flow_y - opt_flow_rate(1); // calculate correction term for terrain variance const float KyHyP = Ky * Hy * _terrain_var; // innovation consistency check - flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]); + flow_test_ratio = sq(_flow_innov(1)) / (sq(gate_size) * _flow_innov_var(1)); if (flow_test_ratio <= 1.0f) { - _terrain_vpos += Ky * _flow_innov[1]; + _terrain_vpos += Ky * _flow_innov(1); // guard against negative variance _terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f); _time_last_of_fuse = _time_last_imu;