Fix attitude error calculation in sim

okalachev · Dec 4, 2023 · 21f6536 · 21f6536
1 parent 1c28b07
commit 21f6536
Showing 1 changed file with 2 additions and 1 deletion.
diff --git 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