filter yaw ok

2020-05-03 19:12:10 +02:00
parent 7f0d9a930d
commit ca7631250c
2 changed files with 7 additions and 2 deletions
@@ -137,7 +137,8 @@ class ImuOdom {
            q.x() = deltaQ(1);
            q.y() = deltaQ(2);
            q.z() = deltaQ(3);    
-            odomPOS = odomPOS + deltaP.cast<double>();
+            odomPOS = odomPOS + odomROT*deltaP.cast<double>(); // V1
+            //odomPOS = odomPOS + deltaP.cast<double>(); // V2
            odomROT = odomROT * q.normalized().toRotationMatrix();
            
            // compute euler