卡尔曼滤波器（THE KALMAN FILTER）的数学原理

0、内容提要

这篇博客想试图证明卡尔曼滤波器（KALMAN FILTER），即就是说明KF(KALMAN FILTER)和EKF（EXTENDED KALMAN FILTER）算法中的步奏是怎么来的，为什么是这样的。
1. 卡尔曼滤波器（KALMAN FILTER）的数学原理
2. 扩展卡尔曼滤波器（EXTENDED KALMAN FILTER）简介

1、卡尔曼滤波器（KALMAN FILTER）的数学原理

1.1、多元正太分布和贝叶斯公式

在KF和EKF的证明中用到了贝叶斯公式和多元正太分布，下面我们简单回顾：
首先是贝叶斯公式：

由于$p(y)$并不是依赖于$x$的变量，所以我们就可将$p(y)^{-1}$写成一个归一化的参数$\eta$

再看看多因素的贝叶斯公式

贝叶斯公式是后验概率，那么他的全概率公式就是：

但是上面个这个式子（2.24）并不能暗示任何概率独立的信息，即就是它推不出独立，独立也推不出它：

再来看看多元正态分布，他的表达式如下：

其中均值为$\mu$，协方差矩阵为$\sum$

1.2、卡尔曼滤波器描述

在这里我们简单的再说说卡尔曼滤波器的模型假设和算法步奏，一边更好的理解推导过程。下面分别介绍卡尔曼滤波器的过程模型和观察模型，过程模型主要描述的是相邻时刻状态转移关系。

公式中的$x_t$是$t$时刻的状态向量，$A_t$和$B_t$是两个矩阵，$A_t$是$n*n$的，$n$就是$x_t$的维度；$B_t$是$n*m$的，$m$就是$u_t$的维度，$u_t$是控制向量。$\varepsilon_t$是噪声项，服从$N(0,R_t)$。下面是$x_t,u_t$的具体形式：

由于$A_t,B_t$是矩阵，那么就可与正太分布相加，那么就可以得到$x_t$的条件概率密度：

观察模型描述的是状态与观察结果之间的关系：

其中，$z_t$是一个$k$维向量，$C_t$是一个$k*n$的矩阵，$\delta_t$是噪声项，服从$N(0,Q_t)$。下面就是$z_t$的条件概率密度：

最后我们再给出卡尔曼滤波器的算法描述：

可以看出卡尔曼滤波器算法是一个递归算法，那么他的第一状态向量怎么得到？

这就第一个状态向量的概率密度，服从$N(\mu_0,\sum_0)$，后面的证明中$bel(x)=p(x)$。

1.3、数学证明

卡尔曼滤波器算法步奏可以分成三部分，2、3行是预测部分，4行是求卡尔曼增益部分，5、6行是修正部分。下面我们以此证明这三个部分：
Part 1：预测，在证明过程中变量头顶有横线的都表示预测。首先利用全概率公式得到，$t$时刻的状态概率表达式：

将其展开：

下面是他的简写形式：

其中：

将$L_t$分解成如下两部分：

与$x_{t-1}$有关的都包含在了$L_t(x_{t-1},x_t)$中，有$L_t(x_t)$无关，我们就可以将它移动到积分号外面：

积分是一个常数，我们既可以将它包含到参数$\eta$中。则上式就可以进一步简化：

下面分别对$L_t$进行一阶导和二阶导：

上式中，$=:$表示记做。令一阶导数为零：

求解$x_{t-1}$：

那么我们就可得到$L_t(x_{t-1},x_t)$的表达式（这个我也没太看懂）：

在将它带回高斯分布密度函数：

概率密度函数求和为1

移项之后：

可以看出这是一个与$x_t$无关的量，那么：

中的$\eta$含有的与上式后面积分项有关的部分为常数。再利用前面的分解式（3.11）得到$L_t(x_t)$的表达式：

用$L_t$的二阶导代替掉$\Psi$，参看（3.15）。其中$x_{t-1}$的一次项下划线为单线，二次项为双线。

上式是关于$x_t$的函数，又由于他对应的高斯分布使用了参数$\eta$，所以可以将关于$x_{t-1}$的项直接删除掉。

在对其求导：

对上式使用Inversion Lemma：

下面是Inversion Lemma的证明：

令$L_t(x_t)$的一阶导为零：

解得$x_t$:

这样我们就证明了算法的第2行。在对$L_t(x_t)$求二阶导：

这是$L_t(x_t)$的曲率，所以它的转置就是$\overline {bel}(x_t)$的协方差矩阵。这样我们就证明算法的第3行。

part 2:卡尔曼增益和参数修正。根据观察模型可得：

做如下简写：

其中：

并对$J_t$求一阶导和二阶导：

$J_t$的二阶导数就是$bel(x_t)$的协方差矩阵：

令$J_t$一阶导数为零：

对上式左端项进行凑项，然后使用分配率：

将上式带回（3.38）式：

因此：

现在定义卡尔曼增益：

便可以从（3.41）中解得：

这就证明了算法的第5行。下面给变换卡尔曼增益$K_t$的形式，使其方便的融入算法流程中：

这就证明了算法的第4行。最后再将（3.37）使用Inversion Lemma进一步变形：

至此我们就从数学上严格的证明了卡尔曼滤波器算法。

2、扩展卡尔曼滤波器（EXTENDED KALMAN FILTER）简介

卡尔曼滤波器是线性算法，我们可定想将它推广到非线性。下面给是EKF的描述：

他的证明与卡尔曼滤波器类似。

---

3、参考文献

Probabilistic Robotics http://download.csdn.net/detail/a_cainiao_a/9447292
end