卡尔曼滤波（2）
一、卡尔曼算法理解
其实如果不去考虑kalman算法是怎么来的，我们只需要知道有下面几个式子就可以了，具体意思可以看wikipedia
二 卡尔曼滤波算法的实现
这里我的算法是运行在avr单片机上的，所以采用的是c语言写的。下面的代码是要放到avr的定时器中断测试刷新的。用示波器测试了一下,这个算法在16M晶振下的运行时间需要0.35ms,而数据采集需要3ms左右,所以选定定时器时间为8ms.之前也写过一阶的kalman算法，运用在自平衡车上，这边是三阶的，主要是矩阵运算.
//kalman.c
float dtTimer = 0.008;
float xk[9] = {0,0,0,0,0,0,0,0,0};
float pk[9] = {1,0,0,0,1,0,0,0,0};
float I[9] = {1,0,0,0,1,0,0,0,1};
float R[9] = {0.5,0,0,0,0.5,0,0,0,0.01};
float Q[9] = {0.005,0,0,0,0.005,0,0,0,0.001};
void matrix_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];
}
}
}
void matrix_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];
}
}
}
void matrix_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[i*3+j];
}
}
}
}
void KalmanFilter(float* am_angle_mat,float* gyro_angle_mat){
uint8_t i,j;
float yk[9];
float pk_new[9];
float K[9];
float KxYk[9];
float I_K[9];
float S[9];
float S_invert[9];
float sdet;
//xk = xk + uk
matrix_add(xk,gyro_angle_mat,xk);
//pk = pk + Q
matrix_add(pk,Q,pk);
//yk = xnew - xk
matrix_sub(am_angle_mat,xk,yk);
//S=Pk + R
matrix_add(pk,R,S);
//S求逆invert
sdet = 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_invert
matrix_multi(pk,S_invert,K);
//KxYk = K * Yk
matrix_multi(K,yk,KxYk);
//xk = xk + K * yk
matrix_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的定时器中断里面执行，进行递归运算.
//isr.c
#include "kalman.h"
float mpu_9dof_values[9]={0};
float am_angle[3];
float gyro_angle[3];
float am_angle_mat[9]={0,0,0,0,0,0,0,0,0};
float gyro_angle_mat[9]={0,0,0,0,0,0,0,0,0};
//8MS
ISR(TIMER0_OVF_vect){
//设置计数开始的初始值
TCNT0 = 130 ; //8ms
sei();
//采集九轴数据
//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]
}