Diff: run.cpp

Revision:

94:579e875a4244

Parent:

93:b827f78a717a

Child:

96:3645f8f9bdd3

--- a/run.cpp	Fri Oct 29 13:30:24 2021 +0000
+++ b/run.cpp	Mon Nov 01 09:18:07 2021 +0000
@@ -32,21 +32,18 @@
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
-    Timer _t;
-    _t.start();
-    
     //EKFの共分散設定
     eskf.setGravity(0.0f,0.0f,9.8f);
     eskf.setPhatPosition(0.1f);
     eskf.setPhatVelocity(0.1f);
     eskf.setPhatAngleError(0.1f);
-    eskf.setPhatAccBias(0.5f);
-    eskf.setPhatGyroBias(0.5f);
-    eskf.setPhatGravity(0.1f);
+    eskf.setPhatAccBias(0.001f);
+    eskf.setPhatGyroBias(0.001f);
+    eskf.setPhatGravity(0.0000001f);
     
     eskf.setQVelocity(0.0025f);
     eskf.setQAngleError(0.00025f);
-    eskf.setQAccBias(0.000001f);
+    eskf.setQAccBias(0.0000001f);
     eskf.setQGyroBias(0.000001f);
     
     Matrix Rgpspos(3,3);
@@ -54,11 +51,12 @@
     Matrix Rgpsvel(3,3);
     setDiag(Rgpsvel,0.1f);
     
-    Matrix RaccConst(3,3);
-    setDiag(RaccConst,400.0f);
+    Matrix RaccConst(4,4);
+    setDiag(RaccConst,50.0f);
+    RaccConst(4,4) = 1.0f;
     //Matrix RgyroConst(2,2);
     //setDiag(RgyroConst,100);
-    
+    _t.start();
     float tgps = _t.read();
     while(1)
     {
@@ -82,7 +80,7 @@
             eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
             tgps = _t.read();
         }else{
-            eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),RaccConst);
+            eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),palt,RaccConst);
             //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst);
         }
         eskf.fuseErr2Nominal();