完善资料让更多小伙伴认识你,还能领取20积分哦, 立即完善>
STM32F103RCT6使用HAL库通过I2C1读取MPU6050模块原始数据,卡尔曼滤波进行数据融合。卡尔曼滤波滞后很严重
卡尔曼滤波代码采用网上的资料,如下 #include "kalman.h" #include "mpu6050.h" #include "math.h" float Accel_x; //X轴加速度值暂存 float Accel_y; //Y轴加速度值暂存 float Accel_z; //Z轴加速度值暂存 float Gyro_x; //X轴陀螺仪数据暂存 float Gyro_y; //Y轴陀螺仪数据暂存 float Gyro_z; //Z轴陀螺仪数据暂存 //float Angle_gy; //由角速度计算的倾斜角度 float Angle_x_temp; //由加速度计算的x倾斜角度 float Angle_y_temp; //由加速度计算的y倾斜角度 float Angle_X_Final; //X最终倾斜角度 float Angle_Y_Final; //Y最终倾斜角度 //角度计算 void Angle_Calcu(void) { //范围为2g时,换算关系:16384 LSB/g //deg = rad*180/3.14 float x,y,z; Accel_x = GetData(MPU_ACCEL_XOUTH_REG); //x轴加速度值暂存,从寄存器读数据 MPU_ACCEL_XOUTH_REG = 0x3B Accel_y = GetData(MPU_ACCEL_YOUTH_REG); //y轴加速度值暂存,从寄存器读数据 MPU_ACCEL_YOUTH_REG = 0x3D Accel_z = GetData(MPU_ACCEL_ZOUTH_REG); //z轴加速度值暂存,从寄存器读数据 MPU_ACCEL_ZOUTH_REG = 0x3F Gyro_x = GetData(MPU_GYRO_XOUTH_REG ); //x轴陀螺仪值暂存,从寄存器读数据 MPU_GYRO_XOUTH_REG 0X43 Gyro_y = GetData(MPU_GYRO_YOUTH_REG); //y轴陀螺仪值暂存,从寄存器读数据 MPU_GYRO_YOUTH_REG 0X45 Gyro_z = GetData(MPU_GYRO_ZOUTH_REG); //z轴陀螺仪值暂存,从寄存器读数据 MPU_GYRO_ZOUTH_REG 0X47 //printf("原始数据 x 加速度x = %d, x 角速度 = %d n", Accel_x,Gyro_x); //处理x轴加速度 if(Accel_x<32764) x=Accel_x/16384; else x=1-(Accel_x-49152)/16384; //处理y轴加速度 if(Accel_y<32764) y=Accel_y/16384; else y=1-(Accel_y-49152)/16384; //处理z轴加速度 if(Accel_z<32764) z=Accel_z/16384; else z=(Accel_z-49152)/16384; //用加速度计算三个轴和水平面坐标系之间的夹角 // Angle_x_temp=(atan(y/z))*180/Pi; // Angle_y_temp=(atan(x/z))*180/Pi; Angle_x_temp=(atan2(z,y))*180/Pi; Angle_y_temp=(atan2(x, z))*180/Pi; //Angle_z_temp=(atan2(y, x))*180/Pi; //角度的正负号 if(Accel_x<32764) Angle_y_temp = +Angle_y_temp; if(Accel_x>32764) Angle_y_temp = -Angle_y_temp; if(Accel_y<32764) Angle_x_temp = +Angle_x_temp; if(Accel_y>32764) Angle_x_temp = -Angle_x_temp; if(Accel_z<32764) {} if(Accel_z>32764) {} //角速度 //向前运动 if(Gyro_x<32768) Gyro_x=-(Gyro_x/16.4);//陀螺仪范围为2000deg/s时,换算关系:16.4 LSB/(deg/s) //向后运动 if(Gyro_x>32768) Gyro_x=+(65535-Gyro_x)/16.4; //向前运动 if(Gyro_y<32768) Gyro_y=-(Gyro_y/16.4);//范围为2000deg/s时,换算关系:16.4 LSB/(deg/s) //向后运动 if(Gyro_y>32768) Gyro_y=+(65535-Gyro_y)/16.4; //向前运动 if(Gyro_z<32768) Gyro_z=-(Gyro_z/16.4);//范围为2000deg/s时,换算关系:16.4 LSB/(deg/s) //向后运动 if(Gyro_z>32768) Gyro_z=+(65535-Gyro_z)/16.4; //Angle_gy = Angle_gy + Gyro_y*0.025; //角速度积分得到倾斜角度.越大积分出来的角度越大 Kalman_Filter_X(Angle_x_temp,Gyro_x); //卡尔曼滤波计算X倾角 Kalman_Filter_Y(Angle_y_temp,Gyro_y); //卡尔曼滤波计算Y倾角 } //卡尔曼参数 float Q_angle = 0.001; float Q_gyro = 0.005; float R_angle = 0.5; float dt = 0.005;//dt为kalman滤波器采样时间; 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[4] ={0,0,0,0}; float PP[2][2] = { { 1, 0 },{ 0, 1 } }; void Kalman_Filter_X(float Accel,float Gyro) //卡尔曼函数 { Angle_X_Final += (Gyro - Q_bias) * dt; //先验估计 Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分 Pdot[1]= -PP[1][1]; Pdot[2]= -PP[1][1]; Pdot[3]= Q_gyro; PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分 PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差 PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle_X_Final; //zk-先验估计 PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; 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[0][1]; PP[0][0] -= K_0 * t_0; //后验估计误差协方差 PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle_X_Final += K_0 * Angle_err; //后验估计 Q_bias += K_1 * Angle_err; //后验估计 Gyro_x = Gyro - Q_bias; //输出值(后验估计)的微分=角速度 } void Kalman_Filter_Y(float Accel,float Gyro) //卡尔曼函数 { Angle_Y_Final += (Gyro - Q_bias) * dt; //先验估计 Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分 Pdot[1]=- PP[1][1]; Pdot[2]=- PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分 PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差 PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle_Y_Final; //zk-先验估计 PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; 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[0][1]; PP[0][0] -= K_0 * t_0; //后验估计误差协方差 PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle_Y_Final += K_0 * Angle_err; //后验估计 Q_bias += K_1 * Angle_err; //后验估计 Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度 } |
|
相关推荐
1个回答
|
|
卡尔曼滤波滞后严重的原因可能有以下几点:
1. 滤波器参数设置不当:卡尔曼滤波器的性能很大程度上取决于其参数设置,如过程噪声协方差矩阵(Q矩阵)和测量噪声协方差矩阵(R矩阵)。如果这些参数设置不当,可能导致滤波器性能下降,出现滞后现象。 2. 采样率较低:如果STM32F103RCT6与MPU6050之间的I2C通信速率较低,可能导致数据采样率不足,从而影响卡尔曼滤波器的性能。 3. 滤波器初始化问题:如果滤波器的状态估计和协方差矩阵在初始化时设置不当,可能导致滤波器在开始时就出现滞后现象。 4. 卡尔曼滤波器实现问题:如果卡尔曼滤波器的实现存在问题,如计算错误或逻辑错误,也可能导致滤波器性能下降。 为了解决这个问题,你可以尝试以下方法: 1. 调整卡尔曼滤波器的参数:尝试调整Q矩阵和R矩阵的值,以找到最佳的滤波器性能。 2. 提高采样率:检查STM32F103RCT6与MPU6050之间的I2C通信速率,尝试提高采样率以获得更好的数据。 3. 检查滤波器初始化:确保滤波器的状态估计和协方差矩阵在初始化时设置正确。 4. 检查卡尔曼滤波器实现:仔细检查卡尔曼滤波器的实现代码,确保没有计算错误或逻辑错误。 5. 使用其他滤波器:如果卡尔曼滤波器的性能仍然不理想,可以尝试使用其他滤波器,如扩展卡尔曼滤波器(EKF)或无迹卡尔曼滤波器(UKF),这些滤波器在处理非线性系统时可能表现更好。 6. 优化代码:检查你的代码,确保没有不必要的延迟或性能瓶颈,这可能会影响滤波器的性能。 通过以上方法,你应该能够找到导致卡尔曼滤波滞后的原因,并采取相应的措施来解决问题。 |
|
|
|
只有小组成员才能发言,加入小组>>
调试STM32H750的FMC总线读写PSRAM遇到的问题求解?
1202 浏览 1 评论
X-NUCLEO-IHM08M1板文档中输出电流为15Arms,15Arms是怎么得出来的呢?
1217 浏览 1 评论
625 浏览 2 评论
STM32F030F4 HSI时钟温度测试过不去是怎么回事?
470 浏览 2 评论
ST25R3916能否对ISO15693的标签芯片进行分区域写密码?
1146 浏览 2 评论
1680浏览 9评论
STM32仿真器是选择ST-LINK还是选择J-LINK?各有什么优势啊?
339浏览 4评论
STM32F0_TIM2输出pwm2后OLED变暗或者系统重启是怎么回事?
335浏览 3评论
329浏览 3评论
stm32cubemx生成mdk-arm v4项目文件无法打开是什么原因导致的?
306浏览 3评论
小黑屋| 手机版| Archiver| 电子发烧友 ( 湘ICP备2023018690号 )
GMT+8, 2024-9-1 14:40 , Processed in 0.862850 second(s), Total 79, Slave 63 queries .
Powered by 电子发烧友网
© 2015 bbs.elecfans.com
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 (电路图) 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证:合字B2-20210191