From 21f65365e62be3b6146edbb771bf9b2218557282 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 4 Dec 2023 20:22:32 +0300 Subject: [PATCH] Fix attitude error calculation in sim --- gazebo/flix.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gazebo/flix.cpp b/gazebo/flix.cpp index eabc8cd..a69e0fc 100644 --- a/gazebo/flix.cpp +++ b/gazebo/flix.cpp @@ -191,7 +191,8 @@ class ModelFlix : public ModelPlugin // calculate attitude estimation error - float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1)); + Quaternion groundtruthAttitude(estimateModel->WorldPose().Rot().W(), estimateModel->WorldPose().Rot().X(), -estimateModel->WorldPose().Rot().Y(), -estimateModel->WorldPose().Rot().Z()); + float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), groundtruthAttitude.rotate(Vector(0, 0, -1))); if (angle < 0.3) { //gzwarn << "att err: " << angle << endl; // TODO: warning