Diff: run.cpp

Revision:

99:98b892ca70ec

Parent:

98:bdaa6bbfb1d9

Child:

100:7589b663d462

--- a/run.cpp	Wed Nov 03 14:59:26 2021 +0000
+++ b/run.cpp	Thu Nov 04 09:42:19 2021 +0000
@@ -8,6 +8,7 @@
     if(hilFlag == false){
         eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z);
     }
+    
     int n_init = 1000;
     for(int i = 0;i<n_init;i++){
         lsm.readAccel();
@@ -19,11 +20,13 @@
         agoffset[3] +=(lsm.gx * M_PI / 180.0f);
         agoffset[4] +=(lsm.gy * M_PI / 180.0f); 
         agoffset[5] +=(lsm.gz * M_PI / 180.0f); 
-        palt0 = lps.pressureToAltitudeMeters(lps.readPressureMillibars());
+        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
     }
     for(int i = 0;i<6;i++){
         agoffset[i] /= float(n_init);
     }
+    gps.Update();
+    gps.CalculateUnit();
     palt0 /= float(n_init);
     twelite.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
     //センサ正常性チェック
@@ -34,11 +37,11 @@
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
-    //EKFの共分散設定
+    //ESKFの共分散設定
     eskf.setGravity(0.0f,0.0f,9.8f);
     eskf.setPhatPosition(0.1f);
     eskf.setPhatVelocity(0.1f);
-    eskf.setPhatAngleError(0.1f);
+    eskf.setPhatAngleError(10.0f);
     eskf.setPhatAccBias(0.001f);
     eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
@@ -48,14 +51,16 @@
     eskf.setQAccBias(0.0000001f);
     eskf.setQGyroBias(0.000001f);
     
-    Matrix Rgpspos(3,3);
-    setDiag(Rgpspos,0.9f);
+    Matrix Rgpspos(2,2);
+    setDiag(Rgpspos,2.0f);
+    //Rgpspos(3,3) = 5.0f;
     Matrix Rgpsvel(3,3);
-    setDiag(Rgpsvel,0.1f);
+    setDiag(Rgpsvel,2.0f);
+    
     
     Matrix RaccConst(4,4);
     setDiag(RaccConst,50.0f);
-    RaccConst(4,4) = 1.0f;
+    RaccConst(4,4) = 5.0f;
     //Matrix RgyroConst(2,2);
     //setDiag(RgyroConst,100);
     _t.start();