基于9軸慣性運動傳感器的三階卡爾曼濾波器算法
最近在玩九軸的慣性傳感器，很是有挑戰性.九軸說的是三軸的加速度計、三軸的陀螺儀以及三軸的磁場傳感器。但是只是單純的測出九個軸的數據沒什么用，關鍵是要能夠融合這九軸數據得出我們想要的結果。這里就運用三階卡爾曼濾波算法來融合這九軸運動數據為三軸的角度。運用這三個角度可以用來做自平衡車或者四軸飛行器.
一、卡爾曼算法理解
其實如果不去考慮kalman算法是怎么來的，我們只需要知道有下面幾個式子就可以了，具體意思可以看上面的wikipedia鏈接
二 卡爾曼濾波算法的實現
這里我的算法是運行在avr單片機上的，所以采用的是c語言寫的。下面的代碼是要放到avr的定時器中斷測試刷新的。用示波器測試了一下,這個算法在16M晶振下的運行時間需要0.35ms,而數據采集需要3ms左右,所以選定定時器時間為8ms.之前也寫過一階的kalman算法，運用在自平衡車上，這邊是三階的，主要是矩陣運算.
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 | //kalman.cfloatdtTimer = 0.008;floatxk[9] = {0,0,0,0,0,0,0,0,0};floatpk[9] = {1,0,0,0,1,0,0,0,0};floatI[9] = {1,0,0,0,1,0,0,0,1};floatR[9] = {0.5,0,0,0,0.5,0,0,0,0.01};floatQ[9] = {0.005,0,0,0,0.005,0,0,0,0.001}; voidmatrix_add(float* mata,float* matb,float* matc){ uint8_t i,j; for(i=0; i<3; i++){ for(j=0; j<3; j++){ matc[i*3+j] = mata[i*3+j] + matb[i*3+j]; } }} voidmatrix_sub(float* mata,float* matb,float* matc){ uint8_t i,j; for(i=0; i<3; i++){ for(j=0; j<3; j++){ matc[i*3+j] = mata[i*3+j] - matb[i*3+j]; } }} voidmatrix_multi(float* mata,float* matb,float* matc){ uint8_t i,j,m; for(i=0; i<3; i++) { for(j=0; j<3; j++) { matc[i*3+j]=0.0; for(m=0; m<3; m++) { matc[i*3+j] += mata[i*3+m] * matb[m*3+j]; } } }} voidKalmanFilter(float* am_angle_mat,float* gyro_angle_mat){uint8_t i,j;floatyk[9];floatpk_new[9];floatK[9];floatKxYk[9];floatI_K[9];floatS[9];floatS_invert[9];floatsdet; //xk = xk + ukmatrix_add(xk,gyro_angle_mat,xk);//pk = pk + Qmatrix_add(pk,Q,pk);//yk = xnew - xkmatrix_sub(am_angle_mat,xk,yk);//S=Pk + Rmatrix_add(pk,R,S);//S求逆invertsdet = S[0] * S[4] * S[8] + S[1] * S[5] * S[6] + S[2] * S[3] * S[7] - S[2] * S[4] * S[6] - S[5] * S[7] * S[0] - S[8] * S[1] * S[3]; S_invert[0] = (S[4] * S[8] - S[5] * S[7])/sdet;S_invert[1] = (S[2] * S[7] - S[1] * S[8])/sdet;S_invert[2] = (S[1] * S[7] - S[4] * S[6])/sdet; S_invert[3] = (S[5] * S[6] - S[3] * S[8])/sdet;S_invert[4] = (S[0] * S[8] - S[2] * S[6])/sdet;S_invert[5] = (S[2] * S[3] - S[0] * S[5])/sdet; S_invert[6] = (S[3] * S[7] - S[4] * S[6])/sdet;S_invert[7] = (S[1] * S[6] - S[0] * S[7])/sdet;S_invert[8] = (S[0] * S[4] - S[1] * S[3])/sdet;//K = Pk * S_invertmatrix_multi(pk,S_invert,K);//KxYk = K * Ykmatrix_multi(K,yk,KxYk);//xk = xk + K * ykmatrix_add(xk,KxYk,xk);//pk = (I - K)*(pk)matrix_sub(I,K,I_K);matrix_multi(I_K,pk,pk_new);//update pk//pk = pk_new;for(i=0; i<3; i++){ for(j=0; j<3; j++){ pk[i*3+j] = pk_new[i*3+j]; } }} |
三 運用卡爾曼濾波器
這里的kalman濾波器是離散數字濾波器，需要迭代運算。這里把算法放到avr的定時器中斷里面執行，進行遞歸運算.
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 | //isr.c#include "kalman.h"floatmpu_9dof_values[9]={0};floatam_angle[3];floatgyro_angle[3];floatam_angle_mat[9]={0,0,0,0,0,0,0,0,0};floatgyro_angle_mat[9]={0,0,0,0,0,0,0,0,0}; //8MSISR(TIMER0_OVF_vect){//設置計數開始的初始值TCNT0 = 130 ; //8mssei();//采集九軸數據//mpu_9dof_values 單位為g與度/s//加速度計和陀螺儀mpu_getValue6(&mpu_9dof_values[0],&mpu_9dof_values[1],&mpu_9dof_values[2],&mpu_9dof_values[3],&mpu_hmc_values[4],&mpu_hmc_values[5]);//磁場傳感器compass_mgetValues(&mpu_9dof_values[6],&mpu_9dof_values[7],&mpu_9dof_values[8]); accel_compass2angle(&mpu_9dof_values[0],&mpu_9dof_values[6],am_angle);gyro2angle(&mpu_9dof_values[3],gyro_angle); am_angle_mat[0] = am_angle[0];am_angle_mat[4] = am_angle[1];am_angle_mat[8] = am_angle[2]; gyro_angle_mat[0] = gyro_angle[1];gyro_angle_mat[4] = - gyro_angle[0];gyro_angle_mat[8] = - gyro_angle[2]; //卡爾曼KalmanFilter(am_angle_mat,gyro_angle_mat); //輸出三軸角度//xk[0] xk[4] xk[8]} |
實測可以準確的輸出三軸的角度，為了獲得更好的響應速度和跟蹤精度還需調整參數.
編輯：admin 最后修改時間：2019-07-31
