极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 110524|回复: 52

卡尔曼滤波学习笔记

[复制链接]
发表于 2014-3-25 15:36:05 | 显示全部楼层 |阅读模式
本帖最后由 savagego 于 2014-3-28 13:32 编辑

原来的程序发现很多错误,所以删掉了

不敢说自己掌握了卡尔曼算法。只是死磕卡尔曼一周后的一点心得。

最近飞控比较火,卡尔曼顺带也热门起来。不过我没打算要做飞控,用它只是想提高电子称的精度(工作上要用,后来发现并不适合卡尔曼,改回原来的平滑滤波了)。

要了解卡尔曼先要扯远一点,

算数平均法是我们常用的滤波方式。多采几个样,相加,求平均数
x=(a0+a1+a2)/3

在这个基础上做一些改进,比如,最近采的样比较重要权重比较高
x=a0*0.1+a1*0.3+a2*0.6

接着呢又觉得0.1,0.3,0.6系数是固定的,比较蠢,比较木头,能不能根据实际情况自动改变
于是就有了自适应滤波

牛人卡尔曼(还活着现在)发明了一种“简单的”自适应滤波方法“卡尔曼滤波”

卡尔曼算法有两个重点,

1,是“预测”
举个例子:飞控里面,我们可以用重力加速传感器测出“现在”相对与地面的角度(重力的方向)。然后就可以“预测” 1秒后 的角度为:“一秒后角度”=“现在角度”+1秒*“旋转角速度”
也就是大家说的“数据融合”,如果出现多个数据融合,用矩阵就会更直观一点。幸运的是飞控不是太复杂,不用死啃矩阵。

2,“p“值更新
这个现在还没搞的很清楚,就不在这里卖了



回复

使用道具 举报

 楼主| 发表于 2014-3-25 16:04:35 | 显示全部楼层
卡尔曼滤波最大的不同是它先用各种方式“计算”“预测值”,然后再用“预测值”和“实际测量值”算出最终结果。
有了最终结果后,还要把“最终结果”和“预测”值做比较,衡量一下是不是“最终结果”是不是靠谱。
如果相差比较小,说明“预测”是比较正确的,下次计算的时候就提高“预测值”的权重,反之就提高“实际测量值”的权重
回复 支持 1 反对 0

使用道具 举报

发表于 2014-3-26 09:35:39 | 显示全部楼层
savagego 发表于 2014-3-25 16:04
卡尔曼滤波最大的不同是它先用各种方式“计算”“预测值”,然后再用“预测值”和“实际测量值”算出最终结 ...

再深入研究一下呗,然后把代码给大家详细讲讲,多谢了
回复 支持 反对

使用道具 举报

发表于 2014-3-26 10:47:10 | 显示全部楼层
savagego 发表于 2014-3-25 16:04
卡尔曼滤波最大的不同是它先用各种方式“计算”“预测值”,然后再用“预测值”和“实际测量值”算出最终结 ...

话说现在的电子秤应该是追求精度越来越不“准”,楼主的精神可嘉。
我的理解是楼主在运用电子秤进行“kalman”滤波时,实际的测量值是由比如砝码等已经标定的待测物体得到,预测值是通过用HX711称重传感器得到的。如果是这样的话,楼主打算怎么表示状态方程,各种系数值如何确定?
我的建议先对电子秤的倾角误差进行补偿,根据补偿后的测量结果再考虑其他减小误差的方法。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-26 11:11:29 | 显示全部楼层
leicheng 发表于 2014-3-26 10:47
话说现在的电子秤应该是追求精度越来越不“准”,楼主的精神可嘉。
我的理解是楼主在运用电子秤进行“ka ...

我的称是用来计数的,所以要的是动态的过程,要从一系列干扰,冲击中过滤出有用的信息。不过最后发现,卡尔曼是有好处,不过没有给这个项目带来质的飞跃,还是要从机械上想办法

点评

不是kalman滤波不能给你的项目带来质的飞跃,而是你的项目不满足kalman滤波的条件~  发表于 2014-3-26 11:17
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-26 11:21:09 | 显示全部楼层
废话不多说,应版主要求,对卡尔曼滤波的程序做详细的解析

  1. float xz=0;
  2. float p_xz=1;
  3. float q_xz=0.0025;
  4. float k_xz=0;
  5. float r_xz=0.25;
  6. float t=0.025;

  7. void calculate_xz()
  8. {
  9. xz=xz+t*gyro[1];
  10. p_xz=p_xz+q_xz;
  11. k_xz=p_xz/(p_xz+r_xz);
  12. xz=xz+k_xz*(Axz-xz);
  13. p_xz=(1-k_xz)*p_xz;
  14. }
复制代码


这个是版主提供的飞控程序的卡尔曼滤波部分,我逐条做解释

1, xz=xz+t*gyro[1];
第一条是“预测”
xz(这次x轴的角度“预测”)=xz(上次x轴的角度)+t(时间)*gyro[1](x轴旋转角速度);


回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-26 11:27:52 | 显示全部楼层
本帖最后由 savagego 于 2014-3-26 11:29 编辑

2,p_xz=p_xz+q_xz;
3, k_xz=p_xz/(p_xz+r_xz);
第二条是防止程序飞掉,因为第三条用到除法,如果除数为0,程序就当机了,所以人为的加了个q_xz(0.0025)。
不过我觉得,换成【 if(p_xz==0) {return xz}】会好点,不会人为的降低精度
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-26 11:36:35 | 显示全部楼层
3, k_xz=p_xz/(p_xz+r_xz);
第三条是卡尔曼系数K计算
这个问题比较核心,有必要认真解释一下
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-26 11:53:43 | 显示全部楼层
卡尔曼系数K示意图

示意图

示意图
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-26 12:09:50 | 显示全部楼层
那么k怎么确定呢?这就要解释另一个概念“P”。
p就是“计算”(不是预测)和“测量”的偏差。打个比方:我们用卡尔曼算法算出来的是5,实际测量值是7,
那么p=7-5=2;
p就是“上一次计算值“和“上一次测量值”的偏差。
如果上一次p比较小,说明我们预测的比较准,k就比较小,结果就偏向预测值。
准确的程序应该是

3, k_xz=sqrt(p_xz*p_xz/(p_xz*p_xz+r_xz*r_xz));//均方差

但是为了减小计算压力,近似简化为

3, k_xz=p_xz/(p_xz+r_xz);//我觉得应该加一个abs();
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-26 12:16:00 | 显示全部楼层
本帖最后由 savagego 于 2014-3-26 12:24 编辑

然后,程序这里有个明显错误:没有计算 r_xz
要在 第三句前加一个 r_xz=Axz-xz;


4,xz=xz+k_xz*(Axz-xz);
第四句,计算出结果,可以参考9楼的图

5,p_xz=(1-k_xz)*p_xz;
第五句有点莫名,直接改写为

p_xz=Axz-xz//更新p,为下次计算做准备
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-26 12:27:36 | 显示全部楼层
shenhaiyu 发表于 2014-3-26 09:35
再深入研究一下呗,然后把代码给大家详细讲讲,多谢了

代码解释了一通,不过不知道你找来的代码有点问题,还是我理解错误。帮我看看

点评

还是建议你先看看论坛的其他帖子,比如黑马前辈的“我的自平衡小车D3——滤波算法”,或者“十大滤波算法程序大全”里面有关卡尔曼滤波的那段。  发表于 2014-3-26 13:19
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-26 13:36:02 | 显示全部楼层
“我的自平衡小车D3——滤波算法”确实是神作,值得一看。“十大滤波算法程序大全”里面的卡尔曼部分是错的
回复 支持 反对

使用道具 举报

发表于 2014-3-27 10:11:54 | 显示全部楼层
savagego 发表于 2014-3-26 13:36
“我的自平衡小车D3——滤波算法”确实是神作,值得一看。“十大滤波算法程序大全”里面的卡尔曼部分是错的

请问是哪里有问题,我也好继续改进改进,谢谢
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-3-28 13:02:24 | 显示全部楼层
zhangzhe0617 发表于 2014-3-27 10:11
请问是哪里有问题,我也好继续改进改进,谢谢
  1. void calculate_xz()
  2. {
  3. xz=xz+t*gyro[1];
  4. p_xz=p_xz+q_xz;
  5. k_xz=p_xz/(p_xz+r_xz);
  6. xz=xz+k_xz*(Axz-xz);
  7. p_xz=(1-k_xz)*p_xz;
  8. }
复制代码


问题应该出在 这句k_xz=p_xz/(p_xz+r_xz);

没有计算 r_xz,你直接用常数代替了

还有最后一句 p值计算没太看懂,能解释一下不?

点评

04句写得稍微有点简洁~  发表于 2014-3-31 21:57
原代码没有问题~。补充下,第05句在一维的情况用除法,多维时是矩阵求逆,一般采用Gauss_Jordan法或更快的求逆方法。savagego童鞋最好动动手把卡尔曼的时间更新方程和测量更新方程离散化一遍,加深理解。  发表于 2014-3-28 19:47
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2020-2-28 14:11 , Processed in 0.056257 second(s), 29 queries .

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

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