授之以渔： 卡尔曼滤波器.docxVIP

一片绿油油的草地上有一条曲折的小径，通向一棵大树。一个要求被提出：从起点沿着小径走到树下。“很简单。”?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矩阵形式为：?通用形式为：?? ?? ?? ???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。用图片来说明：?“系统真实输出”是?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?作为反馈误差，来修正 预测估计器的结果。新的系统结构图如下：?这个框图，就是卡尔曼滤波器的基本构造。学过现代控制理论的同学都这个图应该很熟悉，与状态变量估计控制的图形差不多，只是其中的?K = 1?而且没有噪声项和系统反馈而已。而我们下