惯性传感器的卡尔曼滤波

一、引言

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

想象你在黄昏时分看着一只小鸟飞行穿过浓密的丛林，你只能隐隐约约、断断续续地瞥见小鸟运动的闪现。你试图努力地猜测小鸟在哪里以及下一时刻它会出现在哪里，才不至于失去它的行踪。或者再想象你是二战中的一名雷达操作员，正在跟踪一个微弱的游移目标，这个目标每隔10秒钟在屏幕上闪烁一次。或者回到更远的从前，想象你是开普勒，正试图根据一组通过不规则和不准确的测量间隔得到的非常不精确的角度观测值来重新构造行星的运动轨迹。在所有这些情况下，你都试图根据随对问变化并且带有噪声的观察数据去估计物理系统的状态（例如位置、速度等等）。这个问题可以被形式化表示为时序概率模型上的推理，模型中的转移模型描述了运动的物理本质，而传感器模型则描述了测量过程。为解决这类问题，人们发展出来了一种特殊的表示方法和推理算法——卡尔曼滤波。

二、基本概念

回想一下HMM的基本模型（如下图所示），其中涂有阴影的圆圈（y_t-2, y_t-1, y_t）相当于是观测变量，空白圆圈（x_t-2,x_t-1, x_t）相当于是隐变量。这其实揭示了卡尔曼滤波与HMM之间拥有很深的渊源。回到刚刚提及的那几个例子，你所观测到的物体状态（例如雷达中目标的位置或者速度）相当于是对其真实状态的一种估计（因为观测的过程中必然存在噪声），用数学语言来表述就是P(y_t | x_t)，这就是模型中的测量模型或测量概率（Measurement Probability）。另外一方面，物体当前的（真实）状态应该与其上一个观测状态相关，即存在这样的一个分布P(x_t | x_t-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 的线性估计。设在 [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(•)还是一个线性函数，所以我们同样可以把上式改写成矩阵的形式，则有

Y_t= Hx_t + 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），然后引入一个非常大的噪声，再用卡尔曼滤波来对质点的运动状态进行轨迹。注意是匀速直线运动，所以其中不含有控制变量。

[plain]  view plain  copy

1. Z=(1:100); %观测值    
2. noise=randn(1,100); %方差为1的高斯噪声    
3. Z=Z+noise;    
4.     
5. X=[0; 0]; %状态    
6. Sigma = [1 0; 0 1]; %状态协方差矩阵    
7. F=[1 1; 0 1]; %状态转移矩阵    
8. Q=[0.0001, 0; 0 0.0001]; %状态转移协方差矩阵    
9. H=[1 0]; %观测矩阵    
10. R=1; %观测噪声方差    
11.     
12. figure;    
13. hold on;    
14.     
15. for i=1:100    
16.     
17.   X_ = F*X;    
18.   Sigma_ = F*Sigma*F'+Q;    
19.   K = Sigma_*H'/(H*Sigma_*H'+R);    
20.   X = X_+K*(Z(i)-H*X_);    
21.   Sigma = (eye(2)-K*H)*Sigma_;    
22.       
23.   plot(X(1), X(2), '.','MarkerSize',10); %画点，横轴表示位置，纵轴表示速度    
24. end  
25.   
26. plot([0,100],[1,1],'r-');   

下图给出了上述代码的执行结果。可见经过最开始的几次迭代后，质点运动的状态估计就回到了正确轨迹上，而且估计的结果基本围绕在真实值附近，效果还是很理想的。

五、后记

本文相当于是卡尔曼滤波的入门文，在下一篇中我将深入挖掘一些本文未曾提及的细节（以及一些本文没有给出的数学上的推导）。如果你想将自己对卡尔曼滤波的认识提升到一个更高的档次，推荐你关注我的后续博文。Cheers~ 

参考文献：

【1】Stuart Russell and Peter Norvig. Artificial Intelligence: A Modern Approach. 3rd Edition.

【2】秦永元，张洪钺，汪叔华，卡尔曼滤波与组合导航原理，西北工业大学出版社

【3】徐亦达博士关于卡尔曼滤波的公开课，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html

【4】卡尔曼滤波的原理以及在MATLAB中的实现，http://blog.csdn.net/revolver/article/details/37830675

我们在上一篇文章中通过一个简单的例子算是入门卡尔曼滤波了，本文将以此为基础讨论一些技术细节。

卡尔曼滤波（Kalman Filter）

http://blog.csdn.net/baimafujinji/article/details/50646814

在上一篇文章中，我们已经对HMM和卡尔曼滤波的关联性进行了初步的讨论。参考文献【3】中将二者之间的关系归结为下表。

上表是什么意思呢？我们其实可以下面的式子来表示，其中，w 和 v 分别表示状态转移 和 测量 过程中的不确定性，也即是噪声，既然是噪声就可以假设它们服从一个零均值的高斯分布。这其实跟我们在上一篇文章中所给出的形式是一致的，也就是说我们认为过去的状态如果是 x_t-1，那么当前状态x_t应该是 x_t-1的一个线性变换，而这个估计过程其实是有误差的，用一个零均值的高斯噪声（概率分布）来表达。类似地，当前的测量值y_t应该是真实值 x_t 的一个线性变换，而这个测量过程仍然是有误差的，也用一个零均值的高斯噪声（概率分布）来表达。

      (1)

上一节中我们还讲过，在 [t0, t1] 时间段内的测量为Y，相应的估计为，则当t = t1 时，  称为X(t)的估计（或者称为滤波）。当然现在我们也仅仅需要将注意力放在滤波上，所以最终要求的应该是下面这个式子

根据条件概率的链式法则以及马尔科夫链的无记忆性，再去掉常值系数的情况下，就可以得到下面的结论（如果你对有关数学公式记得不是很清楚可以参考http://blog.csdn.net/baimafujinji/article/details/50441927）

其中，P( x_t | y₁, … , y_t-1)就是Prediction（预测），因为它表示的意义是已知从1到t-1时刻的观测值y₁, … , y_t-1的情况下求t 时刻的状态值x_t。另一方面，P( x_t | y₁, … , y_t)就是Update，因为它表示当我们已经获得y_t时，再对x_t 进行的一个更新（或修正）。

根据马尔科夫链的无记忆性，可知P( y_t | x_t, y₁, … , y_t-1) = P( y_t | x_t) 。就预测部分而言，我们希望引入x_t-1，所以可以采用下面的方法（这其实就是我们在处理普通贝叶斯网络时所用过的方法）

到此为止，其实你应该可以看出来卡尔曼滤波就形成了一个递归求解的过程。也就是说，我们欲求P( x_t | y₁, … , y_t-1)，就需要先求P( x_t-1 | y₁, … , y_t-1)，而欲求P( x_t-1 | y₁, … , y_t-1)，就要先求P( x_t-2 | y₁, … , y_t-1) ……结合上一篇文章介绍的内容，其实可以总结卡尔曼滤波的过程如下

也就是说当t = 1时，我们根据观测值y₁去估计真实状态x₁，这个过程服从一个高斯分布。然后，当t = 2时，我们根据上一个观测值y₁去预测当前的真实状态x₂，在获得该时刻的真实观测值y₂后，我们又可以估计出一个新的真实状态x₂，这时就要据此对由y₁预测的结果进行修正（Update），如此往复。

接下来，我们引入一个服从零均值高斯分布的（噪声）变量 Δx_t-1，

然后试着将Δx_t和Δy_t以Δx_t-1的形式来给出，而且处于方便的考虑，我们忽略掉公式（1）中的控制项 B 和 C，于是有

根据独立性假设，还可知如下结论（这些都是后续计算推导过程中所需要的准备）：

下面我们要做的事情就是推导卡尔曼滤波的五个公式，在上一篇文章中，我们更多地是从感性的角度给出了这些公式。并没有给出详细的数学推导，接下来我们就要来完成这项任务。

综上我们已经完整地给出了卡尔曼滤波的理论推导。对于结论性的东西，你当然可以直接拿来使用。在一些软件包中，卡尔曼滤波无非是一条命令或者一个函数就能搞定。我们之所以还在这里给出它的详细推导，主要是鉴于这种思想其实在机器学习中也被广泛地用到，所以了解这些技术细节仍然十分有意义。

===================================================================================================

如果你是图像处理的同道中人，欢迎加入图像处理算法交流群（单击链接查看群号）。

参考文献：

【1】Stuart Russell and Peter Norvig. Artificial Intelligence: A Modern Approach. 3rd Edition.

【2】秦永元，张洪钺，汪叔华，卡尔曼滤波与组合导航原理，西北工业大学出版社

【3】徐亦达博士关于卡尔曼滤波的公开课，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html

【4】卡尔曼滤波的原理以及在MATLAB中的实现，http://blog.csdn.net/revolver/article/details/37830675