From 114516b78499b6612f0dd3ccabe0bb5e0e23b8cc Mon Sep 17 00:00:00 2001 From: Kamil Ritz Date: Sat, 15 Aug 2020 09:13:41 +0200 Subject: [PATCH] Use new added matrix functions --- EKF/EKFGSF_yaw.cpp | 4 +--- EKF/covariance.cpp | 6 +----- EKF/ekf.cpp | 4 +--- 3 files changed, 3 insertions(+), 11 deletions(-) diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index e933b07885..10948a01a2 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -534,9 +534,7 @@ Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g) if (rowLengthSq > FLT_EPSILON) { // Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0 const float rowLengthInv = 1.5f - 0.5f * rowLengthSq; - ret(r,0) *= rowLengthInv; - ret(r,1) *= rowLengthInv; - ret(r,2) *= rowLengthInv; + ret.row(r) *= rowLengthInv; } } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 70867b8595..2901ba0951 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -953,11 +953,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong // calculate accel bias term aligned with the gravity vector const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; - float down_dvel_bias = 0.0f; - - for (uint8_t axis_index = 0; axis_index < 3; axis_index++) { - down_dvel_bias += _state.delta_vel_bias(axis_index) * _R_to_earth(2, axis_index); - } + const float down_dvel_bias = _state.delta_vel_bias.dot(Vector3f(_R_to_earth.row(2))); // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 3e90cb4e8d..7dbb03dd85 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -324,9 +324,7 @@ void Ekf::calculateOutputStates() const Vector3f delta_angle(imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr); // calculate a yaw change about the earth frame vertical - const float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) + - _R_to_earth_now(2, 1) * delta_angle(1) + - _R_to_earth_now(2, 2) * delta_angle(2); + const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2))); _yaw_delta_ef += spin_del_ang_D; // Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic