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) <script type="math/tex" id="MathJax-Element-1">p(y)</script>并不是依赖于 x <script type="math/tex" id="MathJax-Element-2">x</script>的变量，所以我们就可将p(y)−1<script type="math/tex" id="MathJax-Element-3">p(y)^{-1}</script>写成一个归一化的参数 η <script type="math/tex" id="MathJax-Element-4">\eta</script>

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

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

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

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

其中均值为 μ <script type="math/tex" id="MathJax-Element-5">\mu</script>，协方差矩阵为 ∑ <script type="math/tex" id="MathJax-Element-6">\sum</script>

1.2、卡尔曼滤波器描述

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

公式中的 xt <script type="math/tex" id="MathJax-Element-7">x_t</script>是 t <script type="math/tex" id="MathJax-Element-8">t</script>时刻的状态向量，At<script type="math/tex" id="MathJax-Element-9">A_t</script>和 Bt <script type="math/tex" id="MathJax-Element-10">B_t</script>是两个矩阵， At <script type="math/tex" id="MathJax-Element-11">A_t</script>是 n∗n <script type="math/tex" id="MathJax-Element-12">n*n</script>的， n <script type="math/tex" id="MathJax-Element-13">n</script>就是xt<script type="math/tex" id="MathJax-Element-14">x_t</script>的维度； Bt <script type="math/tex" id="MathJax-Element-15">B_t</script>是 n∗m <script type="math/tex" id="MathJax-Element-16">n*m</script>的， m <script type="math/tex" id="MathJax-Element-17">m</script>就是ut<script type="math/tex" id="MathJax-Element-18">u_t</script>的维度， ut <script type="math/tex" id="MathJax-Element-19">u_t</script>是控制向量。 εt <script type="math/tex" id="MathJax-Element-20">\varepsilon_t</script>是噪声项，服从 N(0,Rt) <script type="math/tex" id="MathJax-Element-21">N(0,R_t)</script>。下面是 xt,ut <script type="math/tex" id="MathJax-Element-22">x_t,u_t</script>的具体形式：

由于 At,Bt <script type="math/tex" id="MathJax-Element-23">A_t,B_t</script>是矩阵，那么就可与正太分布相加，那么就可以得到 xt <script type="math/tex" id="MathJax-Element-24">x_t</script>的条件概率密度：

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

其中， zt <script type="math/tex" id="MathJax-Element-25">z_t</script>是一个 k <script type="math/tex" id="MathJax-Element-26">k</script>维向量，Ct<script type="math/tex" id="MathJax-Element-27">C_t</script>是一个 k∗n <script type="math/tex" id="MathJax-Element-28">k*n</script>的矩阵， δt <script type="math/tex" id="MathJax-Element-29">\delta_t</script>是噪声项，服从 N(0,Qt) <script type="math/tex" id="MathJax-Element-30">N(0,Q_t)</script>。下面就是 zt <script type="math/tex" id="MathJax-Element-31">z_t</script>的条件概率密度：

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

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

这就第一个状态向量的概率密度，服从 N(μ0,∑0) <script type="math/tex" id="MathJax-Element-32">N(\mu_0,\sum_0)</script>，后面的证明中 bel(x)=p(x) <script type="math/tex" id="MathJax-Element-33">bel(x)=p(x)</script>。

1.3、数学证明

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

将其展开：

下面是他的简写形式：

其中：

将Lt<script type="math/tex" id="MathJax-Element-35">L_t</script>分解成如下两部分：

与 xt−1 <script type="math/tex" id="MathJax-Element-36">x_{t-1}</script>有关的都包含在了 Lt(xt−1,xt) <script type="math/tex" id="MathJax-Element-37">L_t(x_{t-1},x_t)</script>中，有 Lt(xt) <script type="math/tex" id="MathJax-Element-38">L_t(x_t)</script>无关，我们就可以将它移动到积分号外面：

积分是一个常数，我们既可以将它包含到参数 η <script type="math/tex" id="MathJax-Element-39">\eta</script>中。则上式就可以进一步简化：

下面分别对 Lt <script type="math/tex" id="MathJax-Element-40">L_t</script>进行一阶导和二阶导：

上式中， =: <script type="math/tex" id="MathJax-Element-41">=:</script>表示记做。令一阶导数为零：

求解 xt−1 <script type="math/tex" id="MathJax-Element-42">x_{t-1}</script>：

那么我们就可得到 Lt(xt−1,xt) <script type="math/tex" id="MathJax-Element-43">L_t(x_{t-1},x_t)</script>的表达式（这个我也没太看懂）：

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

概率密度函数求和为1

移项之后：

可以看出这是一个与 xt <script type="math/tex" id="MathJax-Element-44">x_t</script>无关的量，那么：

中的 η <script type="math/tex" id="MathJax-Element-45">\eta</script>含有的与上式后面积分项有关的部分为常数。再利用前面的分解式（3.11）得到 Lt(xt) <script type="math/tex" id="MathJax-Element-46">L_t(x_t)</script>的表达式：

用 Lt <script type="math/tex" id="MathJax-Element-47">L_t</script>的二阶导代替掉 Ψ <script type="math/tex" id="MathJax-Element-48">\Psi</script>，参看（3.15）。其中 xt−1 <script type="math/tex" id="MathJax-Element-49">x_{t-1}</script>的一次项下划线为单线，二次项为双线。

上式是关于 xt <script type="math/tex" id="MathJax-Element-50">x_t</script>的函数，又由于他对应的高斯分布使用了参数 η <script type="math/tex" id="MathJax-Element-51">\eta</script>，所以可以将关于 xt−1 <script type="math/tex" id="MathJax-Element-52">x_{t-1}</script>的项直接删除掉。

在对其求导：

对上式使用Inversion Lemma：

下面是Inversion Lemma的证明：

令 Lt(xt) <script type="math/tex" id="MathJax-Element-53">L_t(x_t)</script>的一阶导为零：

解得 xt <script type="math/tex" id="MathJax-Element-54">x_t</script>:

这样我们就证明了算法的第2行。在对 Lt(xt) <script type="math/tex" id="MathJax-Element-55">L_t(x_t)</script>求二阶导：

这是 Lt(xt) <script type="math/tex" id="MathJax-Element-56">L_t(x_t)</script>的曲率，所以它的转置就是 bel¯¯¯¯(xt) <script type="math/tex" id="MathJax-Element-57">\overline {bel}(x_t)</script>的协方差矩阵。这样我们就证明算法的第3行。

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

做如下简写：

其中：

并对 Jt <script type="math/tex" id="MathJax-Element-58">J_t</script>求一阶导和二阶导：

Jt <script type="math/tex" id="MathJax-Element-59">J_t</script>的二阶导数就是 bel(xt) <script type="math/tex" id="MathJax-Element-60">bel(x_t)</script>的协方差矩阵：

令 Jt <script type="math/tex" id="MathJax-Element-61">J_t</script>一阶导数为零：

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

将上式带回（3.38）式：

因此：

现在定义卡尔曼增益：

便可以从（3.41）中解得：

这就证明了算法的第5行。下面给变换卡尔曼增益 Kt <script type="math/tex" id="MathJax-Element-62">K_t</script>的形式，使其方便的融入算法流程中：

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

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

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

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

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

---

3、参考文献

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