这些小活动你都参加了吗?快来围观一下吧!>>
电子产品世界 » 论坛首页 » 高校专区 » 坤创E-Geek/天科大新电社 » 【2021全国电赛】坤创1队记录:03 卡尔曼滤波算法mpu6050实现

共1条 1/1 1 跳转至

【2021全国电赛】坤创1队记录:03 卡尔曼滤波算法mpu6050实现

工程师
2021-07-29 20:51:40     打赏

1 引言

Kalman Filter是一个高效的递归滤波器,它可以实现从一系列的噪声测量中,估计动态系统的状态。

起源于Rudolf Emil Kalman在1960年的博士论文和发表的论文《A New Approach to Linear Eiltering and Prediction Problems》(《线性滤波与预测问题的新方法》)。并且最先在阿波罗登月计划轨迹预测上应用成功,此后kalman filter取得重****展和完善。

它的广泛应用已经超过30年,包括机器人导航,控制。传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等,近年来更被广泛应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。

2 实现

平衡小车处理角度     

/**************************************************************************
函数功能:简易卡尔曼滤波
入口参数:加速度、角速度
返回  值:无
**************************************************************************/
void Kalman_Filter(float Accel,float Gyro)
{
angle+=(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;//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+= K_0 * Angle_err; //后验估计
Q_bias+= K_1 * Angle_err; //后验估计
angle_dot   = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}



关键词: 2021全国电赛     卡尔曼滤波    

共1条 1/1 1 跳转至

回复

匿名不能发帖!请先 [ 登陆 注册 ]