你的浏览器版本过低,可能导致网站不能正常访问!
为了你能正常使用网站功能,请使用这些浏览器。

分享一个做平衡车是学卡尔曼滤波的资料

[复制链接]
laotui 发布时间:2015-5-23 10:33
使用MPU6050卡尔曼滤波处理效果还好
6050.jpg

卡尔曼滤波算法C语言实现.pdf

下载

1.19 MB, 下载次数: 216

赞 收藏 1 评论10 发布时间:2015-5-23 10:33

举报

10个回答
真心英雄 回答时间:2015-5-23 13:28:28
谢谢了呃
爱的简单 回答时间:2015-5-23 16:37:35
真心英雄 发表于 2015-5-23 13:289 e. [! G. _8 I" _. a+ ?
谢谢了呃

% s  X$ Q! c$ U* K# X新手,学习来了
lkl0305 回答时间:2015-5-23 16:59:27
多谢分享,学习学习
夏明smile 回答时间:2015-5-23 17:35:22
高大上的滤波,我也只能汗颜了
你好我好大家好! 回答时间:2015-5-24 00:22:07
谢谢分享
带你看世界 回答时间:2015-5-24 01:08:12
不错,不错
wwwhlw 回答时间:2015-5-24 11:11:57
多谢分享。。。
jj_feng 回答时间:2016-8-9 15:13:51
谢谢分享!
zhangxu56726 回答时间:2016-8-9 15:32:40
有实现代码吗?
孤独的文艺青年 回答时间:2016-8-9 18:35:18
我用printf("%0.2f    %0.2f    %0.2f\r\n",Angle,Angle_ax,Gyro_y);函数分别读取的加速度,角速度和倾角,我发现当我快速的改变板子的倾角的时候,比如快速变化10度,Angle(卡尔曼滤波后的倾角)瞬时变化非常快,可能会瞬间变化几十度然后回到正常角度,而当我缓慢变化10度的时候,Angle变化是正常线性变化到10度,在这两种变化中,Angle_ax(从MPU6050读取的值经过处理后的陀螺仪的Y轴数据)的变化一直都是线性正常的,并且Angle的值特别接近Angle_ax的值
  t0 p: y5 ]0 f8 A4 U问题:1,我快速改变板子倾角时Angle的变化正常吗?
4 `; l% K6 m0 K6 `. p- g' a      2,Angle正常变化的时候是应该与Angle_ax的值相近吗?
  [# a3 E. q( P2 u8 x2 G' u) E( D7 B6 G# S* ?( ]3 M$ r' H" p
现在情况就是,就算我是在减小倾角,只要我快速地改变,它显示的倾角都会先增大再减小,而如果我慢速改变的话,倾角就会缓慢减小而不会出现中间的角度增大, k+ K, F- N- j5 f" @8 }. J* }, f& g
7 s0 ]1 j/ w2 U3 E  y5 `- z) {! A
, m' m% i' f, W% g
*************读取数据********************) t- B7 a+ H8 f3 W/ v3 |) s9 x: T
//定义MPU6050内部地址
' s( _* p6 x3 @  j#define        SMPLRT_DIV                0x19        //陀螺仪采样率 典型值 0X07 125Hz3 Y- A( K( s/ t, w+ B# A' k, ]/ Z
#define        CONFIG                          0x1A        //低通滤波频率 典型值 0x00 4 p8 i' O. O* m2 i# B) p  Z8 z
#define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s
7 j2 f" |% I) K  f" t#define        ACCEL_CONFIG        0x1C        //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
# s  P4 t# P5 a; a" A+ i4 ^#define INT_PIN_CFG     0x37
5 O/ G1 P2 D  S#define INT_ENABLE      0x38# A" M) l" I" e7 a, i' w
#define INT_STATUS      0x3A    //只读/ p+ M4 a+ \# _% z% H5 U6 g
#define        ACCEL_XOUT_H        0x3B5 U- E1 m% {" B$ t" G3 b: b
#define        ACCEL_XOUT_L        0x3C( ]/ u/ [' S/ G( S3 Q' i5 ^
#define        ACCEL_YOUT_H        0x3D
$ L" Y) s5 y% m* r) P; _( J1 P#define        ACCEL_YOUT_L        0x3E
7 W& V. @1 U8 F$ \9 K4 R#define        ACCEL_ZOUT_H        0x3F- O. Y- [) g6 L$ p- o, b* n, q
#define        ACCEL_ZOUT_L        0x40
$ z' `6 Q* L0 M$ F; M: y8 z#define        TEMP_OUT_H                0x41
$ u, U4 i; P8 t( o: D#define        TEMP_OUT_L                0x420 P3 S; z) o" i
#define        GYRO_XOUT_H    0x43
# O% m  e9 c0 N7 r#define        GYRO_XOUT_L                0x44        % p6 Z% P2 G; U. S
#define        GYRO_YOUT_H        0x45
7 Q' c) \& T4 n#define        GYRO_YOUT_L                0x46  `" R$ h* P" d! P9 p
#define        GYRO_ZOUT_H        0x471 ]2 z9 m# A+ x3 a* Z/ j1 i
#define        GYRO_ZOUT_L                0x48
7 f, T1 Q8 n- f
& H( Y3 b2 O, O) ~5 m//读取寄存器原生数据
  e% e1 D1 M$ m  M7 M/ ~+ F           
7 Z' n0 W! d! A+ b" C& ^8 x6 X        MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);& H" L& u9 ]* N1 C1 b/ A2 m
        MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);
% f) o5 g& F, s0 c( I        MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
6 A* Y/ n4 ]* p* ]        MPU6050_Raw_Data.Temp =    (buf[6]<<8 | buf[7]);  
) S5 A6 f) X: c; L, N/ _0 s        MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
( B# }3 _: d& K& H8 A4 X        MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
: D) w7 Z5 r; y' I& `. L        MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
/ N( @5 K' }! \8 k4 C3 \      
7 P0 C( k! _; ?      
+ s% I' B7 O1 M; ?6 a3 @8 k" u        //将原生数据转换为实际值,计算公式跟寄存器的配置有关% Y+ Z! S" ^) m9 Z
        MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
# `4 S! W. ~, B1 e" t        MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;
1 i1 T' B  [: L0 e" ~. _* r$ ?/ j        MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0; 6 h1 s: Q, f$ [6 f
              MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5;    . A) p" L  ~7 [8 f/ _- a0 _6 f- Y: t
        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;   . Y+ @. K6 `2 k* x: c5 _! l
        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5;   8 i# o8 h. q* x
    }
0 r/ q8 ]) U! K0 J' Z   
8 P; p' t. S, m. v) o: O. z( I; V. x4 z. Q- h2 X
8 T: Q: l9 H% O6 d% N  m
//******卡尔曼参数************, N2 ]6 v/ |* y5 u$ w
               
7 C1 Q0 c  A0 m" l' {# yconst float Q_angle=0.001;    {0 ?; g6 ]# q  y, g) A
const float Q_gyro=0.003;
8 H5 R5 k# m8 l- y3 n/ v; ^const float R_angle=0.5;; H7 E' B: t3 o; P
const float dt=0.01;                          //dt为kalman滤波器采样时间;- R' E% T  r8 Y. v
const char  C_0 = 1;
9 m6 f' `  [6 L( ?) x- e. _float Q_bias, Angle_err;8 B8 Z" t  m0 I- X' A
float PCt_0, PCt_1, E;
; j% }) s: Q7 f3 Hfloat K_0, K_1, t_0, t_1;4 l# |* ^8 ]. f5 R
float Pdot[4] ={0,0,0,0};
- _$ j9 u; l) K% c# lfloat PP[2][2] = { { 1, 0 },{ 0, 1 } };
% P' T7 ~# F; ?) M0 v! I
& {7 b% |8 N+ i8 a) }/*****************卡尔曼滤波**************************************************/
# E, I( _- C) {0 pvoid Kalman_Filter(float Accel,float Gyro)                ! T' M, w1 Y8 o: J! Q/ z
{( A6 ~1 {6 d/ D0 k$ e5 z& j% N6 Y
        Angle+=(Gyro - Q_bias) * dt; //先验估计; w% }8 ^8 w& X' L5 e
        ( P& s! t" r( i- p( o/ [' ]
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
# S8 K+ {( g0 p+ `' s1 Q
# Z" W1 x5 v+ M3 v3 ^' z2 o        Pdot[1]= -PP[1][1];5 x/ E4 l( k" R: B  Y
        Pdot[2]= -PP[1][1];) q+ @1 f' ^+ T, ~/ D4 u# [8 K
        Pdot[3]=Q_gyro;
" ]; \: [9 l6 Z6 d& Q6 W0 y) A       
- ^$ c( G6 D0 K! j/ Z# V5 O$ g        PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
! d# u4 H4 b# O" t$ K* [        PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差2 Y8 S( L5 {3 X( r- W6 L7 }$ X. m  @
        PP[1][0] += Pdot[2] * dt;
! e$ m* S4 k9 p; v$ ?+ ~& k        PP[1][1] += Pdot[3] * dt;
5 B3 j/ g9 K  l: z$ I                5 t& p6 b' K% K6 E  `0 [
        Angle_err = Accel - Angle;        //zk-先验估计
' o; j0 [' ?" {  L( ?9 Y1 L  M       
& u3 w* f, j' y' B/ q" k        PCt_0 = C_0 * PP[0][0];
, g0 {7 M. _5 ]: K* k        PCt_1 = C_0 * PP[1][0];* Z3 |7 s8 w% J0 w9 W, Y4 v
       
% G- I: L4 O) b+ `        E = R_angle + C_0 * PCt_0;
0 N! {* V. O) J' q$ d       
5 R, N' `3 U2 A1 g5 u* e4 i        K_0 = PCt_0 / E;
3 Z7 v0 e) r9 H6 i        K_1 = PCt_1 / E;
' G! k/ U: e5 _: ]6 b0 b  x' D7 O        ! S( k8 C) S& ~
        t_0 = PCt_0;% C. x+ q8 n2 f  u  u" x. ~  V8 u
        t_1 = C_0 * PP[0][1];
8 ]5 k5 e* Y" a0 N& M% E+ [+ @* m) h  W* P
        PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差/ l6 b/ _: t4 |" r  B) R) v  ?: c3 z
        PP[0][1] -= K_0 * t_1;% k# }9 p! l: r
        PP[1][0] -= K_1 * t_0;/ ~: A8 D3 N( Y1 g/ E. i( D$ i  J' d
        PP[1][1] -= K_1 * t_1;
* F9 N8 N! C9 n, ~2 x, Y               
; T+ F7 Z# Y% |2 O7 S) P7 |9 S3 S: _" u        Angle        += K_0 * Angle_err;         //后验估计
  u: A- J3 E7 l) d+ {7 X2 \        Q_bias        += K_1 * Angle_err;         //后验估计8 ?* O) ~: E' C: f
        Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度
  a4 b# t$ A3 N" a" [
1 Q+ X7 `4 X- K  @9 d1 F$ J# j}1 d- k$ s( q  V5 j7 Y7 s" t, _7 E

2 a) B" W* e; N, u0 t9 U$ r8 L******************倾角计算****************** j0 e2 F2 T6 y% r( G8 z- Q
void Angle_Calculate(void)* ^9 q7 v3 k' c4 V) ?& ~
{1 f( T+ t2 j0 z2 O0 h' l

/ p2 W4 ]( j1 |+ V4 S7 }2 w2 H/****************************加速度****************************************/
1 v- f% V/ P* K( Y+ P        / h( N1 H1 j. E" f
        Accel_x  =  MPU6050_Real_Data.Accel_X;          //读取X轴加速度1 H- q; I0 x( y" A6 p7 j
        Angle_ax = Accel_x*1.2*180/3.14;     //弧度转换为度
  |% H0 e5 t, c6 k: e$ ]- s, `
/ F1 c% D0 @) @, U% n0 Q/****************************角速度****************************************/
& |% `5 |5 U  C* {' U9 ]4 X! J7 _0 }: y       
7 H: i# \# {5 |         Gyro_y = MPU6050_Real_Data.Gyro_Y;             
( ^' a- M: _( S$ K5 s$ C时间dt,所以此处不用积分9 Y4 I( V- a" T/ W
/***************************卡尔曼融合*************************************/( Z# F! U) b3 h6 [4 V# _# C: S4 `! X
        Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角5 [4 Z+ W7 Z7 l# }4 i8 Y/ i
       
, Y% |7 M$ W+ W) V- |; r$ [# i2 y! `3 g6 j
关于
我们是谁
投资者关系
意法半导体可持续发展举措
创新与技术
意法半导体官网
联系我们
联系ST分支机构
寻找销售人员和分销渠道
社区
媒体中心
活动与培训
隐私策略
隐私策略
Cookies管理
行使您的权利
官方最新发布
人形机器人运动控制、感知与智能配电
半导体创新技术与应用方向
EE架构与软件定义汽车
12V/48V 汽车智能配电(SPD)
区域控制单元(ZCU)与分区架构
关注我们
st-img 微信公众号
st-img 手机版