Diff: run.cpp

Revision:

104:20b8caa29185

Parent:

103:fec71c2051c5

Child:

106:2d854e92cebb

--- a/run.cpp	Wed Nov 10 05:22:31 2021 +0000
+++ b/run.cpp	Wed Nov 10 06:34:16 2021 +0000
@@ -64,28 +64,13 @@
     eskf.setQGyroBias(0.000001f);
     eskf.setQMagField(0.001f);
     
-    Matrix Rgpspos(2,2);
-    setDiag(Rgpspos,2.0f);
-    //Rgpspos(3,3) = 5.0f;
-    Matrix Rgpsvel(3,3);
-    setDiag(Rgpsvel,2.0f);
     Matrix Rgps(5,5);
     setDiag(Rgps,2.0f);
     
-    
-    Matrix RaccConst(4,4);
-    setDiag(RaccConst,50.0f);
-    RaccConst(4,4) = 1.0f;
-    //Matrix RgyroConst(2,2);
-    //setDiag(RgyroConst,100);
-    Matrix Rmag(2,2);
-    Rmag(1,1) = 10.0f;
-    Rmag(2,2) = 1.0f;
-    
     Matrix RImuConst(5,5);
     setDiag(RImuConst,50.0f);
-    RImuConst(4,4) = 50.0f;
-    RImuConst(5,5) = 50.0f;
+    RImuConst(4,4) = 1.0f;
+    RImuConst(5,5) = 1.0f;
     _t.start();
     float tgps = _t.read();
     while(1)
@@ -114,13 +99,11 @@
             getGPSval();
         }
         if(gpsUpdateFlag == true){
-            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
-            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
             eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
             
+        }else{
+            eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
         }
-        eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
-        //eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag);
         PIDtick.loop(); 
         
         //制御時間を計測