卡尔曼滤波，平衡车
我用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的值问题：1，我快速改变板子倾角时Angle的变化正常吗？ 2，Angle正常变化的时候是应该与Angle_ax的值相近吗？ *************读取数据********************//定义MPU6050内部地址#define SMPLRT_DIV 0x19 //陀螺仪采样率典型值 0X07 125Hz#define CONFIG 0x1A //低通滤波频率 典型值 0x00 #define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围 典型值 0x18 不自检 2000deg/s#define ACCEL_CONFIG 0x1C //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz#define INT_PIN_CFG 0x37#define INT_ENABLE 0x38#define INT_STATUS 0x3A //只读#define ACCEL_XOUT_H 0x3B#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#define TEMP_OUT_L 0x42#define GYRO_XOUT_H 0x43#define GYRO_XOUT_L 0x44 #define GYRO_YOUT_H 0x45#define GYRO_YOUT_L 0x46#define GYRO_ZOUT_H 0x47#define GYRO_ZOUT_L 0x48 //读取寄存器原生数据 MPU6050_Raw_Data.Accel_X = (buf<<8 | buf); MPU6050_Raw_Data.Accel_Y = (buf<<8 | buf); MPU6050_Raw_Data.Accel_Z = (buf<<8 | buf); MPU6050_Raw_Data.Temp = (buf<<8 | buf); MPU6050_Raw_Data.Gyro_X = (buf<<8 | buf); MPU6050_Raw_Data.Gyro_Y = (buf<<8 | buf); MPU6050_Raw_Data.Gyro_Z = (buf<<8 | buf); //将原生数据转换为实际值，计算公式跟寄存器的配置有关 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; MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X- gyroADC_X_offset)/65.5; MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y- gyroADC_Y_offset)/65.5; MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z- gyroADC_Z_offset)/65.5; } //******卡尔曼参数************ const float Q_angle=0.001;const float Q_gyro=0.003;const float R_angle=0.5;const float dt=0.01; //dt为kalman滤波器采样时间;const char C_0 = 1;float Q_bias, Angle_err;float PCt_0, PCt_1, E;float K_0, K_1, t_0, t_1;float Pdot ={0,0,0,0};float PP = { { 1, 0 },{ 0, 1 } }; /*****************卡尔曼滤波**************************************************/void Kalman_Filter(float Accel,float Gyro) { Angle+=(Gyro- Q_bias) * dt; //先验估计 Pdot=Q_angle- PP - PP; // Pk-先验估计误差协方差的微分 Pdot=-PP; Pdot=-PP; Pdot=Q_gyro; PP+= Pdot * dt; // Pk-先验估计误差协方差微分的积分 PP+= Pdot * dt; // =先验估计误差协方差 PP+= Pdot * dt; PP+= Pdot * dt; Angle_err= Accel - Angle; //zk-先验估计 PCt_0= C_0 * PP; PCt_1= C_0 * PP; E= R_angle + C_0 * PCt_0; K_0= PCt_0 / E; K_1= PCt_1 / E; t_0= PCt_0; t_1= C_0 * PP; PP-= K_0 * t_0; //后验估计误差协方差 PP-= K_0 * t_1; PP-= K_1 * t_0; PP-= K_1 * t_1; Angle += K_0 * Angle_err; //后验估计 Q_bias += K_1 * Angle_err; //后验估计 Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度 } ******************倾角计算*****************void Angle_Calculate(void){ /****************************加速度****************************************/ Accel_x= MPU6050_Real_Data.Accel_X; //读取X轴加速度 Angle_ax= Accel_x*1.2*180/3.14; //弧度转换为度 /****************************角速度****************************************/ Gyro_y = MPU6050_Real_Data.Gyro_Y;/***************************卡尔曼融合*************************************/ Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
现在情况就是，就算我是在减小倾角，只要我快速地改变，它显示的倾角都会先增大再减小，而如果我慢速改变的话，倾角就会缓慢减小而不会出现那些增大的角 感觉不错呀
页:
[1]