Diff: run.cpp

Revision:

123:b63d3524ffbc

Parent:

120:a0da2ce20a8e

--- a/run.cpp	Thu Nov 18 10:10:18 2021 +0000
+++ b/run.cpp	Fri Nov 19 08:22:13 2021 +0000
@@ -66,10 +66,11 @@
     setDiag(Rgpsvel,2.0f);
     //Rgpsvel(3,3) = 10.0f;
     
-    Matrix RImuConst(5,5);
+    Matrix RImuConst(6,6);
     setDiag(RImuConst,50.0f);
-    RImuConst(4,4) = 0.001f;
-    RImuConst(5,5) = 0.001f;
+    RImuConst(4,4) = 0.0001f;
+    RImuConst(5,5) = 0.0001f;
+    RImuConst(6,6) = 0.0001f;
     
     Matrix RotherConst(2,2);
     setDiag(RotherConst,0.1f);
@@ -78,12 +79,13 @@
     setDiag(Racc,50.0f);
     
     Matrix Rheading(2,2);
-    Rheading(1,1) = 0.001f;
-    Rheading(2,2) = 0.001f;
+    Rheading(1,1) = 0.0001f;
+    Rheading(2,2) = 0.0001f;
     //Rheading(3,3) = 0.001f;
     //Rheading(4,4) = 0.1f;
-        _t.start();
+    _t.start();
     float tgps = _t.read();
+    float timu = _t.read();
     while(1)
     {
         float tstart = _t.read();
@@ -113,15 +115,14 @@
             //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
             //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.updateotherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst);
-            //if(_t.read()>20){
-                //eskf.updateMag(MatrixMath::Vector2mat(mag),Rheading);
-            //}
-            //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
         }
+        if(tstart-timu>0.1f){
+            //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
+            eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+            //eskf.updateHeading(MatrixMath::Vector2mat(mag),Rheading);
+            timu = _t.read();
+        }
+        
         PIDtick.loop(); 
         
         //制御時間を計測