当前位置:首页 > 单片机 > 单片机
[导读]最近在玩九轴的惯性传感器,很是有挑战性.九轴说的是三轴的加速度计、三轴的陀螺仪以及三轴的磁场传感器。但是只是单纯的测出九个轴的数据没什么用,关键是要能够融合这九轴数据得出我们想要的结果。这里就运用三阶卡

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

一、卡尔曼算法理解

其实如果不去考虑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.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的定时器中断里面执行,进行递归运算.

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"
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]
}

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

本站声明: 本文章由作者或相关机构授权发布,目的在于传递更多信息,并不代表本站赞同其观点,本站亦不保证或承诺内容真实性等。需要转载请联系该专栏作者,如若文章内容侵犯您的权益,请及时联系本站删除。
换一批
延伸阅读

TDK株式会社(东京证券交易所代码:6762)新近推出B84742A*R725系列滤波器,扩展了其单相EMC滤波器产品组合。新系列滤波器适用于电压高达250V、额定电流从6A到30A的交流和直流应用,是工业和建筑领域日益...

关键字: 滤波器 DIN 导轨 电源

在下述的内容中,小编将会对滤波器的相关消息予以报道,如果滤波器是您想要了解的焦点之一,不妨和小编共同阅读这篇文章哦。

关键字: 滤波器 无源滤波器 有源滤波器

为增进大家对匹配滤波器的认识,本文将对匹配滤波器、匹配滤波器的详细理解予以介绍。

关键字: 滤波器 指数 匹配滤波器

为增进大家对模拟滤波器的认识,本文将对模拟滤波器、模拟滤波器的频率特性予以介绍。

关键字: 滤波器 指数 模拟滤波器

为增进大家对有源滤波器的认识,本文将对有源滤波器、有源滤波器的配置以及有源滤波器的应用予以介绍。

关键字: 滤波器 指数 有源滤波器

2023年12月28日,鼎阳科技正式发布SVA-TB01射频实验教学板。SVA-TB01采用模块化设计,使用者可以自由组合电路,适用于射频工程师入门学习和高校射频通信课程实验。

关键字: 射频 电路 滤波器

在现代社会中,电子设备的广泛应用给人们的生活带来了便利和舒适。然而,随着电子设备数量的增加,电磁干扰(Electromagnetic Interference,EMI)问题也日益突出。EMI滤波器作为一种重要的电磁干扰抑...

关键字: emi 滤波器

一直以来,单相电源滤波都是大家的关注焦点之一。因此针对大家的兴趣点所在,小编将为大家带来单相电源滤波的相关介绍,详细内容请看下文。

关键字: 滤波器 电源滤波器

今天,小编将在这篇文章中为大家带来电源滤波器的有关报道,通过阅读这篇文章,大家可以对它具备清晰的认识,主要内容如下。

关键字: 滤波器 电源滤波器

单相电源滤波器将是下述内容的主要介绍对象,通过这篇文章,小编希望大家可以对它的相关情况以及信息有所认识和了解,详细内容如下。

关键字: 滤波器 电源滤波器 单相电源滤波器
关闭
关闭