Diff: run.cpp

Revision:

100:7589b663d462

Parent:

99:98b892ca70ec

Child:

102:1c77ff6e2a85

--- a/run.cpp	Thu Nov 04 09:42:19 2021 +0000
+++ b/run.cpp	Fri Nov 05 13:48:22 2021 +0000
@@ -41,7 +41,7 @@
     eskf.setGravity(0.0f,0.0f,9.8f);
     eskf.setPhatPosition(0.1f);
     eskf.setPhatVelocity(0.1f);
-    eskf.setPhatAngleError(10.0f);
+    eskf.setPhatAngleError(0.1f);
     eskf.setPhatAccBias(0.001f);
     eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
@@ -56,6 +56,8 @@
     //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);
@@ -77,19 +79,23 @@
         //ekfの更新
         eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
         eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
-        if(tstart-tgps>0.2f){
-            if(hilFlag == true){
+        if(hilFlag == true){
+            if(tstart-tgps>0.2f){
                 getHilGPSval();
+                tgps = _t.read();
+                gpsUpdateFlag = true;
             }else{
-                getGPSval();
+                gpsUpdateFlag = false;
             }
-            eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
-            eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
-            tgps = _t.read();
         }else{
-            eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),palt,RaccConst);
-            //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst);
+            getGPSval;
         }
+        if(gpsUpdateFlag == true){
+            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
+            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
+            eskf.updateGPS(MatrixMath::Vector2mat(pi),MatrixMath::Vector2mat(vi),Rgps);
+        }
+        eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),palt,RaccConst);
         eskf.fuseErr2Nominal();
         
         PIDtick.loop();