Revision 0:f41e4813b4c5, committed 2019-12-30

Comitter:

Joeatsumi

Date:

Mon Dec 30 16:39:22 2019 +0000

Commit message:

20191231

Changed in this revision

diff -r 000000000000 -r f41e4813b4c5 MPU6050.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Mon Dec 30 16:39:22 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Joeatsumi/code/MPU6050_alter/#44c458576810

diff -r 000000000000 -r f41e4813b4c5 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 30 16:39:22 2019 +0000
@@ -0,0 +1,517 @@
+//==================================================
+//Atitude estimation (kalman filter)
+//MPU board: mbed LPC1768
+//Accelerometer +Gyro sensor : GY-521
+//
+//Matrix functions were derived from トランジスタ技術2019年7月号
+//
+//2019/12/30 A.Toda
+//==================================================
+#include "mbed.h"
+#include "MPU6050.h"
+
+//==================================================
+#define RAD_TO_DEG          57.2957795f             // 180 / π
+#define MAX_MEAN_COUNTER 100
+#define ACC_X 1.3//offset of x-axi accelerometer  
+#define FRQ 100//Hz
+//==================================================
+
+DigitalOut led1(LED1);
+
+//Timer interrupt
+Ticker flipper;
+
+//Port Setting
+MPU6050 mpu(p9, p10);  //Accelerometer + Gyro
+                        //(SDA,SCLK)
+
+Serial pc(USBTX, USBRX);    //UART
+
+//==================================================
+//Accelerometer and gyro data
+//==================================================
+
+double acc[3]; //variables for accelerometer
+double gyro[3]; //variables for gyro
+
+double offset_gyro_x=0.0;
+double offset_gyro_y=0.0;
+
+double sum_gyro_x=0.0;
+double sum_gyro_y=0.0;
+
+//==================================================
+//Atitude data
+//==================================================
+double roll_and_pitch_acc[2][1];//atitude from accelerometer
+double roll_and_pitch[2][1];    //atitude from gyro and accelerometer
+
+double threshold_acc,threshold_acc_ini;
+
+//==================================================
+//Matrix definition
+//==================================================
+double MAT_A[2][2];
+double inv_MAT_A[2][2];
+
+double MAT_B[2][2];
+double inv_MAT_B[2][2];
+
+double MAT_C[2][2];
+double inv_MAT_C[2][2];
+
+double MAT_X_k[2][1];
+double pre_MAT_X_k[2][1];
+
+double COV_P[2][2];//covariance matrix of atitude angles
+double pre_COV_P[2][2];//covariance matrix of atitude angles
+double pre_COV_P_2[2][2];//covariance matrix of atitude angles
+double pre_COV_P_3[2][2];//covariance matrix of atitude angles
+
+double COV_Q[2][2];//covariance matrix of gyro inputs
+double COV_R[2][2];//covariance matrix of atitude angles from accelerometer
+
+//=========================================================
+// Matrix common functions
+//=========================================================
+//Matrix addition
+void mat_add(double *m1, double *m2, double *sol, int row, int column)
+{
+    for(int i=0; i<row; i++)
+    {
+        for(int j=0; j<column; j++)
+        {
+            sol[i*column + j] = m1[i*column + j] + m2[i*column + j];    
+        }    
+    }
+    return;
+}
+
+//Matrix subtraction
+void mat_sub(double *m1, double *m2, double *sol, int row, int column)
+{
+    for(int i=0; i<row; i++)
+    {
+        for(int j=0; j<column; j++)
+        {
+            sol[i*column + j] = m1[i*column + j] - m2[i*column + j];    
+        }    
+    }
+    return;
+}
+
+//Matrix multiplication
+void mat_mul(double *m1, double *m2, double *sol, int row1, int column1, int row2, int column2)
+{
+    for(int i=0; i<row1; i++)
+    {
+        for(int j=0; j<column2; j++)
+        {
+            sol[i*column2 + j] = 0;
+            for(int k=0; k<column1; k++)
+            {
+                sol[i*column2 + j] += m1[i*column1 + k]*m2[k*column2 + j];    
+            }
+        }    
+    }
+    return;
+}
+
+//Matrix transposition
+void mat_tran(double *m1, double *sol, int row_original, int column_original)
+{
+    for(int i=0; i<row_original; i++)
+    {
+        for(int j=0; j<column_original; j++)
+        {
+            sol[j*row_original + i] = m1[i*column_original + j];    
+        }    
+    }
+    return;
+}
+
+//Matrix scalar maltiplication
+void mat_mul_const(double *m1,double c, double *sol, int row, int column)
+{
+    for(int i=0; i<row; i++)
+    {
+        for(int j=0; j<column; j++)
+        {
+            sol[i*column + j] = c * m1[i*column + j];    
+        }    
+    }
+    return;
+}
+
+//Matrix inversion (by Gaussian elimination)
+void mat_inv(double *m, double *sol, int column, int row)
+{
+    //allocate memory for a temporary matrix
+    double* temp = (double *)malloc( column*2*row*sizeof(double) );
+    
+    //make the augmented matrix
+    for(int i=0; i<column; i++)
+    {
+        //copy original matrix
+        for(int j=0; j<row; j++)
+        {
+            temp[i*(2*row) + j] = m[i*row + j];  
+        }
+        
+        //make identity matrix
+        for(int j=row; j<row*2; j++)
+        {
+            if(j-row == i)
+            {
+                temp[i*(2*row) + j] = 1;
+            }    
+            else
+            {
+                temp[i*(2*row) + j] = 0;    
+            }
+        }
+    }
+
+    //Sweep (down)
+    for(int i=0; i<column; i++)
+    {
+        //pivot selection
+        double pivot = temp[i*(2*row) + i];
+        int pivot_index = i;
+        double pivot_temp;
+        for(int j=i; j<column;j++)
+        {
+            if( temp[j*(2*row)+i] > pivot )
+            {
+                pivot = temp[j*(2*row) + i];
+                pivot_index = j;
+            }    
+        }  
+        if(pivot_index != i)
+        {
+            for(int j=0; j<2*row; j++)
+            {
+                pivot_temp = temp[ pivot_index * (2*row) + j ];
+                temp[pivot_index * (2*row) + j] = temp[i*(2*row) + j];
+                temp[i*(2*row) + j] = pivot_temp;    
+            }    
+        }
+        
+        //division
+        for(int j=0; j<2*row; j++)
+        {
+            temp[i*(2*row) + j] /= pivot;    
+        }
+        
+        //sweep
+        for(int j=i+1; j<column; j++)
+        {
+            double temp2 = temp[j*(2*row) + i];
+            
+            //sweep each row
+            for(int k=0; k<row*2; k++)
+            {
+                temp[j*(2*row) + k] -= temp2 * temp[ i*(2*row) + k ];    
+            }    
+        }
+    }
+        
+    //Sweep (up)
+    for(int i=0; i<column-1; i++)
+    {
+        for(int j=i+1; j<column; j++)
+        {
+            double pivot = temp[ (column-1-j)*(2*row) + (row-1-i)];   
+            for(int k=0; k<2*row; k++)
+            {
+                temp[(column-1-j)*(2*row)+k] -= pivot * temp[(column-1-i)*(2*row)+k];    
+            }
+        }    
+    }     
+    
+    //copy result
+    for(int i=0; i<column; i++)
+    {
+        for(int j=0; j<row; j++)
+        {
+            sol[i*row + j] = temp[i*(2*row) + (j+row)];    
+        }    
+    }
+    free(temp);
+    return;
+}
+
+//==================================================
+//Gyro and accelerometer functions
+//==================================================
+//get data
+void  aquisition_sensor_values(double *a,double *g){
+    
+    float ac[3],gy[3];
+    
+    mpu.getAccelero(ac);//get acceleration (Accelerometer)
+                                //x_axis acc[0]
+                                //y_axis acc[1]
+                                //z_axis acc[2]
+    mpu.getGyro(gy);   //get rate of angle(Gyro)
+                      //x_axis gyro[0]
+                      //y_axis gyro[1]
+                      //z_axis gyro[2]
+        
+    //Invertion for direction of Accelerometer axis
+    ac[0]*=(-1.0);
+    ac[0]+=ACC_X;
+    
+    ac[2]*=(-1.0);
+        
+    //Unit convertion of rate of angle(radian to degree)
+    gy[0]*=RAD_TO_DEG;
+    gy[0]*=(-1.0);
+        
+    gy[1]*=RAD_TO_DEG;        
+        
+    gy[2]*=RAD_TO_DEG;
+    gy[2]*=(-1.0);
+  
+    for(int i=0;i<3;i++){
+        a[i]=double(ac[i]);
+        g[i]=double(gy[i]);
+        }
+    g[0]-=offset_gyro_x;//offset rejection
+    g[1]-=offset_gyro_y;//offset rejection
+    
+    return;
+    
+}
+
+//calculate offset of gyro
+void offset_calculation_for_gyro(){
+    
+    //Accelerometer and gyro setting 
+    mpu.setAcceleroRange(0);//acceleration range is +-2G
+    mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
+    
+    //calculate offset of gyro
+    for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){
+        aquisition_sensor_values(acc,gyro);
+        sum_gyro_x+=gyro[0];
+        sum_gyro_y+=gyro[1];
+        wait(0.01);
+        }
+    
+    offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER;
+    offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER;
+    
+    return;
+}
+
+//atitude calculation from acceleromter
+void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){
+    
+    //roll_and_pitch[0] = atan2(a[1], a[2])*RAD_TO_DEG;//roll
+    roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll
+    //roll_and_pitch[1] = atan(a[0]/sqrt( a[1]*a[1]+a[2]*a[2] ) )*RAD_TO_DEG;//pitch
+    roll_and_pitch[1] = atan2(a[0],sqrt( a[1]*a[1]+a[2]*a[2] ) )*RAD_TO_DEG;//pitch
+            
+    return;
+}
+
+//atitude calculation
+void atitude_update(){
+    
+    
+    aquisition_sensor_values(acc,gyro);//加速度、ジャイロの取得
+    
+    //入力の設定
+    double MAT_IN[2][1]={};
+    double pre_MAT_IN[2][1]={};
+    
+    MAT_IN[0][0]=gyro[0];
+    MAT_IN[1][0]=gyro[1];
+    
+    /*--------------------Time update phase 時間更新フェーズ--------------------*/
+    //A*x+B*u
+    
+    mat_mul(MAT_A[0], MAT_X_k[0],pre_MAT_X_k[0] ,2, 2, 2, 1);
+    mat_mul(MAT_B[0], MAT_IN[0],pre_MAT_IN[0] ,2, 2, 2, 1);
+    
+    mat_add(pre_MAT_X_k[0],pre_MAT_IN[0], pre_MAT_X_k[0], 2, 1);
+    
+    //A*P*A^T+B*P*B^T
+    double pre_COV_P_1[2][2]={};
+    double pre_COV_P_1_2[2][2]={};
+    
+    double pre_COV_P_2[2][2]={};
+    double pre_COV_P_2_2[2][2]={};
+    
+    mat_mul(MAT_A[0], COV_P[0],pre_COV_P_1[0] ,2, 2, 2, 2);
+    mat_mul(pre_COV_P_1[0], inv_MAT_A[0] , pre_COV_P_1_2[0] ,2, 2, 2, 2);
+    
+    
+    mat_mul(MAT_B[0], COV_Q[0],pre_COV_P_2[0] ,2, 2, 2, 2);
+    mat_mul(pre_COV_P_2[0], inv_MAT_B[0] , pre_COV_P_2_2[0] ,2, 2, 2, 2);
+    
+    mat_add(pre_COV_P_1_2[0],pre_COV_P_2_2[0], pre_COV_P[0], 2, 2);
+    
+    //pc.printf("1;Roll=%f,Pitch=%f\r\n",pre_COV_P[0][0],pre_COV_P[1][1]);
+    /*------------------------------------------------------------------------*/
+    
+    /*--------------------Measurement update phase 観測更新フェーズ--------------------*/  
+      
+    threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
+    
+    //加速度のノルムが9.8m/s^2付近かどうかの判定
+    if((threshold_acc>=0.95*threshold_acc_ini)&&(threshold_acc<=1.05*threshold_acc_ini)){
+              
+              //pc.printf("corrected\r\n");
+              led1=0;
+              
+              double GAIN_K[2][2]={};
+              double pre_GAIN_K_1[2][2]={};
+              double pre_GAIN_K_1_2[2][2]={};
+              double pre_GAIN_K_1_3[2][2]={};
+              double pre_GAIN_K_1_4[2][2]={};
+              
+              double pre_GAIN_K_2[2][2]={};
+              
+              //K=P*C^T*(C*P*C^T+R)^-1
+              //(C*P*C^T+R)^-1
+              mat_mul(MAT_C[0], pre_COV_P[0] , pre_GAIN_K_1[0] ,2, 2, 2, 2);
+              mat_mul(pre_GAIN_K_1[0], inv_MAT_C[0] , pre_GAIN_K_1_2[0] ,2, 2, 2, 2);
+              
+              mat_add(pre_GAIN_K_1_2[0],COV_R[0], pre_GAIN_K_1_3[0], 2, 2);
+              
+              mat_inv(pre_GAIN_K_1_3[0], pre_GAIN_K_1_4[0], 2, 2);
+              
+              //P*C^T
+              mat_mul(pre_COV_P[0], inv_MAT_C[0] , pre_GAIN_K_2[0] ,2, 2, 2, 2);
+              
+              //P*C^T*(C*P*C^T+R)^-1
+              mat_mul(pre_GAIN_K_2[0], pre_GAIN_K_1_4[0] , GAIN_K[0] ,2, 2, 2, 2);
+              
+              double pre_MAT_X_k_2[2][1]={};
+              double pre_MAT_X_k_2_2[2][1]={};
+              
+              //X=X+K(y-C*X)
+              
+              atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc[0]);
+              mat_sub(roll_and_pitch_acc[0], pre_MAT_X_k[0], pre_MAT_X_k_2[0], 2, 1);
+              
+              mat_mul(GAIN_K[0], pre_MAT_X_k_2[0] ,pre_MAT_X_k_2_2[0] ,2, 2, 2, 1);
+              
+              mat_add(pre_MAT_X_k[0], pre_MAT_X_k_2_2[0], MAT_X_k[0], 2, 1);
+              
+              //P=(I-K*C)*P
+              double MAT_UNIT[2][2]={};
+              /*initialize Unit matrix*/
+              MAT_UNIT[0][0]=1.0;
+              MAT_UNIT[0][1]=0.0;
+              MAT_UNIT[1][0]=0.0;
+              MAT_UNIT[1][1]=1.0;
+              /*----------------------*/
+              
+              mat_sub(MAT_UNIT[0], GAIN_K[0], pre_COV_P_3[0], 2, 2);
+              mat_mul(pre_COV_P_3[0], pre_COV_P[0], COV_P[0], 2, 2, 2, 2);
+              
+              
+        }else{
+            //pc.printf("Not corrected\r\n");
+            led1=1;
+            
+            MAT_X_k[0][0]=pre_MAT_X_k[0][0];
+            MAT_X_k[1][0]=pre_MAT_X_k[1][0];
+            
+            COV_P[0][0]=pre_COV_P[0][0];
+            COV_P[1][1]=pre_COV_P[1][1];
+            
+            }
+    
+    pc.printf("%f,%f,Roll=%f,Pitch=%f\r\n",threshold_acc,threshold_acc_ini,MAT_X_k[0][0],MAT_X_k[1][0]);
+    //pc.printf("2;Roll=%f,Pitch=%f\r\n",COV_P[0][0],COV_P[1][1]);
+    
+    /*--------------------------------------------------------------------*/
+        
+    return;
+
+}
+
+
+//==================================================
+//Main
+//==================================================
+int main() {
+    
+    //UART initialization
+    pc.baud(115200);
+    
+    //gyro and accelerometer initialization
+    offset_calculation_for_gyro();
+    
+    //identify initilal atitude
+    aquisition_sensor_values(acc,gyro);
+    atitude_estimation_from_accelerometer(acc,MAT_X_k[0]);
+    
+    threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
+    
+    /*Initialize MAT_A*/
+    MAT_A[0][0]=1.0;
+    MAT_A[0][1]=0.0;
+    MAT_A[1][0]=0.0;
+    MAT_A[1][1]=1.0;
+    
+    mat_tran(MAT_A[0], inv_MAT_A[0], 2, 2);
+    /*----------------*/
+
+    /*Initialize MAT_B*/
+    MAT_B[0][0]=1.0/FRQ;
+    MAT_B[0][1]=0.0;
+    MAT_B[1][0]=0.0;
+    MAT_B[1][1]=1.0/FRQ;
+    
+    mat_tran(MAT_B[0], inv_MAT_B[0], 2, 2);
+    /*----------------*/
+
+    /*Initialize MAT_C*/
+    MAT_C[0][0]=1.0;
+    MAT_C[0][1]=0.0;
+    MAT_C[1][0]=0.0;
+    MAT_C[1][1]=1.0;
+
+    mat_tran(MAT_C[0], inv_MAT_C[0], 2, 2);
+    /*----------------*/
+    
+    /*
+    これら共分散の値は仮の値である。
+    */
+    /*Initialize COV_P*/
+    COV_P[0][0]=1.0;
+    COV_P[0][1]=0.0;
+    COV_P[1][0]=0.0;
+    COV_P[1][1]=1.0;
+    /*----------------*/
+    
+    /*Initialize COV_Q*/
+    COV_Q[0][0]=0.01;
+    COV_Q[0][1]=0.0;
+    COV_Q[1][0]=0.0;
+    COV_Q[1][1]=0.01;
+    /*----------------*/
+    
+    /*Initialize COV_R*/
+    COV_R[0][0]=0.001;
+    COV_R[0][1]=0.0;
+    COV_R[1][0]=0.0;
+    COV_R[1][1]=0.001;
+    /*----------------*/
+    
+    pc.printf("Roll=%f,Pitch=%f\r\n",COV_P[0][0],COV_P[1][1]);
+    
+    //Ticker
+    flipper.attach(&atitude_update, 0.01);
+    
+    while(1) {
+        wait(0.01);
+        
+    }
+}

diff -r 000000000000 -r f41e4813b4c5 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Dec 30 16:39:22 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e1686b8d5b90
\ No newline at end of file