Diff: run.cpp

Revision:

124:7d6b1b62483b

Parent:

120:a0da2ce20a8e

Child:

125:5b5a91004b48

--- a/run.cpp	Fri Nov 19 07:56:16 2021 +0000
+++ b/run.cpp	Fri Nov 19 09:21:51 2021 +0000
@@ -78,8 +78,8 @@
     setDiag(Racc,50.0f);
     
     Matrix Rheading(2,2);
-    Rheading(1,1) = 0.001f;
-    Rheading(2,2) = 0.001f;
+    Rheading(1,1) = 0.1f;
+    Rheading(2,2) = 0.1f;
     //Rheading(3,3) = 0.001f;
     //Rheading(4,4) = 0.1f;
         _t.start();
@@ -115,12 +115,13 @@
             
         }else{
             //eskf.updateOtherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst);
-            eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
+            //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
             //eskf.updateotherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst);
             //if(_t.read()>20){
                 //eskf.updateMag(MatrixMath::Vector2mat(mag),Rheading);
             //}
-            //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+            eskf.updateHeading(MatrixMath::Vector2mat(mag),Rheading);
+            eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
         }
         PIDtick.loop();