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

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

一条最简化的公式：

其中下标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

#include

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

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

好了，本文到此结束。