Diff: run.cpp

Revision:

118:962f226bdf64

Parent:

116:663f5889dd17

--- a/run.cpp	Wed Nov 17 01:40:40 2021 +0000
+++ b/run.cpp	Thu Nov 18 00:50:47 2021 +0000
@@ -49,8 +49,8 @@
     eskf.setPhatAccBias(0.001f);
     eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
-    eskf.setPhatGyroBias(0.01f);
-    eskf.setPhatMagRadius(0.0001f);
+    eskf.setPhatMagBias(0.001f);
+    eskf.setPhatMagRadius(0.001f);
     
     eskf.setQVelocity(0.0025f);
     eskf.setQAngleError(0.00025f);
@@ -79,10 +79,14 @@
     Matrix Racc(3,3);
     setDiag(Racc,50.0f);
     
+    Matrix Rother(2,2);
+    Rother(1,1) = 1.0f;
+    Rother(2,2) = 0.01f;
+    
     Matrix Rmag(3,3);
-    Rmag(1,1) = 0.01f;
-    Rmag(2,2) = 0.01f;
-    Rmag(3,3) = 0.01f;
+    Rmag(1,1) = 0.1f;
+    Rmag(2,2) = 0.1f;
+    Rmag(3,3) = 0.1f;
     //Rmag(4,4) = 0.01f;
         _t.start();
     float tgps = _t.read();
@@ -116,9 +120,10 @@
             //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel);
             
         }else{
-            //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
-            eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag);
-            eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+            eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
+            //eskf.updateOtherConstraints(MatrixMath::Vector2mat(mag),palt,Rother);
+            //eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag);
+            //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
         }
         PIDtick.loop();