From 4fa8ab11a6311caf9cbb9dc0df01297cd7f3b6df Mon Sep 17 00:00:00 2001 From: Andrew Smith Date: Wed, 21 Jan 2026 12:47:38 +0000 Subject: [PATCH 1/2] apply lever-arm offset in world frame --- src/sensorModels/gnssSensor.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/sensorModels/gnssSensor.cpp b/src/sensorModels/gnssSensor.cpp index 0a6a3e1..1e48935 100644 --- a/src/sensorModels/gnssSensor.cpp +++ b/src/sensorModels/gnssSensor.cpp @@ -138,6 +138,15 @@ bool GnssSensor::RunTick(Eigen::Vector3d& gnssOrigin, Eigen::Vector3d& enuToTrac { actualTrackCar = eulerAnglesToRotMat(start_orientation).transpose() * trans + start_position; } + + Eigen::Matrix3d R_body_to_world = eulerAnglesToRotMat(rot).transpose(); + if (trackPreTransformed) + { + R_body_to_world = eulerAnglesToRotMat(start_orientation).transpose() * R_body_to_world; + } + Eigen::Vector3d gpsLeverArmWorld = R_body_to_world * this->position; + actualTrackCar += gpsLeverArmWorld; + Eigen::Vector3d enuCar = rotMatTrackToEnu * actualTrackCar; std::default_random_engine generator(noiseSeed); From b4dca1cf0338d548e40c772fe94e17d7261c825c Mon Sep 17 00:00:00 2001 From: Andrew Smith Date: Wed, 21 Jan 2026 20:02:10 +0000 Subject: [PATCH 2/2] quaternion Z axis bug --- src/quaternion.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/quaternion.cpp b/src/quaternion.cpp index 100be03..e9b04e7 100644 --- a/src/quaternion.cpp +++ b/src/quaternion.cpp @@ -64,10 +64,10 @@ quaternion quatFromEulerAngles(Eigen::Vector3d a) quatY.z = 0.0; quaternion quatZ; - quatX.w = std::cos(z / 2); - quatX.x = 0.0; - quatX.y = 0.0; - quatX.z = std::sin(z / 2); + quatZ.w = std::cos(z / 2); + quatZ.x = 0.0; + quatZ.y = 0.0; + quatZ.z = std::sin(z / 2); quaternion ret; ret.w = 1.0;