卡尔曼滤波算法的简单应用及其C语言实现

看了几篇文章之后,终于可以写出那么一点点自己的理解了。首先,要用这个算法必须先了解它的公式,而想要理解公式就要有概率论的基础,像我现在就没有,所以挺懵的,很多地方没搞清楚。因为本文主要讲卡尔曼的简单应用,那么讲得就是一维卡尔曼滤波的应用,也就是存在单个变量的情况,而且还是单传感器的输入。所以就直接给出在简单场景下的公式了,而不去推导公式,这样更容易理解和入手。

想要了解公式推导的可以阅读这篇文章,里面有详细的推导过程,且十分生动详细:How a Kalman filter works, in pictures | Bzarg

一条最简化的公式:

\hat{X_{k}}  =K_{k}*Z_{k}+(1-K_{k})*\hat{X_{k-1}}   
其中下标k是指当前的状态,举个例子,假设你现在要做的是测量不同时刻的室内温度,那么这个下标k就是指时间,带有下标的参数或变量就是指在k时间下的某某值;\hat{ X_{k} }是当前的估测值(本文中所说的估测值都是指运用卡尔曼的公式后所得的计算结果);K_{k} 是卡尔曼增益(至于卡尔曼增益是什么可以先不去理会,知道这个名词就可以了);Z_{k} 是当前的测量值,也就是仪器测量到的那个数值; \hat{X_{k-1}} 是上一个状态的估测值。

接下来通过一个简单的例子(测量电压)讲应用卡尔曼滤波算法的过程。
原本想直接给出简单场景下的三条简单的公式,但是还是展现下最原始的几条公式吧。

第一步:建模

你要先确定你的模型是否是线性的,当然既然是简单场景下的,那基本满足线性,所以就有了下面的公式:
x_{k}=Ax_{k-1}+Bu_{k} +   w_{k}
z_{k}= Hx_{k}+v_{k} 

①中的u_{k} 是系统的控制量,我们所说的简单场景下一般没有控制量,所以这项就可以直接划去。
A、B、H都是系统参数,在多维的情景下它们都是矩阵,而在我们所说的简单场景下,它们只是个常数而已,而且基本是值为1的常量。
w_{k} —— 符合高斯分布的过程噪声,其协方差在下文中为Q
v_{k} —— 符合高斯分布的测量噪声,其协方差在下文中为R
上述的噪声在我理解来就是所谓的误差。一个仪器不管多么精密,总是会存在误差,只是误差大小有所不同而已。

再给出几条公式以便进入下一步环节:
P_{k}=AP_{k-1}A^T+Q 
K_{k}=P_{k-1}H^T(HP_{k-1} H^T+R)^-1 
x_{k}=x_{k-1}+K_{k}(z_{k}-Hx_{k-1}     )
P_{k}=(1-K_{k}H)P_{k-1} 

①和③是随着时间更新的方程,也就是预测值;④⑤⑥是随着测量值的变化而变化的,就是在运用卡尔曼算法后得到的较为精确的估测值,可称其为更正值。

①到⑥这几条公式目前看着还是很高深,但是前面说过,在简单场景下的应用A、B、H这几个参数都是常数,而且基本是1,所以这几条公式就变得很简单了,建议自己动手写一下以增加印象,如果不愿意也没关系,我会呈现给你。下面①→①表示原先的公式①变成我们需要的简化后的第一条公式,以此类推。

①→①x_{k} =x_{k-1}
③→②P_{k} =P_{k-1}
④→③K_{k}= \frac{P_{k-1} }{P_{k-1}+R }
⑤→④x_{k}= x_{k-1}+K_{k}(z_{k} - x_{k-1}  )
⑥→⑤P_{k}= (1-K_{k})P_{k-1} 

那么程序就很容易实现了,运用迭代即可。接下来只要确定x和P初值,程序就可以跑起来。不过正是因为卡尔曼滤波算法的强大,我们也就不用很苦恼初值到底取什么好,因为我们只要给出初值,程序就能运行下去,而卡尔曼滤波器自己会抹去不合理之处,故不妨设x_{0} =0,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

这些只是我的理解,错误在所难免,还请批评指正。

好了,本文到此结束。

最后编辑于
©著作权归作者所有,转载或内容合作请联系作者
平台声明:文章内容(如有图片或视频亦包括在内)由作者上传并发布,文章内容仅代表作者本人观点,简书系信息发布平台,仅提供信息存储服务。

推荐阅读更多精彩内容