基于9轴惯性运动传感器的三阶卡尔曼滤波器算法

最近在玩九轴的惯性传感器,很是有挑战性.九轴说的是三轴的加速度计、三轴的陀螺仪以及三轴的磁场传感器。但是只是单纯的测出九个轴的数据没什么用,关键是要能够融合这九轴数据得出我们想要的结果。这里就运用三阶卡尔曼滤波算法来融合这九轴运动数据为三轴的角度。运用这三个角度可以用来做自平衡车或者四轴飞行器.

个人原创,版权所有,转载请注明原文出处,并保留原文链接:

https://www.embbnux.com/2015/01/30/9_dof_imu_with_kalman_filter_on_avr/

参考: kalman wikipedia    KalmanFilteredIMU

一、卡尔曼算法理解

其实如果不去考虑kalman算法是怎么来的,我们只需要知道有下面几个式子就可以了,具体意思可以看上面的wikipedia链接

kalman_embbnux_blog

二 卡尔曼滤波算法的实现

这里我的算法是运行在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[m*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]
}

实测可以准确的输出三轴的角度,为了获得更好的响应速度和跟踪精度还需调整参数.

《基于9轴惯性运动传感器的三阶卡尔曼滤波器算法》上有19条评论

  1. 大神你好,咨询你一个问题。卡尔曼滤波前的accel_compass2angle(&mpu_9dof_values[0],&mpu_9dof_values[6],am_angle);gyro2angle(&mpu_9dof_values[3],gyro_angle);这两个函数能解释一下吗?谢谢!

      1. 大佬能把变换代码发出来多好,即使单位不一样也可以知道怎么转化,现在一脸懵逼不知道磁场和加速度怎么计算的出来am_angle

  2. 矩阵乘法matc[i*3+j] += mata[i*3+m] * matb[i*3+j];是错误的,
    应该改成matc[i*3+j] += mata[i*3+m] * matb[m*3+j];

  3. 大神您好!
    问题1:
    在函数 KalmanFilter(am_angle_mat,gyro_angle_mat); 里所有矩阵 其实进行计算的 只有 3位,为什么您要把它们设成 9位的,而且取了
    第 0,4 ,8,位?

    问题2:
    楼上向您请教的 accel_compass2angle(&mpu_9dof_values[0],&mpu_9dof_values[6],am_angle);
    gyro2angle(&mpu_9dof_values[3],gyro_angle);

    可以请您解释详细一点么,如果不方便,可以发给我看看么?

    问题3:
    请问只用 加速度计和陀螺仪,不用磁力计,可以直接使用三阶卡尔曼滤波么,是不是效果不好?

    最近正在做 这个,遇到许多问题,看到您的帖子,十分想请教您。谢谢!

    1. gyro2angle和accel_compass2angle两个函数都是数据的换算,不同的传感器不一样。就是把传感器测到的数值转换为角度单位。主要运算为3×3矩阵的运势,取0 4 8的原因是因为计算中使用了对角单位矩阵,所以得出的值也是对角矩阵

    1. accel_compass2angle这个函数主要是把加速度计的数据和磁力计的数据做一次运算,转为粗略的角度数据,单位为度,gyro2angle这个是把陀螺仪的数据转为度每秒,主要都是角度的换算和单位换算,因为采用不同的传感器这两个就不同,所以我也就没写

  4. 谢谢,那加速度计和磁力计数据转为角度是怎么运算的呢,有没有参考公式啊,刚刚接触这方面,问的问题比较菜,请见谅哈

  5. gyro2angle这个函数可以将陀螺仪原始数据除以灵敏度转化得到度每秒,那加速度计和磁力计这个转化关系从文档里面好像没找到啊

  6. 我就是解算姿态要用到sqrt、atan2、asin这几个函数,
    可是无法include math.h
    卡在这里了。。。。。。。。

    本来想自己写这几个函数,Fast inverse square root都编译不了:
    warning: dereferencing type-punned pointer will break strict-aliasing rules
    这个算法得用两种类型的指正指向同一个地址。

    现在不知道怎么办了

  7. 这个线性卡尔曼滤波器,对数据进行处理后的结果确实好了很多,但是有一个问题在于,当姿态角(如Yaw)从-179变为+179度过程中,实际上只偏移了两度,但滤波后状态改变是从 -179 逐渐增大为 179 度,导致路径转了一圈,看上去偏移了 358 度。另外对于滤波器的参数 Hk, Bk 似乎都设为了单位矩阵。

  8. 大神您好!
    问题1:mpu_getValue6();
    compass_mgetValues();
    accel_compass2angle();
    gyro2angle();
    这几个函数原型是什么,在哪里可以查阅到?
    里面的参数是什么?

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注

Time limit is exhausted. Please reload the CAPTCHA.

此站点使用Akismet来减少垃圾评论。了解我们如何处理您的评论数据