Diff: run.cpp

Revision:

122:68efdee114fa

Parent:

120:a0da2ce20a8e

--- a/run.cpp	Thu Nov 18 10:10:18 2021 +0000
+++ b/run.cpp	Fri Nov 19 05:44:23 2021 +0000
@@ -49,12 +49,13 @@
     eskf.setPhatAccBias(0.001f);
     eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
+     eskf.setPhatMagBias(0.00001f);
     
     eskf.setQVelocity(0.0025f);
     eskf.setQAngleError(0.00025f);
     eskf.setQAccBias(0.0000001f);
     eskf.setQGyroBias(0.000001f);
-    
+    eskf.setQMagBias(0.000001f);
     Matrix Rgps(5,5);
     setDiag(Rgps,2.0f);
     //Rgps(6,6) = 100.0f;
@@ -66,10 +67,10 @@
     setDiag(Rgpsvel,2.0f);
     //Rgpsvel(3,3) = 10.0f;
     
-    Matrix RImuConst(5,5);
+    Matrix RImuConst(4,4);
     setDiag(RImuConst,50.0f);
-    RImuConst(4,4) = 0.001f;
-    RImuConst(5,5) = 0.001f;
+    RImuConst(4,4) = 0.005f;
+    //RImuConst(5,5) = 0.5f;
     
     Matrix RotherConst(2,2);
     setDiag(RotherConst,0.1f);
@@ -109,7 +110,7 @@
             getGPSval();
         }
         if(gpsUpdateFlag == true){
-            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
+            //eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
             //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
             //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel);