仔细看完你就懂卡尔曼滤波（Kalman Filter）

一、引言

以下我们引用文献【1】中的一段话作为本文的開始：

想象你在黄昏时分看着一仅仅小鸟飞行穿过浓密的丛林。你仅仅能隐隐约约、断断续续地瞥见小鸟运动的闪现。你试图努力地猜測小鸟在哪里以及下一时刻它会出如今哪里，才不至于失去它的行踪。或者再想象你是二战中的一名雷达操作员，正在跟踪一个微弱的游移目标。这个目标每隔10秒钟在屏幕上闪烁一次。

或者回到更远的从前。想象你是开普勒，正试图依据一组通过不规则和不准确的測量间隔得到的非常不精确的角度观測值来又一次构造行星的运动轨迹。在全部这些情况下。你都试图依据随对问变化并且带有噪声的观察数据去预计物理系统的状态（比如位置、速度等等）。这个问题能够被形式化表示为时序概率模型上的推理，模型中的转移模型描写叙述了运动的物理本质，而传感器模型则描写叙述了測量过程。

为解决这类问题。人们发展出来了一种特殊的表示方法和推理算法——卡尔曼滤波。

二、基本概念

回忆一下HMM的基本模型（例如以下图所看到的）。当中涂有阴影的圆圈（yt-2,yt-1,yt）相当于是观測变量，空白圆圈（xt-2,xt-1,xt）相当于是隐变量。

这事实上揭示了卡尔曼滤波与HMM之间拥有非常深的渊源。

回到刚刚提及的那几个样例，你所观測到的物体状态（比如雷达中目标的位置或者速度）相当于是对其真实状态的一种预计（由于观測的过程中必定存在噪声），用数学语言来表述就是P(yt|xt)，这就是模型中的測量模型或測量概率（Measurement Probability）。另外一方面，物体当前的（真实）状态应该与其上一个观測状态相关，即存在这样的一个分布P(xt|xt-1)，这就是模型中的转移模型或转移概率（Transition Probability）。当然，HMM中隐变量必须都是离散的，观測变量并无特殊要求。

而从信号处理的角度来讲，滤波是从混合在一起的诸多信号中提取出所需信号的过程[2]。比如，我们有一组含有噪声的行星执行轨迹。我们希望滤除当中的噪声，预计行星的真实运动轨迹。这一过程就是滤波。

假设从机器学习和数据挖掘的角度来说。滤波是一个理性智能体为了把握当前状态以便进行理性决策所採取的行动[1]。比方，前两天我们没出门，可是我们能够从房间里观察路上的行人有没有打伞（观測状态）来预计前两天有没有下雨（真实状态）。

基于这些情况，如今我们要来决策今天（是否会有雨以及）外出是否须要打伞。这个过程就是滤波。

读者应该注意把握上面两个定义的统一性。

所谓预计就是依据測量得出的与状态X(t) 有关的数据Y(t) =h[X(t)] + V(t) 解算出X(t)的计算值，当中随机向量V(t) 为測量误差，称为X的预计，Y 称为 X 的測量。

由于是依据Y(t) 确定的．所以 是Y(t) 的函数。

若 是Y 的线性函数。则 X 称作 X 的线性预计。设在 [t0, t1] 时间段内的測量为Y。对应的预计为。则

当t=t1时。 称为X(t)的预计。

当t>t1肘，称为X(t)的预測；

当t< t1 时， 称为X(t)的平滑。

最优预计是指某一指标函数达到最值时的预计。

卡尔曼滤波就是一种线性最优滤波器。

由于后面会用到。这里我们补充一下关于协方差矩阵的概念。

设n维随机变量（X1,X2,…,Xn）的2阶混合中心距

σij= cov(Xi,Xj) = E[(Xi-E(Xi))(Xj-E(Xj))], (i,j = 1, 2,…,n)

都存在，则称矩阵

为n维随机变量（X1,X2,…,Xn）的协方差矩阵，协方差矩阵是一个对称矩阵，并且对角线是各个维度的方差。

维基百科中还给出了协方差矩阵的一些重要性质，比如以下这两条（此处不做具体证明）。

兴许的内容会用到当中的第一条。

三、卡尔曼滤波的方程推导

直接从数学公式和概念入手来考虑卡尔曼滤波无疑是一件非常枯燥的事情。为了便于理解，我们仍然从一个现实中的实例開始以下的介绍。这一过程中你所需的预备知识仅仅是高中程度的物理学内容。

假如如今有一辆在路上做直线运动的小车（例如以下所看到的），该小车在t时刻的状态能够用一个向量来表示，当中pt表示他当前的位置，vt表示该车当前的速度。当然，司机还能够踩油门或者刹车来给车一个加速度ut。ut相当于是一个对车的控制量。显然，假设司机既没有踩油门也没有踩刹车，那么ut就等于0。此时车就会做匀速直线运动。

假设我们已知上一时刻t-1时小车的状态。如今来考虑当前时刻t小车的状态。显然有

易知。上述两个公式中，输出变量都是输入变量的线性组合，这也就是称卡尔曼滤波器为线性滤波器的原因所在。既然上述公式表征了一种线性关系。那么我们就能够用一个矩阵来表示它，则有

假设另当中的

则得到卡尔曼滤波方程组中的第一条公式——状态预測公式，而F就是状态转移矩阵。它表示我们怎样从上一状态来猜測当前状态。而B则是控制矩阵，它表示控制量u怎样作用于当前状态。

   （1）

上式中x顶上的hat表示为预计值（而非真实值）。等式左端部分的右上标“-”表示该状态是依据上一状态猜測而来的，稍后我们还将对其进行修正以得到最优预计。彼时才干够将“-”去掉。

既然我们是在对真实值进行预计，那么就理应考虑到噪声的影响。

实践中，我们通常都是假设噪声服从一个0均值的高斯分布。即Noise~Guassian(0,σ)。

比如对于一个一维的数据进行预计时，若要引入噪声的影响。事实上仅仅要考虑当中的方差σ就可以。当我们将维度提高之后。为了综合考虑各个维度偏离其均值的程度，就须要引入协方差矩阵。

回到我们的样例。系统中每一个时刻的不确定性都是通过协方差矩阵Σ来给出的。

并且这样的不确定性在每一个时刻间还会进行传递。也就是说不仅当前物体的状态（比如位置或者速度）是会（在每一个时刻间）进行传递的，并且物体状态的不确定性也是会（在每一个时刻间）进行传递的。这样的不确定性的传递就能够用状态转移矩阵来表示，即（注意。这里用到了前面给出的关于协方差矩阵的性质）

可是我们还应该考虑到。预測模型本身也并不绝对准确的，所以我们要引入一个协方差矩阵Q来表示预測模型本身的噪声（也即是噪声在传递过程中的不确定性），则有

  （2）

这就是卡尔曼滤波方程组中的第二条公式，它表示不确定性在各个时刻间的传递关系。

继续我们的小汽车样例。你应该注意到，前面我们所讨论的内容都是环绕小汽车的真实状态展开的。

而真实状态我们事实上是无法得知的，我们仅仅能通过观測值来对真实值进行预计。

所以如今我们在路上布设了一个装置来測定小汽车的位置。观測到的值记为Y(t)。

并且从小汽车的真实状态到其观測状态另一个变换关系。这个变换关系我们记为h(•)。并且这个h(•)还是一个线性函数。此时便有（该式前面以前给出过）

Y(t) =h[X(t)] + V(t)

当中V(t)表示观測的误差。既然h(•)还是一个线性函数，所以我们相同能够把上式改写成矩阵的形式。则有

Yt=Hxt+ v

就本例而言，观測矩阵H= [1 0]，这事实上告诉我们x和Z的维度不一定非得相同。

在我们的样例中，x是一个二维的列向量，而Z仅仅是一个标量。此时当把x与上面给出的H相乘就会得出一个标量，此时得到的Y就是x中的首个元素，也就是小车的位置。

相同，我们还须要用一个协方差矩阵R来代替上述式子中的v来表示观測中的不确定性。

当然，由于Z是一个一维的值，所以此时协方差矩阵R也仅仅有一维，也就是仅仅有一个值，即观測噪声之高斯分布的參数σ。假设我们有非常多装置来測量小汽车的不同状态，那么Z就会是一个包括全部观測值的向量。

接下来要做的事情就是对前面得出的状态预计进行修正，具体而言就是利用以下这个式子

    （4）

直观上来说，上式并不难理解。前面我们提到。是依据上一状态猜測而来的。那么它与“最优”预计值之间的差距如今就是等式右端加号右側的部分。表示实际观察值与预估的观測值之间的残差。这个残差再乘以一个系数K就能够用来对预计值进行修正。

K称为卡尔曼系数，它也是一个矩阵，它是对残差的加权矩阵。有的资料上称其为滤波增益阵。

   （3）

上式的推导比較复杂，有兴趣深入研究的读者能够參阅文献【2】（P35~P37）。

假设有时间我会在后面再做具体推导。可是如今我们仍然能够定性地对这个系数进行解读：滤波增益阵首先权衡预測状态协方差矩阵Σ和观測值矩阵R的大小。并以此来认为我们是更倾向于相信预測模型还是具体观測模型。

假设相信预測模型多一点。那么这个残差的权重就会小一点。反之亦然。假设相信观察模型多一点，这个残差的权重就会大一点。不仅如此，滤波增益阵还负责把残差的表现形式从观測域转换到了状态域。

比如本题中观測值Z仅仅是一个一维的向量，状态x是一个二维的向量。

所以在实际应用中，观測值与状态值所採用的描写叙述特征或者单位都有可能不同，显然直接用观測值的残差去更新状态值是不合理的。

而利用卡尔曼系数，我们就能够完毕这样的转换。比如。在小车运动这个样例中，我们仅仅观察到了汽车的位置，但K里面已经包括了协方差矩阵P的信息（P里面就给出了速度和位置的相关性）。所以它利用速度和位置这两个维度的相关性，从位置的残差中推算出了速度的残差。

从而让我们能够对状态值x的两个维度同一时候进行修正。

最后，还需对最优预计值的噪声分布进行更新。所使用的公式为

 （5）

至此。我们便获得了实现卡尔曼滤波所需的全部五个公式，我在前面分别用（1）~（5）的标记进行了编号。我如今把它们再次罗列出来：

我们将这五个公式分成预測组和更新组。

预測组总是依据前一个状态来预计当前状态。更新组则依据观測信息来对预測信息进行修正。以期达到最优预计之目的。

四、一个简单的实例

当然，你可能困惑于卡尔曼滤波是否真的有效。以下利用文献[4]中给出的样例（为提升显示效果。笔者略有改动）来演示卡尔曼滤波的威力。

这个样例模拟质点进行匀速直线运动（速度为1），然后引入一个非常大的噪声。再用卡尔曼滤波来对质点的运动状态进行轨迹。注意是匀速直线运动。所以当中不含有控制变量。

Z=(1:100); %观測值
noise=randn(1,100); %方差为1的高斯噪声
Z=Z+noise;
  
X=[0; 0]; %状态
Sigma = [1 0; 0 1]; %状态协方差矩阵
F=[1 1; 0 1]; %状态转移矩阵
Q=[0.0001, 0; 0 0.0001]; %状态转移协方差矩阵
H=[1 0]; %观測矩阵
R=1; %观測噪声方差
  
figure;
hold on;
  
for i=1:100
  
  X_ = F*X;
  Sigma_ = F*Sigma*F'+Q;
  K = Sigma_*H'/(H*Sigma_*H'+R);
  X = X_+K*(Z(i)-H*X_);
  Sigma = (eye(2)-K*H)*Sigma_;
    
  plot(X(1), X(2), '.','MarkerSize',10); %画点，横轴表示位置。纵轴表示速度
end

plot([0,100],[1,1],'r-');

下图给出了上述代码的执行结果。

可见经过最開始的几次迭代后。质点运动的状态预计就回到了正确轨迹上，并且预计的结果基本环绕在真实值附近，效果还是非常理想的。

审核编辑：汤梓红