卡尔曼滤波器大泄密.doc

授之以渔： 卡尔曼滤波器 ....大泻蜜 .........................highgear （一） 一片绿油油的草地上有一条曲折的小径，通向一棵大树。一个要求被提出：从起点沿着小径走到树下。“很简单。” A说，于是他丝毫不差地沿着小径走到了树下。现在，难度被增加了：蒙上眼。“也不难，我当过特种兵。” B说，于是他歪歪扭扭地走到了树 ………. 旁。“唉，好久不练，生疏了。”“看我的，我有 DIY 的 GPS！” C说，于是他像个醉汉似地走到了树………. 旁。“唉，这个 GPS 软件没做好，漂移太大。”“我来试试。” 旁边一人拿过 GPS,??蒙上眼，居然沿着小径走到了树下。“这么厉害！你是什么人?”“卡尔曼 ! ”“卡尔曼？！你是卡尔曼？”众人大吃一惊。“我是说这个 GPS 卡而慢。”这段时间研究了一下卡尔曼滤波器，有一些心得，写出来与大家分享。卡尔曼滤波器与我以前讲过的FIR, IIR 滤波器完全不一样，与其说属于滤波器，不如说是属于最优控制的范畴。下面的内容涉及相当多的控制理论知识，对于在这方面不足的同学可能有些吃力。不过不要紧，大家关注结果，会应用就够了,??那些晦涩的理论和推导可以忽略。我也会用图片让大家更直观的理解卡尔曼滤波器。首先回顾一下传统数字滤波器。对于一个线性时不变系统，施加一个输入 u(t) ，我们可以得到一个输出 y(t) . 如果输入是一个冲击，则输出y(t) 被称作冲击响应，用 h(t) 来表示，是系统的内核。对于任意 u(t), 输出 y(t) 可以通过 u(t) 与冲击响应 h(t) 的卷积得到，这是 FIR 滤波器的基本原理。我们还可以通过系统微分方程转换为差分方程，或是通过 laplace 传递函数转换到差分方程，最后得到一个递推公式，这种形式的滤波器就是IIR 滤波器。以前讲过，一个系统可以用时域的微分方程来建立，然后可以用 laplace 的传递函数来处理，把解微分方程变为多项式乘法，可以简单的求解。还有另外一种处理形式就是状态空间，以矩阵形式来处理微分方程或微分方程组，利用矩阵变换求解，类同齐次方程组的矩阵形式。例如微分方程:? ?? ?? ?? ???y’’ +3y’ + 2y = u让? ?? ?X1 = y,? ?? ? X2 = y’ = X1’,? ?则上式变为：? ?? ?? ?? ? X2’ = -3 X2??– 2 X1??- u? ?? ?? ?? ? X1’ =? ?? ?X2矩阵形式为： 下载 (3.04 KB) 2011-12-7 21:35 通用形式为： ? ?? ?? ???X’ = A*X + B*u ? ?? ?? ???Y = C*X.可以看到，可以很轻易的微分方程或微分方程组转换到状态空间形式，而状态空间与laplace 传递函数之间可以相互转换，事实上矩阵A 的特征值就是s传递函数的极点。系统的传递函数（阵）可以通过矩阵变换得到：? ?? ?? ?? ? ??Y(s) = C * (s * I - A) -1 * B同理，连续域的微分方程对应了离散域的差分方程，s 对应了z,? ?离散域状态空间相应的变为：? ?? ?? ?? ???X(k) = A*X(k-1) + B(u-1)? ?? ?? ?? ???Y(k) = C*X(k)我们现在来看看蒙眼走小径的走法问题。假设A 走过的路径是真真正的路径，为Za; B是用自己的大脑作为预测估计器，走出了一个预测路径，为 Zb; C 用测量器，走出了一条测量路径，为Zc。用图片来说明： 下载 (10.92 KB) 2011-12-7 21:47 “系统真实输出”是 A 走过的路径： Za = C * X；“测量输出”是Zb.? ?Zb = Za + V，这里 V 是噪声，即GPS 的漂移；“预测估计输出”是 Zc = C * X^,X^是预测的状态。T 是采样延时。现在，蒙上眼的情况下有两种选择，GPS 或大脑预测估计器。如果GPS很准而预测不准，那么可以选择GPS；如果预测准确而GPS不准，那么选择预测估计器， 等等， 没有反馈的预测估计器会因为累积误差而导致越来越不准。如果两个都不准，该如何取舍？如何把两者结合在一起呢？ 我们可以设置一个信心指数 K，K 在 0 与1之间，来说明对测量值还是预测值的信任程度：? ?? ???Z = K * Zb + (1 – K) * Zc??= Zc + K*(Zb – Zc)? ?? ?? ???(1)可以看出， 当 K = 1 和 0 时，分别选择了GPS 或预测估计器. 现在，可以把误差 Zb -Zc 作为反馈误差，