Diff: run.cpp

Revision:

113:3e47d9881529

Parent:

112:6a33ea9f6fed

Child:

114:88abd741f44d

Child:

117:f899fd694e2a

--- a/run.cpp	Tue Nov 16 01:21:01 2021 +0000
+++ b/run.cpp	Tue Nov 16 13:57:32 2021 +0000
@@ -45,19 +45,15 @@
     eskf.setGravity(0.0f,0.0f,9.8f);
     eskf.setPhatPosition(0.1f);
     eskf.setPhatVelocity(0.1f);
-    eskf.setPhatAngleError(0.1f);
+    eskf.setPhatAngleError(0.5f);
     eskf.setPhatAccBias(0.001f);
     eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
-    eskf.setPhatMagBias(0.001f);
-    eskf.setPhatMagRadius(0.5f);
     
     eskf.setQVelocity(0.0025f);
     eskf.setQAngleError(0.00025f);
     eskf.setQAccBias(0.0000001f);
     eskf.setQGyroBias(0.000001f);
-    eskf.setQMagBias(0.000001f);
-    eskf.setQMagRadius(0.000001f);
     
     Matrix Rgps(5,5);
     setDiag(Rgps,2.0f);
@@ -75,10 +71,15 @@
     RImuConst(4,4) = 0.01f;
     RImuConst(5,5) = 0.01f;
     
+    
+    Matrix Racc(3,3);
+    setDiag(Racc,50.0f);
+    
     Matrix Rmag(3,3);
-    Rmag(1,1) = 10.0f;
-    Rmag(2,2) = 1.0f;
-    Rmag(3,3) = 2.0f;
+    Rmag(1,1) = 0.1f;
+    Rmag(2,2) = 0.001f;
+    Rmag(3,3) = 0.1f;
+    //Rmag(4,4) = 0.001f;
         _t.start();
     float tgps = _t.read();
     while(1)
@@ -111,8 +112,9 @@
             //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel);
             
         }else{
-            eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
-            //eskf.updateMag(MatrixMath::Vector2mat(mag),palt,Rmag);
+            //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
+            eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag);
+            eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
         }
         PIDtick.loop();