使用MPU6050卡尔曼滤波处理效果还好

谢谢了呃

真心英雄 发表于 2015-5-23 13:28
- B7 Z4 \& @2 N3 T, x; n9 M: `谢谢了呃

" X& a' @( Y. n, B" f4 C9 F+ o2 {' T新手，学习来了

多谢分享，学习学习

高大上的滤波，我也只能汗颜了

谢谢分享

不错，不错

多谢分享。。。

谢谢分享！

有实现代码吗？

我用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的值
$ Z& R; ]: N2 x9 z" }问题：1，我快速改变板子倾角时Angle的变化正常吗？
      2，Angle正常变化的时候是应该与Angle_ax的值相近吗？

$ d+ _# M2 H7 `' A  a% [8 E* j现在情况就是，就算我是在减小倾角，只要我快速地改变，它显示的倾角都会先增大再减小，而如果我慢速改变的话，倾角就会缓慢减小而不会出现中间的角度增大


*************读取数据********************
//定义MPU6050内部地址
# A$ d5 v) z2 ?#define        SMPLRT_DIV                0x19        //陀螺仪采样率 典型值 0X07 125Hz
#define        CONFIG                          0x1A        //低通滤波频率 典型值 0x00
# j9 \8 {1 g! v$ I+ Z$ b' x  N9 \. D#define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s
#define        ACCEL_CONFIG        0x1C        //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
/ T8 M4 ?; b% Q( M0 Z9 a#define INT_PIN_CFG     0x37
#define INT_ENABLE      0x38
, D0 B" X' p6 X3 U#define INT_STATUS      0x3A    //只读
9 r2 j+ {7 Q! u#define        ACCEL_XOUT_H        0x3B
1 t: O  [' t( d/ j% A6 Y* A#define        ACCEL_XOUT_L        0x3C
#define        ACCEL_YOUT_H        0x3D
#define        ACCEL_YOUT_L        0x3E
#define        ACCEL_ZOUT_H        0x3F
#define        ACCEL_ZOUT_L        0x40
#define        TEMP_OUT_H                0x41
6 ?$ F; n1 D, r7 P) L+ V#define        TEMP_OUT_L                0x42
#define        GYRO_XOUT_H    0x43
7 n8 B* A0 i+ G! o1 C#define        GYRO_XOUT_L                0x44       
#define        GYRO_YOUT_H        0x45
# m8 q9 D$ Z( N$ w7 q& i#define        GYRO_YOUT_L                0x46
# N' m! ?! I  W! q$ f# |+ J+ o#define        GYRO_ZOUT_H        0x47
#define        GYRO_ZOUT_L                0x48

/ U/ X5 D2 m- o4 b' c//读取寄存器原生数据
           
        MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);
        MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);
! M+ ?/ Z7 C9 X; o        MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
        MPU6050_Raw_Data.Temp =    (buf[6]<<8 | buf[7]);  
        MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
/ A7 W! n) }" u4 }: _0 g        MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
+ l) A' D2 S; |$ Y        MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
      
      
        //将原生数据转换为实际值，计算公式跟寄存器的配置有关
        MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
        MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;
- i7 s" d: O2 A$ }7 X( O) x' V4 I/ F        MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;
              MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5;   
* ~, z( ~" L/ E7 u* t7 k        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;   
$ ?9 S" G( b, J        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5;   
% r' x( [, v% g$ W( }% G1 }+ v    }
" k$ S9 w/ M; x6 L% |   

' Y' x4 c2 w6 h" R$ L3 P0 m
//******卡尔曼参数************
               
2 S# `2 |4 }6 `# v9 R! [const float Q_angle=0.001;  
- B! q( @% Z  Dconst float Q_gyro=0.003;
const float R_angle=0.5;
, U+ q% u4 F- u) |: j3 Gconst float dt=0.01;                          //dt为kalman滤波器采样时间;
const char  C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
" A4 c/ [! S9 i/ f/ ~float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
/ c) U9 J" s. m: H% \" S3 }3 y, c, ^$ ]! D
/*****************卡尔曼滤波**************************************************/
void Kalman_Filter(float Accel,float Gyro)               
4 \) h% A3 W% Y9 l* s: o3 }; j0 `{
2 g+ i( s2 a0 E4 _8 V        Angle+=(Gyro - Q_bias) * dt; //先验估计
( S( W6 O  {" v2 j, o* P/ a% P+ U: E       
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
7 D6 E4 l7 s& J; I, z+ I% n
9 @# J: g9 j) L* p- Q        Pdot[1]= -PP[1][1];
        Pdot[2]= -PP[1][1];
3 J) D* ~, P! c: q        Pdot[3]=Q_gyro;
. j5 w' M/ N& f7 v       
' A8 T" G7 e, w        PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
        PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
        PP[1][0] += Pdot[2] * dt;
7 `9 B' [8 l) s5 y        PP[1][1] += Pdot[3] * dt;
* U0 e# \6 I: o2 ^               
+ K1 @6 S) A8 m        Angle_err = Accel - Angle;        //zk-先验估计
! P) X$ N- F! x6 x3 @. ~       
+ s1 @. y1 @8 s9 o( q        PCt_0 = C_0 * PP[0][0];
' Z3 f- R1 }# W9 H6 ^  R        PCt_1 = C_0 * PP[1][0];
8 ^+ Q5 V- u# I& O) w       
        E = R_angle + C_0 * PCt_0;
       
+ b- Y! n6 s5 Q        K_0 = PCt_0 / E;
2 X1 X9 h4 g6 Y        K_1 = PCt_1 / E;
       
% m5 _+ v1 z" B        t_0 = PCt_0;
6 J+ C3 E* U0 c# o, l        t_1 = C_0 * PP[0][1];
; s+ v3 a" x2 f3 t# S
$ M" ?" E/ f. w0 A) G  q        PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差
! w! _0 y; w5 y5 d        PP[0][1] -= K_0 * t_1;
5 a3 Y( G6 }" _8 \( z" Y        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
/ X, E: C* X6 P               
4 n' K+ s0 T! e        Angle        += K_0 * Angle_err;         //后验估计
        Q_bias        += K_1 * Angle_err;         //后验估计
        Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度
" x) d1 s. H+ [7 m. {6 r
( D+ t* l' v6 C}
. b; x- J7 s1 @4 I0 _
******************倾角计算*****************
void Angle_Calculate(void)
{
7 d. s* L+ ^; N+ s0 f
/****************************加速度****************************************/
3 `/ K; ~: T5 Z  h2 j' \       
1 \) X$ Q3 g' y: @, w        Accel_x  =  MPU6050_Real_Data.Accel_X;          //读取X轴加速度
7 R$ c; h6 U  ?        Angle_ax = Accel_x*1.2*180/3.14;     //弧度转换为度
. S0 r7 ^2 c$ r8 m7 N3 X
/****************************角速度****************************************/
. e( q, H$ u; g% U- n3 `8 W       
- L- o( ?5 u  E. v         Gyro_y = MPU6050_Real_Data.Gyro_Y;             
时间dt，所以此处不用积分
/***************************卡尔曼融合*************************************/
        Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角
       
1 S! ?2 |2 v# z0 V1 |