Diff: main.cpp

Revision:

37:dad55cf05e3d

Parent:

36:e68ce293306e

Child:

38:acc7cdcf56dd

--- a/main.cpp	Wed Mar 03 03:34:11 2021 +0000
+++ b/main.cpp	Wed Mar 03 09:32:10 2021 +0000
@@ -39,6 +39,7 @@
 
 Matrix qhat(4,1);
 Matrix Phat(4,4);
+Matrix Qgyro(3,3);
 Matrix Racc(3,3);
 
 
@@ -150,7 +151,7 @@
         //MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
 }
 
-void updateBetweenMeasures(){
+void getIMUval(){
         // gx gy gz ax ay az
         accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
         ax = ax - offsetVal[0];
@@ -160,14 +161,18 @@
         gy = gy - offsetVal[4];
         gz = gz - offsetVal[5];
         // 加速度値を分解能で割って加速度(G)に変換する
-        acc_x = ax / ACCEL_SSF;  //FS_SEL_0 16,384 LSB / g
-        acc_y = ay / ACCEL_SSF;
-        acc_z = az / ACCEL_SSF;
+        acc_x = float(ax) / ACCEL_SSF;  //FS_SEL_0 16,384 LSB / g
+        acc_y = float(ay) / ACCEL_SSF;
+        acc_z = float(az) / ACCEL_SSF;
         // 角速度値を分解能で割って角速度(rad per sec)に変換する
         gyro_x = float(gx) / GYRO_SSF * 0.0174533f;  // (rad/s)
         gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
         gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
+}
+
+void updateBetweenMeasures(){
         
+        getIMUval();
 
         Matrix phi(4,4);
         phi <<  1.0               << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
@@ -179,6 +184,95 @@
         float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
         qhat *= (1.0f/ qnorm);
         
+        float q0 = qhat.getNumber( 1, 1 );
+        float q1 = qhat.getNumber( 2, 1 ); 
+        float q2 = qhat.getNumber( 3, 1 ); 
+        float q3 = qhat.getNumber( 4, 1 ); 
+        
+        Matrix B(4,3);
+        B   << q1  << q2 << q3
+            <<-q0  << q3 <<-q2
+            <<-q3  <<-q0 << q1             
+            << q2  <<-q1 <<-q0;
+        B *= 0.5f;
+        Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
+        
+        
+}
+
+void updateAcrossMeasures(){
+        getIMUval();
+        
+        Matrix uacc(3,1);
+        Matrix vacc(3,1);
+        
+        uacc << 0 << 0 << -1;
+        float accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
+        vacc << acc_x/accnorm << acc_y/accnorm << acc_z/accnorm;
+        
+        float q0 = qhat.getNumber( 1, 1 );
+        float q1 = qhat.getNumber( 2, 1 ); 
+        float q2 = qhat.getNumber( 3, 1 ); 
+        float q3 = qhat.getNumber( 4, 1 ); 
+        
+        Matrix A1(3,3);
+        A1 << q0 << q3 << -q2
+           <<-q3 << q0 << q1
+           <<q2  <<-q1 <<q0;
+        A1 *= 2.0f;
+        
+        Matrix A2(3,3);   
+        A2 << q1 << q2 << q3
+           << q2 <<-q1 << q0
+           << q3 <<-q0 <<-q1;
+        A2 *= 2.0f;
+        
+        Matrix A3(3,3);
+        A3 <<-q2 << q1 <<-q0
+           << q1 << q2 << q3
+           << q0 << q3 <<-q2;
+        A3 *= 2.0f;
+        
+        Matrix A4(3,3);
+        A4 <<-q3 << q0 << q1
+           <<-q0 <<-q3 << q2
+           << q1 << q2 << q3;
+        A4 *= 2.0f;
+        
+        Matrix H(3,4);
+
+        Matrix ab1(A1*uacc);
+        Matrix ab2(A2*uacc);
+        Matrix ab3(A3*uacc);
+        Matrix ab4(A4*uacc);
+
+        H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
+          << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
+          << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
+        
+        Matrix D(3,3);
+        D << q0*q0 + q1*q1 - q2*q2 - q3*q3
+          << 2*(q1*q2 + q0*q3)
+          << 2*(q1*q3 - q0*q2)
+          << 2*(q1*q2 - q0*q3)
+          << q0*q0 - q1*q1 + q2*q2 - q3*q3
+          << 2*(q2*q3 + q0*q1)
+          << 2*(q1*q3 + q0*q2)
+          << 2*(q2*q3 - q0*q1)
+          << q0*q0 - q1*q1 - q2*q2 + q3*q3;
+        
+        Matrix err(3,1);
+        err = vacc-D*uacc;
+        
+        Matrix K(4,3);
+        K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Racc);
+        
+        Matrix dq(4,1);
+        dq = K*err;
+        qhat = qhat-dq;
+        
+        float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
+        qhat *= (1.0f/ qnorm);
 }
 
 void computeAngles()
@@ -224,7 +318,11 @@
          << 0 << 0.0001 <<0 <<0
          << 0 << 0 <<0.0001 <<0
          << 0 << 0 << 0 << 0.0001;
-         
+    
+    Qgyro << 0.0001 << 0 <<0
+          << 0 << 0.0001 <<0
+          << 0 << 0 <<0.0001;     
+    
     Racc << 0.0001 << 0 <<0
          << 0 << 0.0001 <<0
          << 0 << 0 <<0.0001;
@@ -232,7 +330,7 @@
     
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
-    //pc.baud(115200);
+    pc.baud(115200);
     //sd.baud(115200);
     accelgyro.initialize();
     //加速度計のフルスケールレンジを設定
@@ -249,6 +347,7 @@
         //姿勢角を更新
         //updateAttitude();
         updateBetweenMeasures();
+        updateAcrossMeasures();
         computeAngles();
         PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する