ドローン用計測制御基板の作り方 vol.1 ハードウェア編 p.19掲載 カルマンフィルタによる姿勢推定
Dependencies: mbed MPU6050_alter
main.cpp
- Committer:
- Joeatsumi
- Date:
- 2019-12-30
- Revision:
- 0:f41e4813b4c5
File content as of revision 0:f41e4813b4c5:
//================================================== //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); } }