aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix attitude error calculation in sim

Browse Source
This commit is contained in:
Oleg Kalachev 2023-12-04 20:22:32 +03:00
parent e360110430
commit 02aac609ab
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gazebo/flix.cpp
View File

@ -190,7 +190,8 @@ public:
// 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