授之以渔： 卡尔曼滤波器

《授之以渔： 卡尔曼滤波器》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。

1、一片绿油油的草地上有一条曲折的小径，通向一棵大树。一个要求被提出：从起点沿着小径走到树下。“很简单。” A说，于是他丝毫不差地沿着小径走到了树下。现在，难度被增加了：蒙上眼。“也不难，我当过特种兵。” B说，于是他歪歪扭扭地走到了树 ………. 旁。“唉，好久不练，生疏了。”“看我的，我有 DIY 的 GPS！”C说，于是他像个醉汉似地走到了树………. 旁。“唉，这个 GPS 软件没做好，漂移太大。”“我来试试。”旁边一人拿过 GPS,  蒙上眼，居然沿着小径走到了树下。“这么厉害！你是什么人?”“卡尔曼!”“卡尔曼？！你是卡尔曼？”众人大吃一惊。“我是说这个 GPS 卡而

2、慢。”这段时间研究了一下卡尔曼滤波器，有一些心得，写出来与大家分享。卡尔曼滤波器与我以前讲过的FIR,IIR 滤波器完全不一样，与其说属于滤波器，不如说是属于最优控制的范畴。下面的内容涉及相当多的控制理论知识，对于在这方面不足的同学可能有些吃力。不过不要紧，大家关注结果，会应用就够了,  那些晦涩的理论和推导可以忽略。我也会用图片让大家更直观的理解卡尔曼滤波器首先回顾一下传统数字滤波器。对于一个线性时不变系统，施加一个输入 u(t) ，我们可以得到一个输出 y(t). 如果输入是一个冲击，则输出y(t) 被称作冲击响应，用 h(t) 来表示，是系统的内核。对于任意u(t)

3、, 输出 y(t) 可以通过 u(t) 与冲击响应 h(t) 的卷积得到，这是 FIR 滤波器的基本原理。我们还可以通过系统微分方程转换为差分方程，或是通过 laplace 传递函数转换到差分方程，最后得到一个递推公式，这种形式的滤波器就是IIR 滤波器。以前讲过，一个系统可以用时域的微分方程来建立，然后可以用 laplace 的传递函数来处理，把解微分方程变为多项式乘法，可以简单的求解。还有另外一种处理形式就是状态空间，以矩阵形式来处理微分方程或微分方程组，利用矩阵变换求解，类同齐次方程组的矩阵形式。例如微分方程:          y’’+3y’+2y=u让    X

4、1=y,    X2=y’=X1’,  则上式变为：        X2’=-3X2  –2X1  -u        X1’=    X2矩阵形式为： 通用形式为：         X’=A*X+B*u         Y=C*X.可以看到，可以很轻易的微分方程或微分方程组转换到状态空间形式，而状态空间与laplace 传递函数之间可以相互转换，事实上矩阵A 的特征值就是s传递函数的极点。系统的传递函数（阵）可以通过矩阵变换得到：           Y(s)=C*(s*I-A) -1 *B同理，连续域的微分方程对应了离散域的差分方程，s 对应了z,  离散域状态空间相

5、应的变为：          X(k)=A*X(k-1)+B(u-1)          Y(k)=C*X(k)我们现在来看看蒙眼走小径的走法问题。假设A 走过的路径是真真正的路径，为Za;B是用自己的大脑作为预测估计器，走出了一个预测路径，为 Zb;C 用测量器，走出了一条测量路径，为Zc。用图片来说明： “系统真实输出”是 A 走过的路径： Za=C*X；“测量输出”是Zb.  Zb=Za+V，这里 V 是噪声，即GPS 的漂移；“预测估计输出”是 Zc=C*X^,X^是预测的状态。T 是采样延时。现在，蒙上眼的情况下有两种选择，GPS 或大脑预测估计器。如果GPS很

6、准而预测不准，那么可以选择GPS；如果预测准确而GPS不准，那么选择预测估计器，等等，没有反馈的预测估计器会因为累积误差而导致越来越不准。如果两个都不准，该如何取舍？如何把两者结合在一起呢？ 我们可以设置一个信心指数 K，K 在 0 与1之间，来说明对测量值还是预测值的信任程度：      Z=K*Zb+(1–K)*Zc  =Zc+K*(Zb–Zc)        (1)可以看出，当 K=1 和 0 时，分别选择了GPS 或预测估计器. 现在，可以把误差 Zb-Zc 作为反馈误差，来修正预测估计器的结果。新的系统结构图如下： 这个框图，就是卡尔曼滤波器的基本构造。学过现代

7、控制理论的同学都这个图应该很熟悉，与状态变量估计控制的图形差不多，只是其中的 K=1 而且没有噪声项和系统反馈而已。而我们下面的任务，就是如何确定这个K 值。......以下略去三百字的方差，与协方差的介绍.  自己看吧： http://zh.wikipedia.org/wiki/%E5%8D%8F%E6%96%B9%E5%B7%AE........以下略去 五百字的kalmanFilterGainK 的推导。自己看吧： http://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B