Diff: run.cpp

Revision:

119:a21e283730d1

Parent:

117:f899fd694e2a

Child:

120:a0da2ce20a8e

--- a/run.cpp	Wed Nov 17 05:12:24 2021 +0000
+++ b/run.cpp	Thu Nov 18 08:48:29 2021 +0000
@@ -71,15 +71,17 @@
     RImuConst(4,4) = 0.01f;
     RImuConst(5,5) = 0.01f;
     
+    Matrix RotherConst(2,2);
+    setDiag(RotherConst,0.1f);
     
     Matrix Racc(3,3);
     setDiag(Racc,50.0f);
     
-    Matrix Rmag(3,3);
-    Rmag(1,1) = 0.0001f;
-    Rmag(2,2) = 0.0001f;
-    Rmag(3,3) = 0.0001f;
-    //Rmag(4,4) = 0.001f;
+    Matrix Rheading(3,3);
+    Rheading(1,1) = 0.001f;
+    Rheading(2,2) = 0.001f;
+    Rheading(3,3) = 0.001f;
+    //Rheading(4,4) = 0.1f;
         _t.start();
     float tgps = _t.read();
     while(1)
@@ -112,8 +114,12 @@
             //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel);
             
         }else{
+            //eskf.updateOtherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst);
             //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
-            eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag);
+            //eskf.updateotherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst);
+            //if(_t.read()>20){
+                eskf.updateMag(MatrixMath::Vector2mat(mag),Rheading);
+            //}
             eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
         }
         PIDtick.loop();