找回密码
 注册
关于网站域名变更的通知
查看: 743|回复: 1
打印 上一主题 下一主题

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

[复制链接]

该用户从未签到

跳转到指定楼层
1#
发表于 2019-2-21 12:30 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式

EDA365欢迎您登录!

您需要 登录 才可以下载或查看,没有帐号?注册

x
基于9轴惯性运动传感器的三阶卡尔曼滤波器算法
/ r- F3 @2 S+ f! @( R, E, T7 c: G1 Y$ ]
最近在玩九轴的惯性传感器,很是有挑战性.九轴说的是三轴的加速度计、三轴的陀螺仪以及三轴的磁场传感器。但是只是单纯的测出九个轴的数据没什么用,关键是要能够融合这九轴数据得出我们想要的结果。这里就运用三阶卡尔曼滤波算法来融合这九轴运动数据为三轴的角度。运用这三个角度可以用来做自平衡车或者四轴飞行器.
一、卡尔曼算法理解
其实如果不去考虑kalman算法是怎么来的,我们只需要知道有下面几个式子就可以了,具体意思可以看上面的wikipedia链接
二 卡尔曼滤波算法的实现
这里我的算法是运行在avr单片机上的,所以采用的是c语言写的。下面的代码是要放到avr的定时器中断测试刷新的。用示波器测试了一下,这个算法在16M晶振下的运行时间需要0.35ms,而数据采集需要3ms左右,所以选定定时器时间为8ms.之前也写过一阶的kalman算法,运用在自平衡车上,这边是三阶的,主要是矩阵运算.
//kalman.c
/ s% `' J; u# w! L- [3 ]
float dtTimer   = 0.008;
& Y* ?/ @1 N  i- p6 Y5 P
float xk[9] = {0,0,0,0,0,0,0,0,0};

7 h1 x. ^8 n( `+ W
float pk[9] = {1,0,0,0,1,0,0,0,0};
- `* o* B& `/ T* y+ A) s
float I[9]  = {1,0,0,0,1,0,0,0,1};

/ R3 n7 R6 L# S* t* q' u; Y0 j
float R[9]  = {0.5,0,0,0,0.5,0,0,0,0.01};
- E  o# S+ V5 ^4 s& R7 t( m" }
float Q[9] = {0.005,0,0,0,0.005,0,0,0,0.001};
, w0 D! {5 m# a- E& r
' _, m7 \/ q2 e& E$ B' q
void matrix_add(float* mata,float* matb,float* matc){

) j+ n* i  S* \9 g; ]) e  ~
    uint8_t i,j;
  k4 _+ [$ ]9 p% H. t1 }
    for (i=0; i<3; i++){
8 |# e; `  t  ]2 I
       for (j=0; j<3; j++){

8 s7 M: a' U2 H1 V3 ^  h; j
          matc[i*3+j] = mata[i*3+j] + matb[i*3+j];

* v  \4 e, t7 n+ C/ u& Z
       }
' t$ w" ]- Q# s9 D9 y. Z
    }

  \' `" s/ K4 @( _% E/ v3 m
}
. C% X- v' [* E4 ?; }

6 u/ {. F5 t, E( ^
void matrix_sub(float* mata,float* matb,float* matc){

) p+ Q$ `" e1 p) J
    uint8_t i,j;

6 W5 w8 L$ B( Y( D" I$ ?  ?/ [7 h
    for (i=0; i<3; i++){

$ y9 l- d0 o1 b# Q, ?; p# c
       for (j=0; j<3; j++){

$ g) y6 t1 J9 B0 b3 P4 A
          matc[i*3+j] = mata[i*3+j] - matb[i*3+j];

$ n# V2 m( c! q4 S( `# U9 @
       }
! B( [- ~1 n1 s
    }

3 D0 l4 _: O: y: D
}
0 q7 I6 F+ m+ Y! P

2 E2 i+ @9 G% A( }$ D5 N
void matrix_multi(float* mata,float* matb,float* matc){

  |! \. E. x1 t6 l3 @
  uint8_t i,j,m;
' M' p$ ?7 N0 H% a7 K
  for (i=0; i<3; i++)

5 [: G, ^! N0 |/ r+ F
  {

! U+ s& s/ ], X% y  K+ q2 P
    for (j=0; j<3; j++)

/ Q% f7 W8 c5 x  Q9 Y4 W
   {

% ?  H4 ^" }1 R% P0 j
      matc[i*3+j]=0.0;
+ x# ~& m8 n4 ?& O! q6 O$ {: j, G5 E+ o
      for (m=0; m<3; m++)
9 z2 Z6 P3 S7 O
     {
. a* v7 E) K' Y5 u" i/ d. U0 }6 L% x
        matc[i*3+j] += mata[i*3+m] *  matb[m*3+j];

7 h1 n- r7 c8 t) ^3 G6 R2 w
      }
& o! G! y4 F8 t% K) k; {
    }
( B6 C( V8 N0 h0 ^0 P  d
}
6 F2 V( ^, i1 u) w, S+ Q
}

% L  e  D: R- r: a5 k7 y* o/ n
: s! a. Y) q" z! x  B6 @0 N4 b+ s& T- h
void KalmanFilter(float* am_angle_mat,float* gyro_angle_mat){

, o: M: b0 |, K4 e+ |
uint8_t i,j;

) n- N) |. f: ]* m# p
float yk[9];
6 B1 k. [; V' v1 _' ?4 s7 Q3 B
float pk_new[9];

# s* u6 I# p- O2 a: V
float K[9];

: V8 M! i1 Y7 B: a0 o7 G' a/ z
float KxYk[9];

# J1 ^6 c  s/ @
float I_K[9];
7 l1 Y$ l- z- i. Q
float S[9];

! n) ^3 |8 W1 z, M
float S_invert[9];
+ J9 |1 ~6 Q7 d! d( {. `2 ?
float sdet;
" k) z# a  ^- k# Y* i% o( o) ~

4 F1 E8 u1 |  g9 [
//xk = xk + uk
1 d) m5 @' r% ]) q* w; N1 x
matrix_add(xk,gyro_angle_mat,xk);

" H. {2 o+ F, ~: a9 y$ Y
//pk = pk + Q
* N4 t! A; q3 Z" f( R( L
matrix_add(pk,Q,pk);

9 A7 C; {4 w. S# m+ j
//yk =  xnew - xk
2 n- {! I2 J& ?) [% s9 w
matrix_sub(am_angle_mat,xk,yk);
' R4 D4 z5 l& g
//S=Pk + R

8 c: F: m2 q, \8 {$ x6 z6 V
matrix_add(pk,R,S);

5 |. k0 @$ V( X0 h/ |" Y
//S求逆invert

" t4 J+ j8 C% U$ R
sdet = S[0] * S[4] * S[8]
. B4 C" G' v+ [' ?
          + S[1] * S[5] * S[6]

/ y- f* ~& z. i& ^/ d3 j; d
          + S[2] * S[3] * S[7]
% _6 e% {+ L( ?. n, i* a$ p
          - S[2] * S[4] * S[6]

* u* Z$ G: l2 Y7 Z& U# [: S4 i4 x* A
          - S[5] * S[7] * S[0]

% ~# K( a, V2 G& s6 \' _& F4 u
          - S[8] * S[1] * S[3];
3 T- t% q' Z) M6 a( ]% g
! X; U# M% ?' A  S. z
S_invert[0] = (S[4] * S[8] - S[5] *  S[7])/sdet;
% x' F5 h! ^% v) }* l! U3 r
S_invert[1] = (S[2] * S[7] - S[1] *  S[8])/sdet;
0 Y5 D/ s: X2 w3 t# u
S_invert[2] = (S[1] * S[7] - S[4] *  S[6])/sdet;
6 f8 X1 f% T$ k* {

* G/ I# P) d7 I4 a
S_invert[3] = (S[5] * S[6] - S[3] *  S[8])/sdet;

7 \" X4 t4 u7 {1 P
S_invert[4] = (S[0] * S[8] - S[2] *  S[6])/sdet;

5 D6 g. X- C8 Y; _: ^
S_invert[5] = (S[2] * S[3] - S[0] *  S[5])/sdet;

4 A$ t: p2 |5 p8 B1 j
3 X% u$ ~! N$ X0 N. q* I
S_invert[6] = (S[3] * S[7] - S[4] *  S[6])/sdet;

; {: S1 B1 p6 V& r, i% N
S_invert[7] = (S[1] * S[6] - S[0] *  S[7])/sdet;

* q' k- |6 h) T& L2 g' @$ E6 ?
S_invert[8] = (S[0] * S[4] - S[1] *  S[3])/sdet;

+ u9 m' X2 U# \. M; t
//K = Pk * S_invert

# V$ C+ q5 C5 D
matrix_multi(pk,S_invert,K);
3 p/ J# L) t8 ^8 C+ Z) h+ P8 ?" H
//KxYk = K * Yk

$ ^3 w. u; k0 ]' w+ H, I& z0 l
matrix_multi(K,yk,KxYk);

7 _, {1 ~1 a( }- W- F( L5 |
//xk = xk + K * yk
9 ~4 o0 L2 z1 U4 V6 n$ @* x
matrix_add(xk,KxYk,xk);

9 C4 q) r# F1 l* k  d3 n, }1 U7 [6 s
//pk = (I - K)*(pk)
8 b/ u4 i! g2 Q; z& c
matrix_sub(I,K,I_K);

, Y& U/ s+ G# `  w
matrix_multi(I_K,pk,pk_new);
, w0 K9 v; m) v; o. E+ x
//update pk

2 ]1 V$ u: x3 j. x* B& P
//pk = pk_new;

. n& w( g4 E' J9 `
for (i=0; i<3; i++){
3 e1 S8 Z! z( e' H, q4 U
    for (j=0; j<3; j++){

, a% R8 G6 K0 B
        pk[i*3+j] = pk_new[i*3+j];

* @5 L6 l  S5 t2 T: {1 l& h# F
    }
/ m) ^& K9 K' i8 U$ ^- h1 D' ], F
  }
7 C8 V3 {7 `0 E# `
}
+ f1 y! q* s, J* e8 c8 I# ^

8 y4 G5 n" n/ h; [) ?
三 运用卡尔曼滤波器
这里的kalman滤波器是离散数字滤波器,需要迭代运算。这里把算法放到avr的定时器中断里面执行,进行递归运算.
//isr.c

5 }; v* M1 `( B" E6 L. n# K: l0 `; |8 i! @
#include "kalman.h"

/ C) l+ T4 M7 ?3 Q+ R/ x' `0 g
float mpu_9dof_values[9]={0};
" W4 R1 X0 e$ v; o+ R( W% q1 j
float am_angle[3];

+ `8 |% f! }* B* h. i8 S9 F# y
float gyro_angle[3];
  o5 S2 T' P% r
float am_angle_mat[9]={0,0,0,0,0,0,0,0,0};

( t: K" n0 s% j. y! |8 S- P, t
float gyro_angle_mat[9]={0,0,0,0,0,0,0,0,0};
  \, P* F' f# f6 Q4 E

* i% m! R, R  X
//8MS

1 C( V6 c' [) `0 E" f) c
ISR(TIMER0_OVF_vect){

+ R, Z8 V+ E) e
//设置计数开始的初始值

7 h! u7 t& I% F$ h& P6 ?7 u3 Z
TCNT0 = 130 ;  //8ms
' N1 ~" b  c  H- k) k6 ?
sei();

- _, _. l$ P4 U8 ~2 p" P8 H% g
//采集九轴数据
" `7 @% O% @4 A- e
//mpu_9dof_values 单位为g与度/s

! w  v. a- b6 n
//加速度计和陀螺仪
8 G# ^4 t4 s1 z" t* x& H
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]);

9 T% S7 P2 c# P7 ^( O
//磁场传感器
9 b1 |1 k# m- E. [
compass_mgetValues(&mpu_9dof_values[6],&mpu_9dof_values[7],&mpu_9dof_values[8]);

6 M7 B8 X, U- m- h: U; X! @7 h1 ?. U$ {3 a5 t# B9 Z3 r7 d  v
accel_compass2angle(&mpu_9dof_values[0],&mpu_9dof_values[6],am_angle);

4 N# J4 X6 a4 g* D1 n2 ?
gyro2angle(&mpu_9dof_values[3],gyro_angle);

+ s- j" G8 Z4 x. t. m" R+ v% P
am_angle_mat[0] = am_angle[0];

2 m; \: f" Y+ V
am_angle_mat[4] = am_angle[1];
8 w1 B# A6 ~7 I( t
am_angle_mat[8] = am_angle[2];
3 v& a" ?0 Y0 _1 P8 P5 U
9 v0 T! W# D8 Q7 y
gyro_angle_mat[0] = gyro_angle[1];

2 p5 g$ F* E# S+ a! _
gyro_angle_mat[4] = - gyro_angle[0];
4 C: ^( h, `. M9 A! G& \# Z4 i; O5 G
gyro_angle_mat[8] = - gyro_angle[2];

1 I2 o( F* ]5 A3 w- u3 P0 L& X, B+ p3 d+ N4 q9 J8 x, O
//卡尔曼

1 p& M: Q" m3 P* I) c. N
KalmanFilter(am_angle_mat,gyro_angle_mat);

) X  e& k7 C( w7 I/ h
# R7 C# ?% Y# c6 Q+ h3 a$ l
//输出三轴角度

- C5 c/ e* w$ f) V3 b
//xk[0] xk[4] xk[8]

5 k" L1 z! W3 ]1 ^6 W' W/ V7 d( R
}

6 ^, e+ L/ I+ P

9 |8 E+ I  h# r& o" C3 b% \$ H
实测可以准确的输出三轴的角度,为了获得更好的响应速度和跟踪精度还需调整参数.
4 r5 Z! t3 V* V3 T5 K

该用户从未签到

2#
发表于 2019-2-21 16:18 | 只看该作者
研究一下,谢谢分享
您需要登录后才可以回帖 登录 | 注册

本版积分规则

关闭

推荐内容上一条 /1 下一条

EDA365公众号

关于我们|手机版|EDA365电子论坛网 ( 粤ICP备18020198号-1 )

GMT+8, 2025-10-31 07:17 , Processed in 0.140625 second(s), 23 queries , Gzip On.

深圳市墨知创新科技有限公司

地址:深圳市南山区科技生态园2栋A座805 电话:19926409050

快速回复 返回顶部 返回列表