卡尔曼滤波学习笔记

savagego

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

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

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

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

算数平均法是我们常用的滤波方式。多采几个样，相加，求平均数
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“值更新
这个现在还没搞的很清楚，就不在这里卖了

savagego

卡尔曼滤波最大的不同是它先用各种方式“计算”“预测值”，然后再用“预测值”和“实际测量值”算出最终结果。
有了最终结果后，还要把“最终结果”和“预测”值做比较，衡量一下是不是“最终结果”是不是靠谱。
如果相差比较小，说明“预测”是比较正确的，下次计算的时候就提高“预测值”的权重，反之就提高“实际测量值”的权重

shenhaiyu

savagego 发表于 2014-3-25 16:04
卡尔曼滤波最大的不同是它先用各种方式“计算”“预测值”，然后再用“预测值”和“实际测量值”算出最终结 ...

再深入研究一下呗，然后把代码给大家详细讲讲，多谢了

leicheng

savagego 发表于 2014-3-25 16:04
卡尔曼滤波最大的不同是它先用各种方式“计算”“预测值”，然后再用“预测值”和“实际测量值”算出最终结 ...

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

savagego

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

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

savagego

废话不多说，应版主要求，对卡尔曼滤波的程序做详细的解析

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. 
   
8. void calculate_xz()
   
9. {
   
10. xz=xz+t*gyro[1];
    
11. p_xz=p_xz+q_xz;
    
12. k_xz=p_xz/(p_xz+r_xz);
    
13. xz=xz+k_xz*(Axz-xz);
    
14. p_xz=(1-k_xz)*p_xz;
    
15. }

复制代码

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

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

savagego

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}】会好点，不会人为的降低精度

savagego

3， k_xz=p_xz/(p_xz+r_xz);
第三条是卡尔曼系数K计算
这个问题比较核心，有必要认真解释一下

savagego

卡尔曼系数K示意图

savagego

那么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（）；

savagego

然后,程序这里有个明显错误：没有计算 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，为下次计算做准备

savagego

shenhaiyu 发表于 2014-3-26 09:35
再深入研究一下呗，然后把代码给大家详细讲讲，多谢了

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

savagego

“我的自平衡小车D3——滤波算法”确实是神作，值得一看。“十大滤波算法程序大全”里面的卡尔曼部分是错的

zhangzhe0617

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

请问是哪里有问题，我也好继续改进改进，谢谢

savagego

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值计算没太看懂，能解释一下不？