搜索
bottom↓
回复: 26

贡献一个mpu6050的卡尔曼滤波程序

[复制链接]

出0入0汤圆

发表于 2014-11-12 21:34:02 | 显示全部楼层 |阅读模式
本帖最后由 salahading 于 2014-11-13 20:33 编辑
  1. /**********Kalman_Filter *********/

  2. /* 系统状态变量*/
  3. float x[2] = {0,0};
  4. /* 采样时间 */
  5. float Ts = 0.001;
  6. /*误差的协方差矩阵*/
  7. float p[2][2] = {{1,0},{0,1}};
  8. /* Kalman增益 */
  9. float K[2] = {0,0};
  10. /* 系统状态协方差矩阵对角元素 */
  11. float Q0 = 0.00001,Q1 = 0.00003;
  12. /* 系统输出 高斯噪声方差*/
  13. float R = 0.5;
  14. /*  
  15.         Zk 倾角仪器或加速度计测量的系统角度  Gyro 陀螺仪的角速度输出
  16.         x[0] 系统倾角
  17.         x[1] 陀螺仪漂移
  18. */
  19. void Kalman_Filter(float Zk,float Gyro)               
  20. {
  21.        
  22.         /*
  23.           X(k+1) = AX(k) + B*U
  24.           A = {{1,-Ts},{0,1}}    B = {Ts,0}
  25.           Zk = Hx
  26.           Zk  系统输出    H = {1 ,0}
  27.           计算系统状态估计值
  28.         */
  29.         x[0] = x[0] + Ts*(Gyro - x[1]);
  30.         x[1] = x[1];
  31.         /*
  32.           误差的协方差估计
  33.         */
  34.         p[0][0] = p[0][0] - Ts*(p[0][1]+p[1][0]) + Ts*Ts*p[1][1] + Q0;
  35.         p[0][1] = p[0][1] - Ts*p[1][1];
  36.         p[1][0] = p[1][0] - Ts*p[1][1];
  37.         p[1][1] = p[1][1] + Q1;
  38.        
  39.         /*
  40.                 计算Kalman增益
  41.         */
  42.         K[0] = p[0][0]/(p[0][0]+R);
  43.         K[1] = p[1][0]/(p[0][0]+R);
  44.        
  45.         /*
  46.                 校正先验协方差估计
  47.         */
  48.         p[0][0] = p[0][0] - K[0]*p[0][0];
  49.         p[0][1] = p[0][1] - K[0]*p[0][1];
  50.         p[1][0] = p[1][0] - K[1]*p[0][0];
  51.         p[1][1] = p[1][1] - K[1]*p[0][1];
  52.        
  53.         /*
  54.     校正系统状态估计值
  55.         */
  56.         x[0] = x[0] + K[0]*(Zk - x[0]);
  57.         x[1] = x[1] + K[1]*(Zk - x[0]);
  58.        
  59. }
复制代码


有同学说的他人的程序是不是下面的
  1. void Kalman_Filter1(float Accel,float Gyro)               
  2. {
  3.         Angle+=(Gyro - Q_bias) * dt; //先验估计

  4.        
  5.         Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

  6.         Pdot[1]=- PP[1][1];
  7.         Pdot[2]=- PP[1][1];
  8.         Pdot[3]=Q_gyro;
  9.        
  10.         PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
  11.         PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
  12.         PP[1][0] += Pdot[2] * dt;
  13.         PP[1][1] += Pdot[3] * dt;
  14.                
  15.         Angle_err = Accel - Angle;        //zk-先验估计
  16.        
  17.         PCt_0 = C_0 * PP[0][0];
  18.         PCt_1 = C_0 * PP[1][0];
  19.        
  20.         E = R_angle + C_0 * PCt_0;
  21.        
  22.         K_0 = PCt_0 / E;
  23.         K_1 = PCt_1 / E;
  24.        
  25.         t_0 = PCt_0;
  26.         t_1 = C_0 * PP[0][1];

  27.         PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差
  28.         PP[0][1] -= K_0 * t_1;
  29.         PP[1][0] -= K_1 * t_0;
  30.         PP[1][1] -= K_1 * t_1;
  31.                
  32.         Angle        += K_0 * Angle_err;         //后验估计
  33.         Q_bias        += K_1 * Angle_err;         //后验估计
  34.         Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度
  35. }
复制代码


那这个是不是卡尔曼滤波呢?

阿莫论坛20周年了!感谢大家的支持与爱护!!

如果想吃一顿饺子,就得从冰箱里取出肉,剁馅儿,倒面粉、揉面、醒面,擀成皮儿,下锅……
一整个繁琐流程,就是为了出锅时那一嘴滚烫流油的热饺子。

如果这个过程,禁不住饿,零食下肚了,饺子出锅时也就不香了……《非诚勿扰3》

出0入0汤圆

发表于 2014-11-12 21:38:05 | 显示全部楼层
收下了 你应该讲解两句的 什么入口出口的

出55入66汤圆

发表于 2014-11-12 21:39:25 | 显示全部楼层
你确定这是Kalman滤波?

出0入0汤圆

发表于 2014-11-12 21:40:14 | 显示全部楼层
看看……

出0入10汤圆

发表于 2014-11-12 21:52:21 | 显示全部楼层
  Mark.谢谢楼主的分享!

出0入0汤圆

发表于 2014-11-12 22:15:04 | 显示全部楼层
这是什么卡尔曼滤波  ?

出0入0汤圆

 楼主| 发表于 2014-11-13 14:43:45 | 显示全部楼层
cpumaker 发表于 2014-11-12 21:39
你确定这是Kalman滤波?

你确定不是吗

出0入0汤圆

 楼主| 发表于 2014-11-13 14:44:17 | 显示全部楼层
qs6361036 发表于 2014-11-12 22:15
这是什么卡尔曼滤波  ?

离散卡尔曼

出0入0汤圆

发表于 2014-11-13 14:47:48 | 显示全部楼层
有人验证过么??效果怎么样?

出55入66汤圆

发表于 2014-11-13 15:20:18 | 显示全部楼层

quite sure

出0入0汤圆

发表于 2014-11-13 16:31:08 | 显示全部楼层
像是Kalman filter的形式,但是估计的只是一个scaler而不会vector,没看懂在估计啥?

出0入0汤圆

发表于 2014-11-13 17:11:19 | 显示全部楼层
之前看过别人写的。你这个应该不是!

出0入0汤圆

发表于 2014-11-13 17:23:46 来自手机 | 显示全部楼层
单值滤波,算不上卡尔曼,卡尔曼实际上是融合过程,不是滤波过程

出0入0汤圆

 楼主| 发表于 2014-11-13 20:34:03 | 显示全部楼层
nnnkey 发表于 2014-11-13 17:23
单值滤波,算不上卡尔曼,卡尔曼实际上是融合过程,不是滤波过程

看一下第二段程序是不是?

出0入0汤圆

 楼主| 发表于 2014-11-13 20:34:37 | 显示全部楼层
tomy 发表于 2014-11-13 17:11
之前看过别人写的。你这个应该不是!

你说的是不是第二段程序呢?

出0入0汤圆

 楼主| 发表于 2014-11-13 20:35:14 | 显示全部楼层
cpumaker 发表于 2014-11-12 21:39
你确定这是Kalman滤波?

请问第二段程序是不是卡尔曼滤波呢?

出0入0汤圆

 楼主| 发表于 2014-11-14 21:49:13 | 显示全部楼层
cpumaker 发表于 2014-11-12 21:39
你确定这是Kalman滤波?

还请指教啊

出0入0汤圆

 楼主| 发表于 2014-11-14 21:49:34 | 显示全部楼层
tomy 发表于 2014-11-13 17:11
之前看过别人写的。你这个应该不是!

还请指教

出55入66汤圆

发表于 2014-11-14 22:13:16 | 显示全部楼层

第二段应该是,你可以假设一组数据然后加入高斯噪声,看看滤波后的效果

出0入0汤圆

 楼主| 发表于 2014-11-15 01:27:06 | 显示全部楼层
cpumaker 发表于 2014-11-14 22:13
第二段应该是,你可以假设一组数据然后加入高斯噪声,看看滤波后的效果 ...

刚看完电影,心情不错,就不刻薄了,只说一句,别不懂装懂

出55入66汤圆

发表于 2014-11-15 09:59:58 | 显示全部楼层
salahading 发表于 2014-11-15 01:27
刚看完电影,心情不错,就不刻薄了,只说一句,别不懂装懂

你的自信从何而来?

出55入66汤圆

发表于 2014-11-15 10:02:15 | 显示全部楼层
salahading 发表于 2014-11-15 01:27
刚看完电影,心情不错,就不刻薄了,只说一句,别不懂装懂

第二段估计就是网上抄的一段mpu-6050程序

出0入0汤圆

发表于 2014-11-15 10:04:05 | 显示全部楼层
先收藏着,以后有时间研究下

出0入0汤圆

发表于 2014-11-15 12:35:02 | 显示全部楼层
salahading 发表于 2014-11-13 20:34
你说的是不是第二段程序呢?

卡尔曼滤波算法C语言实现(转2)
http://www.amobbs.com/thread-5571611-1-1.html
(出处: amoBBS 阿莫电子论坛)


这个帖子讲得满详细,有研究的可以参考看看。

出0入0汤圆

发表于 2014-11-15 12:36:38 | 显示全部楼层
salahading 发表于 2014-11-13 20:34
你说的是不是第二段程序呢?

卡尔曼滤波算法C语言实现(转2)
http://www.amobbs.com/thread-5571611-1-1.html
(出处: amoBBS 阿莫电子论坛)


这个帖子讲得满详细,有研究的可以参考看看。 论坛里发这个算法的人很多,你自己收看看,对比下。


别人纠正的不对,只是为你好,并不是想对你怎么样。

出0入0汤圆

发表于 2017-3-28 10:43:18 | 显示全部楼层
tomy 发表于 2014-11-15 12:36
卡尔曼滤波算法C语言实现(转2)
http://www.amobbs.com/thread-5571611-1-1.html
(出处: amoBBS 阿莫电子 ...

楼主写的那段代码是对的,他只是直接将别人写的二维卡尔曼滤波中的C_0=1了,更直观的展现了卡尔曼滤波的五个经典公式

出0入0汤圆

发表于 2017-3-30 18:52:37 | 显示全部楼层
nnnkey 发表于 2014-11-13 17:23
单值滤波,算不上卡尔曼,卡尔曼实际上是融合过程,不是滤波过程

卡尔曼实际是一个观测器
回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。【本论坛不允许直接上传手机拍摄图片,浪费大家下载带宽和论坛服务器空间,请压缩后(图片小于1兆)才上传。压缩方法可以在微信里面发给自己(不要勾选“原图),然后下载,就能得到压缩后的图片】。另外,手机版只能上传图片,要上传附件需要切换到电脑版(不需要使用电脑,手机上切换到电脑版就行,页面底部)。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子技术论坛 ( 粤ICP备2022115958号, 版权所有:东莞阿莫电子贸易商行 创办于2004年 (公安交互式论坛备案:44190002001997 ) )

GMT+8, 2024-3-28 23:27

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

快速回复 返回顶部 返回列表