看了几篇文章之后,终于可以写出那么一点点自己的理解了。首先,要用这个算法必须先了解它的公式,而想要理解公式就要有概率论的基础,像我现在就没有,所以挺懵的,很多地方没搞清楚。因为本文主要讲卡尔曼的简单应用,那么讲得就是一维卡尔曼滤波的应用,也就是存在单个变量的情况,而且还是单传感器的输入。所以就直接给出在简单场景下的公式了,而不去推导公式,这样更容易理解和入手。
想要了解公式推导的可以阅读这篇文章,里面有详细的推导过程,且十分生动详细:How a Kalman filter works, in pictures | Bzarg
一条最简化的公式:
其中下标k是指当前的状态,举个例子,假设你现在要做的是测量不同时刻的室内温度,那么这个下标k就是指时间,带有下标的参数或变量就是指在k时间下的某某值;是当前的估测值(本文中所说的估测值都是指运用卡尔曼的公式后所得的计算结果);
是卡尔曼增益(至于卡尔曼增益是什么可以先不去理会,知道这个名词就可以了);
是当前的测量值,也就是仪器测量到的那个数值;
是上一个状态的估测值。
接下来通过一个简单的例子(测量电压)讲应用卡尔曼滤波算法的过程。
原本想直接给出简单场景下的三条简单的公式,但是还是展现下最原始的几条公式吧。
第一步:建模
你要先确定你的模型是否是线性的,当然既然是简单场景下的,那基本满足线性,所以就有了下面的公式:
①
②
①中的是系统的控制量,我们所说的简单场景下一般没有控制量,所以这项就可以直接划去。
A、B、H都是系统参数,在多维的情景下它们都是矩阵,而在我们所说的简单场景下,它们只是个常数而已,而且基本是值为1的常量。—— 符合高斯分布的过程噪声,其协方差在下文中为Q
—— 符合高斯分布的测量噪声,其协方差在下文中为R
上述的噪声在我理解来就是所谓的误差。一个仪器不管多么精密,总是会存在误差,只是误差大小有所不同而已。
再给出几条公式以便进入下一步环节:
③
④
⑤
⑥
①和③是随着时间更新的方程,也就是预测值;④⑤⑥是随着测量值的变化而变化的,就是在运用卡尔曼算法后得到的较为精确的估测值,可称其为更正值。
①到⑥这几条公式目前看着还是很高深,但是前面说过,在简单场景下的应用A、B、H这几个参数都是常数,而且基本是1,所以这几条公式就变得很简单了,建议自己动手写一下以增加印象,如果不愿意也没关系,我会呈现给你。下面①→①表示原先的公式①变成我们需要的简化后的第一条公式,以此类推。
①→①=
③→②
④→③
⑤→④
⑥→⑤
那么程序就很容易实现了,运用迭代即可。接下来只要确定x和P初值,程序就可以跑起来。不过正是因为卡尔曼滤波算法的强大,我们也就不用很苦恼初值到底取什么好,因为我们只要给出初值,程序就能运行下去,而卡尔曼滤波器自己会抹去不合理之处,故不妨设=0,
=1。注意:p的初值不能取0,p取值为0就意味着你完全相信测量的结果,认为测量不存在误差,这样的仪器不存在且卡尔曼滤波器就起不到作用了。这也说明了:p越接近0代表越相信仪器的测量结果。
全程最难的地方莫过于确定协方差R和Q的值。R的值我们或许能较好的确定,因为一般情况下我们能较清楚的知道仪器的测量误差,所以这是需要自己去猜测计算的,而Q超出了我的能力范围,此处也就不去讲怎么调了。参数的值决定精度,想要了解如何调参的朋友可以阅读下面这篇文章,里面有较详细的解说:
卡尔曼滤波的简单实现(Matlab & OC) - 简书
另外,我没有想明白为什么①中过程噪声也被直接0化处理了,但是不影响应用,因此,此处就不做解释了。有疑惑的朋友有兴趣的话可以自己再去另行学习。下面给出数据,可以去实现一下,看看程序得到的结果是否一样。
如果没错的话,得到的结果应该是下面这样的
然后给出一个图表,让我们直观的感受下卡尔曼滤波的伟大
在图中我们可以看到曲线是在变平稳,像是在趋向某个值。
讲到这,一个简单应用的示例基本讲完了,下面展示我的代码:
#include <stdio.h>
#include <math.h>
int main()
{
double R,z,p,x,k;
printf("X = ");
scanf("%lf",&x);
printf("P = ");
scanf("%lf",&p);
printf("R = ");
scanf("%lf",&R);
printf("Z = ");
while(scanf("%lf",&z) != EOF) //kalman algorithm
{
k = p / (p+R);
x = x + k * (z-x);
p = (1-k) * p;
printf("k=%.3f x=%.3f p=%.3f\nZ = ",k,x,p);
} //end
return 0;
}
上面是个需要靠手纯输入的,可以利用文本输入输出来进一步完善,只需要简单的几行代码就行。如需要完整的应用代码可至此处进行下载:提取码:mnh4或者卡尔曼滤波算法一维多传感器的简单应用_C语言.zip 里面包含有数据,以及使用Excel进行数据处理的可视化结果。
写文章是个技术活,想要表达清楚更是技术活,本文语言比较拙劣,想要更清晰理解的可以阅读下面这篇文章,本文主要参考的对象就是它:
Bilgin's Blog | Kalman Filter For Dummies
这些只是我的理解,错误在所难免,还请批评指正。
好了,本文到此结束。