Diff: run.cpp

Revision:

120:a0da2ce20a8e

Parent:

119:a21e283730d1

Child:

122:68efdee114fa

Child:

123:b63d3524ffbc

Child:

124:7d6b1b62483b

--- a/run.cpp	Thu Nov 18 08:48:29 2021 +0000
+++ b/run.cpp	Thu Nov 18 10:10:18 2021 +0000
@@ -68,8 +68,8 @@
     
     Matrix RImuConst(5,5);
     setDiag(RImuConst,50.0f);
-    RImuConst(4,4) = 0.01f;
-    RImuConst(5,5) = 0.01f;
+    RImuConst(4,4) = 0.001f;
+    RImuConst(5,5) = 0.001f;
     
     Matrix RotherConst(2,2);
     setDiag(RotherConst,0.1f);
@@ -77,10 +77,10 @@
     Matrix Racc(3,3);
     setDiag(Racc,50.0f);
     
-    Matrix Rheading(3,3);
+    Matrix Rheading(2,2);
     Rheading(1,1) = 0.001f;
     Rheading(2,2) = 0.001f;
-    Rheading(3,3) = 0.001f;
+    //Rheading(3,3) = 0.001f;
     //Rheading(4,4) = 0.1f;
         _t.start();
     float tgps = _t.read();
@@ -115,12 +115,12 @@
             
         }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.updateMag(MatrixMath::Vector2mat(mag),Rheading);
             //}
-            eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+            //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
         }
         PIDtick.loop();