From 5030b21d2eb667e249d23f17c1f27725374cd81f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 20 Oct 2022 09:41:26 -0400 Subject: [PATCH] ekf2: replace quatToInverseRotMat if only used once --- src/modules/ekf2/EKF/drag_fusion.cpp | 3 +-- src/modules/ekf2/EKF/mag_control.cpp | 5 ++--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 8a89803f75..dda97c9dfc 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -83,8 +83,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // predicted specific forces // calculate relative wind velocity in earth frame and rotate into body frame const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd); - const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal); - const Vector3f rel_wind_body = earth_to_body * rel_wind_earth; + const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth); // perform sequential fusion of XY specific forces for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 03ccf9d208..09af32b0d6 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -80,9 +80,8 @@ void Ekf::controlMagFusion() // compute magnetometer innovations (for estimator_aid_src_mag logging) // rotate magnetometer earth field state into body frame - const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); - const Vector3f mag_I_rot = R_to_body * _state.mag_I; - const Vector3f mag_innov = mag_I_rot - mag_observation; + const Vector3f mag_I_body = _state.quat_nominal.rotateVectorInverse(_state.mag_I); + const Vector3f mag_innov = mag_I_body - mag_observation; resetEstimatorAidStatus(_aid_src_mag); _aid_src_mag.timestamp_sample = mag_sample.time_us;