From 8eb2a0a3ec5d0f83471bed89fc0d4aeb15139225 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Dec 2022 09:43:29 -0500 Subject: [PATCH] ekf2: delete orientation_covariances_euler() - usage replaced with now public calcRotVecVariances() --- src/modules/ekf2/EKF/ekf.h | 8 ++-- src/modules/ekf2/EKF/ekf_helper.cpp | 59 +---------------------------- src/modules/ekf2/EKF2.cpp | 2 +- 3 files changed, 5 insertions(+), 64 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 836782a77a..69434b161b 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -204,8 +204,6 @@ public: // get the orientation (quaterion) covariances matrix::SquareMatrix orientation_covariances() const { return P.slice<4, 4>(0, 0); } - matrix::SquareMatrix orientation_covariances_euler() const; - // get the linear velocity covariances matrix::SquareMatrix velocity_covariances() const { return P.slice<3, 3>(4, 4); } @@ -391,6 +389,9 @@ public: // use the latest IMU data at the current time horizon. Quatf calculate_quaternion() const; + // rotate quaternion covariances into variances for an equivalent rotation vector + Vector3f calcRotVecVariances() const; + // set minimum continuous period without GPS fail required to mark a healthy GPS status void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } @@ -967,9 +968,6 @@ private: // calculate the measurement variance for the optical flow sensor float calcOptFlowMeasVar(const flowSample &flow_sample); - // rotate quaternion covariances into variances for an equivalent rotation vector - Vector3f calcRotVecVariances(); - // initialise the quaternion covariances using rotation vector variances // do not call before quaternion states are initialised void initialiseQuatCovariances(Vector3f &rot_vec_var); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 3dd48d438c..49adfeab6a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -966,7 +966,7 @@ void Ekf::updateVerticalDeadReckoningStatus() } // calculate the variances for the rotation vector equivalent -Vector3f Ekf::calcRotVecVariances() +Vector3f Ekf::calcRotVecVariances() const { Vector3f rot_var_vec; float q0, q1, q2, q3; @@ -1521,60 +1521,3 @@ void Ekf::resetGpsDriftCheckFilters() _gps_vertical_position_drift_rate_m_s = NAN; _gps_filtered_horizontal_velocity_m_s = NAN; } - -matrix::SquareMatrix Ekf::orientation_covariances_euler() const -{ - // Jacobian matrix (3x4) containing the partial derivatives of the - // Euler angle equations with respect to the quaternions - matrix::Matrix G; - - // quaternion components - float q1 = _state.quat_nominal(0); - float q2 = _state.quat_nominal(1); - float q3 = _state.quat_nominal(2); - float q4 = _state.quat_nominal(3); - - // numerator components - float n1 = 2 * q1 * q2 + 2 * q2 * q4; - float n2 = -2 * q2 * q2 - 2 * q3 * q3 + 1; - float n3 = 2 * q1 * q4 + 2 * q2 * q3; - float n4 = -2 * q3 * q3 - 2 * q4 * q4 + 1; - float n5 = 2 * q1 * q3 + 2 * q2 * q4; - float n6 = -2 * q1 * q2 - 2 * q2 * q4; - float n7 = -2 * q1 * q4 - 2 * q2 * q3; - - // Protect against division by 0 - float d1 = n1 * n1 + n2 * n2; - float d2 = n3 * n3 + n4 * n4; - - if (fabsf(d1) < FLT_EPSILON) { - d1 = FLT_EPSILON; - } - - if (fabsf(d2) < FLT_EPSILON) { - d2 = FLT_EPSILON; - } - - // Protect against square root of negative numbers - float x = math::max(-n5 * n5 + 1, 0.0f); - - // compute G matrix - float sqrt_x = sqrtf(x); - float g00_03 = 2 * q2 * n2 / d1; - G(0, 0) = g00_03; - G(0, 1) = -4 * q2 * n6 / d1 + (2 * q1 + 2 * q4) * n2 / d1; - G(0, 2) = -4 * q3 * n6 / d1; - G(0, 3) = g00_03; - G(1, 0) = 2 * q3 / sqrt_x; - G(1, 1) = 2 * q4 / sqrt_x; - G(1, 2) = 2 * q1 / sqrt_x; - G(1, 3) = 2 * q2 / sqrt_x; - G(2, 0) = 2 * q4 * n4 / d2; - G(2, 1) = 2 * q3 * n4 / d2; - G(2, 2) = 2 * q2 * n4 / d2 - 4 * q3 * n7 / d2; - G(2, 3) = 2 * q1 * n4 / d2 - 4 * q4 * n7 / d2; - - const matrix::SquareMatrix quat_covariances = P.slice<4, 4>(0, 0); - - return G * quat_covariances * G.transpose(); -} diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 8f19d499c5..186b7d6478 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1196,7 +1196,7 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp) _ekf.position_covariances().diag().copyTo(odom.position_variance); // orientation covariance - _ekf.orientation_covariances_euler().diag().copyTo(odom.orientation_variance); + _ekf.calcRotVecVariances().copyTo(odom.orientation_variance); odom.reset_counter = _ekf.get_quat_reset_count() + _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()