Diff: setup.cpp

Revision:

68:b9f6938fab9d

Parent:

65:ea184054e659

Child:

70:99f974d8960e

--- a/setup.cpp	Tue Jun 22 02:45:43 2021 +0000
+++ b/setup.cpp	Mon Jun 28 01:40:26 2021 +0000
@@ -33,25 +33,53 @@
     wait(5);
     pc.serial.printf("Acc and Gyro Calibration Start\r\n");
     
-    int gxs = 0;
-    int gys = 0;
-    int gzs = 0;
     int iter_n = 10000;
     
+    long axs = 0;
+    long ays = 0;
+    long azs = 0;
+    double axs2 = 0.0f;
+    double ays2 = 0.0f;
+    double azs2 = 0.0f;    
+    long gxs = 0;
+    long gys = 0;
+    long gzs = 0;
+    double gxs2 = 0.0f;
+    double gys2 = 0.0f;
+    double gzs2 = 0.0f;
     for(int i = 0;i<iter_n ;i++)
     {
-        accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
+        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+        axs += ax;
+        ays += ay;
+        azs += az;
+        axs2 += double(ax*ax)/iter_n;
+        ays2 += double(ay*ay)/iter_n;
+        azs2 += double(az*az)/iter_n;
+        
         gxs += gx;
         gys += gy;
-        gzs -= gz;
+        gzs += gz;
+        gxs2 += double(gx*gx)/iter_n;
+        gys2 += double(gy*gy)/iter_n;
+        gzs2 += double(gz*gz)/iter_n;
         //wait(0.01);
     }
+    axs = axs /iter_n; 
+    ays = ays /iter_n; 
+    azs = azs /iter_n;
     gxs = gxs /iter_n; 
     gys = gys /iter_n; 
-    gzs = gzs /iter_n; 
-    agoffset[3] = gxs;
-    agoffset[4] = gys;
-    agoffset[5] = gzs;
+    gzs = gzs /iter_n;
+    double var_accx = (axs2 - double(axs*axs))/ ACCEL_SSF / ACCEL_SSF;
+    double var_accy = (ays2 - double(ays*ays))/ ACCEL_SSF / ACCEL_SSF;
+    double var_accz = (azs2 - double(azs*azs))/ ACCEL_SSF / ACCEL_SSF;
+    double var_gyrox = (gxs2 - double(gxs*gxs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f;
+    double var_gyroy = (gys2 - double(gys*gys))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f; 
+    double var_gyroz = (gzs2 - double(gzs*gzs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f;
+    pc.serial.printf("AccCovariance : %f, %f, %f \r\n",var_accx,var_accy,var_accz); 
+    pc.serial.printf("GyroCovariance : %f, %f, %f \r\n",var_gyrox,var_gyroy,var_gyroz);
+    
     pc.serial.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
     
     
@@ -78,7 +106,7 @@
     magCalibrator.getExtremes(magbiasMin,magbiasMax);
     pc.serial.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
     pc.serial.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
-    magCalibrator.getExtremes(magbiasMin,magbiasMax);
+    magCalibrator.setExtremes(magbiasMin,magbiasMax);
     
     pc.serial.printf("Calculating pitch/roll Offset \r\n");  
     //姿勢オフセットを計算
@@ -86,6 +114,14 @@
     rpy_align.x = 0.0f*M_PI/180.0f;
     float ave_pitch = 0.0f;
     float ave_roll = 0.0f;
+    ekf.Q(4,4) = 0.00001f;
+    ekf.Q(5,5) = 0.00001f;
+    ekf.Q(6,6) = 0.00001f;
+    ekf.Qab(1,1) = 0.0f;
+    ekf.Qab(2,2) = 0.0f;
+    ekf.Qab(3,3) = 0.0f;
+    getIMUval();
+    ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
     Timer _t;
     _t.start();
     for (int i = 0 ; i < 2200; i++)
@@ -93,10 +129,12 @@
         float tstart = _t.read();
         //姿勢角を更新
         getIMUval();
-        //ekf.updateBetweenMeasures(gyro, att_dt);
-        //ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
-        //ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
-        //ekf.computeAngles(rpy, rpy_g, rpy_align);
+        ekf.updateQhat(gyro, att_dt);
+        ekf.updateErrState(gyro, att_dt);
+        ekf.updateStaticAccMeasures(acc,accref);
+        ekf.fuseErr2Qhat();
+        ekf.updateMagMeasures(mag);
+        ekf.computeAngles(rpy, rpy_align);
         if(i>199)
         {
             ave_pitch += rpy.y;