Diff: run.cpp

Revision:

93:b827f78a717a

Parent:

92:00460f6df439

Child:

94:579e875a4244

Child:

95:43717535c354

--- a/run.cpp	Thu Oct 28 09:44:47 2021 +0000
+++ b/run.cpp	Fri Oct 29 13:30:24 2021 +0000
@@ -8,7 +8,22 @@
     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();
+        lsm.readMag();
+        lsm.readGyro();
+        agoffset[0] += lsm.ax * 9.8f;
+        agoffset[1] += lsm.ay * 9.8f;
+        agoffset[2] += lsm.az * 9.8f-9.8f;
+        agoffset[3] +=(lsm.gx * M_PI / 180.0f);
+        agoffset[4] +=(lsm.gy * M_PI / 180.0f); 
+        agoffset[5] +=(lsm.gz * M_PI / 180.0f); 
+    }
+    for(int i = 0;i<6;i++){
+        agoffset[i] /= 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]);
     //センサ正常性チェック
     //usaPack通信開始
     pc.Subscribe(0000, &(vp));
@@ -20,31 +35,39 @@
     Timer _t;
     _t.start();
     
-    float tgps = _t.read();
+    //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.setQVelocity(0.0025f);
+    eskf.setQAngleError(0.00025f);
+    eskf.setQAccBias(0.000001f);
+    eskf.setQGyroBias(0.000001f);
+    
     Matrix Rgpspos(3,3);
-    Rgpspos(1,1) = 5000.0f;
-    Rgpspos(2,2) = 5000.0f;
-    Rgpspos(3,3) = 5000.0f;
+    setDiag(Rgpspos,0.9f);
     Matrix Rgpsvel(3,3);
-    Rgpsvel(1,1) = 5000.0f;
-    Rgpsvel(2,2) = 5000.0f;
-    Rgpsvel(3,3) = 5000.0f;
+    setDiag(Rgpsvel,0.1f);
+    
     Matrix RaccConst(3,3);
-    RaccConst(1,1) = 980000.0f;
-    RaccConst(2,2) = 980000.0f;
-    RaccConst(3,3) = 980000.0f;
-    Matrix RgyroConst(2,2);
-    RgyroConst(1,1) = 100000.0f;
-    RgyroConst(2,2) = 100000.0f;
+    setDiag(RaccConst,400.0f);
+    //Matrix RgyroConst(2,2);
+    //setDiag(RgyroConst,100);
+    
+    float tgps = _t.read();
     while(1)
     {
         float tstart = _t.read();
         //センサの値を取得
         if(hilFlag == true){
             getHilIMUval();
+        }else{
             getIMUval();
-        }else{
-            //getIMUval();
         }
         //ekfの更新
         eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
@@ -59,10 +82,8 @@
             eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
             tgps = _t.read();
         }else{
-            //eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),RaccConst);
+            eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),RaccConst);
             //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst);
-            eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
-            eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
         }
         eskf.fuseErr2Nominal();