卡尔曼滤波原理快速理解

在看本文章之前请先保证自己明白什么是协方差，它有什么含义，什么叫最小均方误差估计，什么是多元高斯分布，以及什么是最大似然估计。

引言
1960年，卡尔曼发表了他著名的用递归方法解决离散数据线性滤波问题的论文。从那以后，得益于数字计算技术的进步，卡尔曼滤波器已成为推广研究和应用的主题，尤其是在自主或协助导航领域。
卡尔曼滤波器由一系列递归数学公式描述。它们提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。卡尔曼滤波器应用广泛且功能强大：它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。
本质上来讲，滤波就是一个信号处理与变换（去除或减弱不想要的成分，增强所需成分）的过程，这个过程既可以通过硬件来实现，也可以通过软件来实现。 卡尔曼滤波属于一种软件滤波方法，其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。

1 离散线性卡尔曼滤波

最初的卡尔曼滤波算法被称为基本卡尔曼滤波算法，适用于解决随机线性离散系统的状态或参数估计问题。
1.1被估计的过程信号
卡尔曼滤波器用于估计离散时间过程的状态变量x∈R^n。这个离散时间过程由以下离散随机差分方程描述:
(1.1)
定义观测变量z∈R^m，得到量测方程:
(1.2)
随机信号w_k和v_k分别表示过程激励噪声和观测噪声。假设它们为相互独立，正态分布的白色噪声（服从如下多元高斯分布，要问为什么服从高斯分布请把文章看完）:
p(w)~N(0,Q) (1.3)
p(v) ∼ N(0,R) (1.4)
实际系统中， 过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R可能会随每次迭代计算而变化。但在这儿我们假设它们是常数。
当控制函数或过程激励噪声为零时，差分方程1.1中的n × n阶增益矩阵A将上一时刻k-1的状态线性映射到当前时刻k的状态。实际中A可能随时间变化，但在这儿假设为常数。n × l 阶矩阵 B 代表可选的控制输入u∈R^l的增益。量测方程1.2中的m × n阶矩阵 H表示状态变量对测量变量的增益。实际中H可能随时间变化，但在这儿假设为常数。

1.2滤波器的计算原型
定义 （−代表先验，ˆ代表估计）为在已知第k步以前状态情况下第k步的先验状态估计。定义为已知测量变量时第k步的后验状态估计。由此定义先验估计误差和后验 估计误差:

先验估计误差的协方差为:
(1.5)
后验估计误差的协方差为:
(1.6)
式1.7构造了卡尔曼滤波器的表达式:先验估计和加权的测量变量及其预测之差的线性组合构成了后验状态估计。
(1.7)
式1.7中测量变量及其预测之差被称为测量过程的革新或残余 。残余反映了预测值和实际值之间的不一致程度。残余为零表明二者完全吻合。
式1.7中n × m阶矩阵K叫做残余的增益或混合因数，作用是使1.6式中的后验估计误差协方差最小。可以通过以下步骤计算K:首先将1.7式代入的定义式，再将代入1.6式中，求得期望后，将1.6式中的对K求导。并使一阶导数为零从而解得K值。K的一种表示形式为:
(1.8)（具体推导见文章最后附录！！）
由1.8式可知，观测噪声协方差 R 越小，残余的增益越大 K 越大。特别地， R 趋向于零时，有:

另一方面， 先验估计误差协方差越小，残余的增益 K 越小。特别地，趋向于零时，有:

增益 K 的另一种解释 （这种解释比较好理解，在估计值与测量值之间权衡，选择更相信哪个）是随着测量噪声协方差 R 趋于零，测量变量的权重越来越大，而的预测的权重越来越小。另一方面，随着先验估计误差协方差趋于零，测量变量的权重越来越小，而的预测的权重越来越大。

1.3滤波器的概率原型解释
1.7式的解释来源于贝叶斯规则:的更新取决于在已知先前的测量变量的情况下的先验估计的概率分布。卡尔曼滤波器表达式中包含了状态分布的前二阶矩。

后验状态估计1.7式反应了状态分布的均值（一阶矩）——如果条件式1.3和1.4成立，均值的估计便是正态分布的。 后验 估计误差协方差1.6式反映了状态分布的方差（二阶非中心矩）。在已知的情况下，的分布可写为:

1.4离散卡尔曼滤波器算法
卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的状态，然后以（含噪声的）测量变量的方式获得反馈。因此卡尔曼滤波器可分为两个部分： 时间更新 方程和测量更新方程。时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。测量更新方程负责反馈——也就是说，它将先验估计和新的测量变量结合以构造改进的后验估计。
时间更新方程也可视为预估方程，测量更新方程可视为校正方程。最后的估计算法成为一种具有数值解的预估－校正算法，如图1-1所示。

图 1-1: 离散卡尔曼滤波器循环更新图。 时间更新方程将当前状态变量作为先验估计及时地向前投射到测量更新方程， 测量更新方程校正先验估计以获得状态的后验估计。
表1-1和表1-2分别给出了时间更新方程和测量更新方程的具体形式。
表 1-1: 离散卡尔曼滤波器时间更新方程

请再次注意表1-1中的时间更新方程怎样将状态估计和协方差估计从 k-1 时刻向前推算到 k 时刻。 A 和 B 来自式1.1， Q 来自式1.3，滤波器的初始条件在早先的引用中讨论过。

表 1-2: 离散卡尔曼滤波器状态更新方程

测量更新方程首先做的是计算卡尔曼增益Kk。注意1.11式和1.8式是相同的。其次便测量输出以获得，然后按1.12式（与1.7式相同）产生状态的后验估计。最后按1.13式估计状态的后验协方差。
计算完时间更新方程和测量更新方程，整个过程再次重复。上一次计算得到的后验估计被作为下一次计算的先验估计 。

图 1-2: 卡尔曼滤波器工作原理图，由图1-1和表1-1及表1-2结合得到。

附录
求取K时最小均方误差起到了作用，顺便在这里回答为什么噪声必须服从高斯分布，在进行参数估计的时候，估计的一种标准叫最大似然估计，它的核心思想就是你手里的这些相互间独立的样本既然出现了，那就说明这些样本概率的乘积应该最大（概率大才出现嘛）。如果样本服从概率高斯分布，对他们的概率乘积取对数ln后，你会发现函数形式将会变成一个常数加上样本最小均方误差的形式。因此，看似直观上很容易理解的最小均方误差理论上来源就出于那里（详细过程还请自行谷歌）。
先看估计值和真实值间误差的协方差矩阵，提醒一下协方差矩阵的对角线元素就是方差，求这个协方差矩阵，就是为了利用他的对角线元素的和计算得到均方差.

这里请注意ek是向量，它由各个系统状态变量的误差组成。把前面得到的估计值代入这里能够化简得：
(1)式
同理，能够得到预测值和真实值之间误差的协方差矩阵：

注意到系统状态x变量和测量噪声之间是相互独立的。于是展开（1）式可得：

最后得到：

继续展开：

接下来最小均方差开始正式登场了，回忆之前提到的，协方差矩阵的对角线元素就是方差。这样一来，把矩阵P的对角线元素求和，用字母T来表示这种算子，他的学名叫矩阵的迹。

最小均方差就是使得上式最小，对未知量K求导,令导函数等于0，就能找到K的值。


注意这个计算式K，转换矩阵H式常数，测量噪声协方差R也是常数。因此K的大小将与预测值的误差协方差有关。不妨进一步假设，上面式子中的矩阵维数都是1*1大小的，并假设H=1，不等于0。那么K可以写成如下：

附录内容来自博文http://blog.csdn.net/heyijia0327/article/details/17487467