基于9軸慣性運(yùn)動(dòng)傳感器的三階卡爾曼濾波器算法
最近在玩九軸的慣性傳感器，很是有挑戰(zhàn)性.九軸說(shuō)的是三軸的加速度計(jì)、三軸的陀螺儀以及三軸的磁場(chǎng)傳感器。但是只是單純的測(cè)出九個(gè)軸的數(shù)據(jù)沒(méi)什么用，關(guān)鍵是要能夠融合這九軸數(shù)據(jù)得出我們想要的結(jié)果。這里就運(yùn)用三階卡爾曼濾波算法來(lái)融合這九軸運(yùn)動(dòng)數(shù)據(jù)為三軸的角度。運(yùn)用這三個(gè)角度可以用來(lái)做自平衡車或者四軸飛行器.
一、卡爾曼算法理解
其實(shí)如果不去考慮kalman算法是怎么來(lái)的，我們只需要知道有下面幾個(gè)式子就可以了，具體意思可以看上面的wikipedia鏈接
二 卡爾曼濾波算法的實(shí)現(xiàn)
這里我的算法是運(yùn)行在avr單片機(jī)上的，所以采用的是c語(yǔ)言寫(xiě)的。下面的代碼是要放到avr的定時(shí)器中斷測(cè)試刷新的。用示波器測(cè)試了一下,這個(gè)算法在16M晶振下的運(yùn)行時(shí)間需要0.35ms,而數(shù)據(jù)采集需要3ms左右,所以選定定時(shí)器時(shí)間為8ms.之前也寫(xiě)過(guò)一階的kalman算法，運(yùn)用在自平衡車上，這邊是三階的，主要是矩陣運(yùn)算.
| 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]; } }} |
三 運(yùn)用卡爾曼濾波器
這里的kalman濾波器是離散數(shù)字濾波器，需要迭代運(yùn)算。這里把算法放到avr的定時(shí)器中斷里面執(zhí)行，進(jìn)行遞歸運(yùn)算.
| 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){//設(shè)置計(jì)數(shù)開(kāi)始的初始值TCNT0 = 130 ; //8mssei();//采集九軸數(shù)據(jù)//mpu_9dof_values 單位為g與度/s//加速度計(jì)和陀螺儀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]);//磁場(chǎng)傳感器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]} |
實(shí)測(cè)可以準(zhǔn)確的輸出三軸的角度，為了獲得更好的響應(yīng)速度和跟蹤精度還需調(diào)整參數(shù).
編輯：admin 最后修改時(shí)間：2019-07-31
