使用MPU6050卡尔曼滤波处理效果还好
新手,学习来了
问题:1,我快速改变板子倾角时Angle的变化正常吗?
2,Angle正常变化的时候是应该与Angle_ax的值相近吗?
' 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内部地址
#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
#define ACCEL_CONFIG 0x1C //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
#define INT_PIN_CFG 0x37
#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
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F- O. Y- [) g6 L$ p- o, b* n, q
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x420 P3 S; z) o" i
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44 % p6 Z% P2 G; U. S
#define GYRO_YOUT_H 0x45
#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
//读取寄存器原生数据
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]);
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]);
MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
//将原生数据转换为实际值,计算公式跟寄存器的配置有关% Y+ Z! S" ^) m9 Z
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;
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
}
) 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
const float Q_angle=0.001; {0 ?; g6 ]# q y, g) A
const float Q_gyro=0.003;
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;
float Q_bias, Angle_err;8 B8 Z" t m0 I- X' A
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;4 l# |* ^8 ]. f5 R
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
/*****************卡尔曼滤波**************************************************/
void 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-先验估计误差协方差的微分
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;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差2 Y8 S( L5 {3 X( r- W6 L7 }$ X. m @
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
5 t& p6 b' K% K6 E `0 [
Angle_err = Accel - Angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];* Z3 |7 s8 w% J0 w9 W, Y4 v
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
! 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];
0 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;
Angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计8 ?* O) ~: E' C: f
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}1 d- k$ s( q V5 j7 Y7 s" t, _7 E
******************倾角计算****************** 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
/****************************加速度****************************************/
/ 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; //弧度转换为度
/****************************角速度****************************************/
Gyro_y = MPU6050_Real_Data.Gyro_Y;
时间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
; r$ [# i2 y! `3 g6 j