【免费】卡尔曼滤波算法的程序实现和推导过程1资源-CSDN文库

需积分: 0 96 浏览量 2022-08-04 15:11:10 上传评论收藏 67KB PDF 举报

资源推荐

资源详情

资源评论

卡尔曼滤波算法的程序实现和推导过程

--- 蒋海林（ QQ：280586940）---

卡尔曼滤波算法由匈牙利裔美国数学家鲁道夫·卡尔曼（ Rudolf Emil Kalman ）创立，这个数学家特么牛

逼，1930 年出生，现在还能走能跳，吃啥啥麻麻香，但他的卡尔曼滤波算法已经广泛应用在航空航天，导

弹发射，卫星在轨运行等很多高大上的应用中。

让我们一边膜拜一边上菜吧，下面就是卡尔曼滤波算法的经典程序，说是经典，因为能正常运行的程序都

长得差不多，在此向原作者致敬。看得懂的，帮我纠正文中的错误；不太懂的，也不要急，让我慢慢道来。

最后希望广大朋友转载时，能够保留我的联系方式，一则方便后续讨论共同进步，二则支持奉献支持正能

量。

void Kalman_Filter(float Gyro,float Accel)/// 角速度，加速度

{

/// 陀螺仪积分角度（先验估计）

Angle_Final = Angle_Final + (Gyro - Q_bias) * dt;

/// 先验估计误差协方差的微分

Pdot[0] = Q_angle - PP[0][1] - PP[1][0];

Pdot[1] = - PP[1][1];

Pdot[2] = - PP[1][1];

Pdot[3] = Q_gyro;

/// 先验估计误差协方差的积分

PP[0][0] += Pdot[0] * dt;

PP[0][1] += Pdot[1] * dt;

PP[1][0] += Pdot[2] * dt;

PP[1][1] += Pdot[3] * dt;

/// 计算角度偏差

Angle_err = Accel - Angle_Final;

/// 卡尔曼增益计算

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_Final += K_0 * Angle_err; /// 后验估计最优角度值

Q_bias += K_1 * Angle_err; /// 更新最优估计值的偏差

Gyro_Final = Gyro - Q_bias; /// 更新最优角速度值

本内容试读结束，登录后可阅读更多

下载后可阅读完整内容，剩余5页未读，立即下载

内容反馈

洪蛋蛋

粉丝: 21
资源: 335

最新资源

资源上传下载、课程学习等过程中有任何疑问或建议，欢迎提出宝贵意见哦~我们会及时处理！点击此处反馈

feedback-tip