From b5f011a31fb30cb225d237bf3427b0719f95b8d2 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 28 Mar 2025 15:18:22 +0100 Subject: [PATCH] cal: transpose rotation instead of computing inverse This is equivalent for an orthonormal matrix --- src/lib/sensor_calibration/Accelerometer.hpp | 2 +- src/lib/sensor_calibration/Gyroscope.hpp | 4 ++-- src/lib/sensor_calibration/Magnetometer.hpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index ef631a00f1..428701efcd 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -88,7 +88,7 @@ public: matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { // updated calibration offset = existing offset + bias rotated to sensor frame and unscaled - return _offset + (_rotation.I() * bias).edivide(_scale); + return _offset + (_rotation.T() * bias).edivide(_scale); } bool ParametersLoad(); diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 6d160a1217..f7f2ec2078 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -85,14 +85,14 @@ public: inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const { - return (_rotation.I() * corrected_data) + _thermal_offset + _offset; + return (_rotation.T() * corrected_data) + _thermal_offset + _offset; } // Compute sensor offset from bias (board frame) matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { // updated calibration offset = existing offset + bias rotated to sensor frame - return _offset + (_rotation.I() * bias); + return _offset + (_rotation.T() * bias); } bool ParametersLoad(); diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 5f9dd72d43..df2b97df1e 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -104,7 +104,7 @@ public: matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { // updated calibration offset = existing offset + bias rotated to sensor frame and unscaled - return _offset + (_scale.I() * _rotation.I() * bias); + return _offset + (_scale.I() * _rotation.T() * bias); } bool ParametersLoad();