卡尔曼滤波(kalman filter)超详细推导

1. 概率论相关知识

这一节主要回忆概率论的一些相关基础知识，包括全概率公式、贝叶斯公式、协方差矩阵、多维高斯分布等等，对这些熟悉的可以直接跳到第2节看贝叶斯滤波

1.1 条件概率

$$\begin{array}{rl}& \mathrm{P}(x,y)=\mathrm{P}(x|y)\mathrm{P}(y)\\ & \mathrm{P}(x,y,z,t)=\mathrm{P}(x|y,z,t)\mathrm{P}(y,z,t)\\ & \mathrm{P}(x,y|z,t)=\mathrm{P}(x|y,z,t)\mathrm{P}(y|z,t)\end{array}$$

1.2 全概率公式

离散型
$$\mathrm{P}(x)=\sum _i\mathrm{P}(x|y_i)\mathrm{P}(y_i)$$
连续型
$$\mathrm{P}(x)=\int \mathrm{P}(x|y)\mathrm{P}(y)\mathrm{d}y$$

1.3 贝叶斯公式

贝叶斯公式其实就是联合概率、条件概率公式、全概率公式的扩展
对于联合概率$\mathrm{P}(x,y)$有
$\mathrm{P}(x,y)=\mathrm{P}(x|y)\mathrm{P}(y)=\mathrm{P}(y|x)\mathrm{P}(x)$
那么得到贝叶斯公式:
$$\begin{array}{rl}\mathrm{P}(x|y)& =\frac{\mathrm{P}(y|x)\mathrm{P}(x)}{\mathrm{P}(y)}\\ & =\frac{\mathrm{P}(y|x)\mathrm{P}(x)}{\sum _i\mathrm{P}(y|x_i)\mathrm{P}(x_i)}离散型\\ & =\frac{\mathrm{P}(y|x)\mathrm{P}(x)}{\int \mathrm{P}(y|x)\mathrm{P}(x)\mathrm{d}x}连续型\end{array}$$

---

在建模的时候我们如果用$x$表示状态（需要优化的变量），$y$表示观测（实际测量得到的数据）
那么，贝叶斯公式中：
我们常常称 $\mathrm{P}(x)$ 先验（prior）
称 $\mathrm{P}(y|x)$ 似然（likelihood）
称 $\mathrm{P}(x|y)$ 后验（posterior）
而 $\mathrm{P}(y)$ 是观测值，是某一个特定的常数，可以将其记为 $1/\eta$
则有
$$\mathrm{P}(x|y)=\eta \mathrm{P}(y|x)\mathrm{P}(x)$$
也就是
$$\mathrm{P}(x|y)\propto \mathrm{P}(y|x)\mathrm{P}(x)$$

有一点要提一下的就是
比如观测值与状态值有某种线性关系 $y=ax+b$，那么，就当然也有 $x=(y-b)/a,(a\ne 0)$
意思就是如果给出状态样本 $x$ 去看观测 $y$ 的概率，讲的就是似然
如果知道观测值 $y$ 去看状态 $x$ 的可能性，讲的就是后验
它们其实是同一个式子从两个不同方向看的结果

1.4 贝叶斯递推公式

在实验中，一般观测值会有很多。
那么根据贝叶斯公式可得，在$n$次观测下的后验概率
$$\mathrm{P}(x|y_{1:n})=\mathrm{P}(x)\prod _{i=1}^n{\eta }_i\mathrm{P}(y_i|x)$$
（符号$y_{1:n}$表示$y_1,y_2,...,y_n$）

推导过程如下：
由于$y$为观测数据（实际测量得到），所以一般每次测量都是相互独立的，并不相关
所以，$\mathrm{P}(y_n|y_{1:n-1})=\mathrm{P}(y_n)$
接下来，我们记所求后验概率$\mathrm{P}(x|y_{1:n})={\Phi }_n$，那么${\Phi }_0=\mathrm{P}(x)$
则
$$\begin{array}{rl}{\Phi }_n=\mathrm{P}(x|y_{1:n})& =\frac{\mathrm{P}(y_n|y_{1:n-1},x)\mathrm{P}(x|y_{1:n-1})}{\mathrm{P}(y_n|y_{1:n-1})}\\ & =\frac{\mathrm{P}(y_n|x)\mathrm{P}(x|y_{1:n-1})}{\mathrm{P}(y_n)}\\ & ={\eta }_n\mathrm{P}(y_n|x)\mathrm{P}(x|y_{1:n-1})\\ & ={\eta }_n\mathrm{P}(y_n|x){\Phi }_{n-1}\\ & ={\eta }_n\mathrm{P}(y_n|x){\eta }_{n-1}\mathrm{P}(y_{n-1}|x){\Phi }_{n-2}\\ & \dots \\ & =\prod _{i=1}^n{\eta }_i\mathrm{P}(y_i|x)\cdot {\Phi }_0\\ & =\mathrm{P}(x)\prod _{i=1}^n{\eta }_i\mathrm{P}(y_i|x)\end{array}$$

1.5 协方差矩阵

还记得两个随机变量$x$,$y$的协方差表示为$\mathrm{C}\mathrm{o}\mathrm{v}(x,y)$
多个随机变量$x_1,x_2,\dots ,x_n$的协方差矩阵如下：
$$\begin{array}{rl}\mathbf{\Sigma }& =\left(\begin{array}{cccc}\mathrm{C}\mathrm{o}\mathrm{v}(x_1,x_1)& \mathrm{C}\mathrm{o}\mathrm{v}(x_1,x_2)& \dots & \mathrm{C}\mathrm{o}\mathrm{v}(x_1,x_n)\\ \mathrm{C}\mathrm{o}\mathrm{v}(x_2,x_1)& \mathrm{C}\mathrm{o}\mathrm{v}(x_2,x_2)& \dots & \mathrm{C}\mathrm{o}\mathrm{v}(x_2,x_n)\\ ⋮& ⋮& \ddots & ⋮\\ \mathrm{C}\mathrm{o}\mathrm{v}(x_n,x_1)& \mathrm{C}\mathrm{o}\mathrm{v}(x_n,x_2)& \dots & \mathrm{C}\mathrm{o}\mathrm{v}(x_n,x_n)\end{array}\right)\\ & \\ & =\left(\begin{array}{cccc}\mathrm{V}\mathrm{a}\mathrm{r}(x_1)& \mathrm{C}\mathrm{o}\mathrm{v}(x_1,x_2)& \dots & \mathrm{C}\mathrm{o}\mathrm{v}(x_1,x_n)\\ \mathrm{C}\mathrm{o}\mathrm{v}(x_2,x_1)& \mathrm{V}\mathrm{a}\mathrm{r}(x_2)& \dots & \mathrm{C}\mathrm{o}\mathrm{v}(x_2,x_n)\\ ⋮& ⋮& \ddots & ⋮\\ \mathrm{C}\mathrm{o}\mathrm{v}(x_n,x_1)& \mathrm{C}\mathrm{o}\mathrm{v}(x_n,x_2)& \dots & \mathrm{V}\mathrm{a}\mathrm{r}(x_n)\end{array}\right)\end{array}$$
其中$\mathrm{V}\mathrm{a}\mathrm{r}(\cdot )$表示方差
协方差矩阵表示了多个随机变量之间的相关程度

1.6 多维高斯分布

本科学过二维高斯分布概率密度
$$f(x_1,x_2)=\left(2\pi {\sigma }_1{\sigma }_2\sqrt{1-{\rho }^2}{)}^{-1}\exp \right[-\frac{1}{2(1-{\rho }^2)}(\frac{(x_1-{\mu }_1{)}^2}{{\sigma }_1^2}-\frac{2\rho (x_1-{\mu }_1)(x_2-{\mu }_2)}{{\sigma }_1{\sigma }_2}+\frac{(x_2-{\mu }_2{)}^2}{{\sigma }_2^2})]$$
由于
$$\begin{array}{rl}& \rho =\frac{\mathrm{C}\mathrm{o}\mathrm{v}(x_1,x_2)}{\sqrt{\mathrm{V}\mathrm{a}\mathrm{r}(x_1)\mathrm{V}\mathrm{a}\mathrm{r}(x_2)}}=\frac{\mathrm{C}\mathrm{o}\mathrm{v}(x_1,x_2)}{{\sigma }_1{\sigma }_2}\\ & \\ & \mathbf{\Sigma }=\left(\begin{array}{cc}{\sigma }_1^2& \rho {\sigma }_1{\sigma }_2\\ \rho {\sigma }_1{\sigma }_2& {\sigma }_2^2\end{array}\right)\\ & \\ & \det \mathbf{\Sigma }=(1-{\rho }^2){\sigma }_1^2{\sigma }_2^2\end{array}$$
于是二维高斯分布的概率密度可以改为用协方差矩阵表示（实际上 $n$ 维高斯分布概率密度也是它）：
$$f(\mathbf{x})=\det \left(2\pi \mathbf{\Sigma }{)}^{-\frac{1}{2}}\exp \right\{-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\}$$

---

相关性质
记 $\mathbf{J}(\mathbf{x})=\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })$
那么
$$f(\mathbf{x})=\det (2\pi \mathbf{\Sigma }{)}^{-\frac{1}{2}}{e}^{-\mathbf{J}(\mathbf{x})}$$
对$\mathbf{J}$关于$\mathbf{x}$求导：
$$\begin{array}{rl}& \frac{\partial \mathbf{J}}{\partial \mathbf{x}}={\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\\ & \frac{{\partial }^2\mathbf{J}}{\partial {\mathbf{x}}^2}={\mathbf{\Sigma }}^{-1}\end{array}$$可以看出，对于$\mathbf{J}$:

1. 一阶导数为零的点是分布的均值点
2. 二阶导数是分布的协方差矩阵的逆

---

2. 贝叶斯(Bayes)滤波

卡尔曼滤波是基于贝叶斯滤波的一种改进，这里先说明一下贝叶斯滤波的相关原理。
已经熟悉运动状态和贝叶斯滤波的这一节也可以跳过。

2.1 变量定义

运动过程一般需要用到的变量主要有3个：
${\mathbf{x}}_i$表示$i$时刻的状态。比如当前坐标、速度等等。
${\mathbf{z}}_i$表示$i$时刻的观测值。指 用其他手段或工具可以得到目标的状态。例如使用视觉、触碰等传感器获得外界状态等等。
${\mathbf{u}}_i$表示$i$时刻的控制量。指 外界给他施加的作用力。例如推一下、跳一下等等。

---

2.2 马尔科夫假设

贝叶斯滤波是一个基于马尔科夫假设的算法
马尔科夫模型如图所示：

---

• 马尔科夫假设认为 $t$ 时刻的状态 ${\mathbf{x}}_t$ 只与 $t-1$ 时刻的状态 ${\mathbf{x}}_{t-1}$ 和 $t$ 时刻的控制量 ${\mathbf{u}}_t$ 相关，与之前无关：
  
  即：
  $$\mathrm{P}({\mathbf{x}}_t|{\mathbf{x}}_{1:t-1},{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})=\mathrm{P}({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)$$
  （符号${\mathbf{x}}_{1:t}$表示${\mathbf{x}}_1,{\mathbf{x}}_2,...,{\mathbf{x}}_t$）

---

• 马尔科夫假设认为 $t$ 时刻的观测值 ${\mathbf{z}}_t$ 只与 $t$ 时刻的状态 ${\mathbf{x}}_t$ 相关，与之前无关：
  
  即：
  $$\mathrm{P}({\mathbf{z}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{x}}_{1:t},{\mathbf{u}}_{1:t})=\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)$$

---

马尔科夫假设其实是比较苛刻的，真实世界中有很多因素会扰乱马尔科夫假设。

2.3 运动观测方程

从马尔科夫的两个假设我们可以得出两个重要的方程，即状态转移方程与观测方程
$$\{\begin{array}{rlr}{\mathbf{x}}_t& =f({\mathbf{x}}_{t-1},{\mathbf{u}}_t)+{\mathbf{\epsilon }}_t& \cdot \cdot \cdot 状态转移方程\\ {\mathbf{z}}_t& =h({\mathbf{x}}_t)+{\mathbf{\delta }}_t& \cdot \cdot \cdot 观测方程\end{array}$$
这里的$f(\cdot )$和$h(\cdot )$可以是线性也可以是非线性的，${\mathbf{\epsilon }}_t$ 和 ${\mathbf{\delta }}_t$ 为误差项
（$\mathbf{u}$ 在这个模型下其实是一个常数，在没有外力干扰的时候，也可以将它去掉简化状态方程）
可以看出，这两个方程分别对应了马尔科夫两个假设中的概率
$$\{\begin{array}{rl}& \mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_t,{\mathbf{x}}_{t-1})\\ & \mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)\end{array}$$

2.4 算法思想

目标
通过上一时刻的状态后验概率$\mathrm{P}({\mathbf{x}}_{t-1}|{\mathbf{u}}_{1:t-1},{\mathbf{z}}_{1:t-1})$，推导出当前时刻的后验概率$\mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t})$
把这俩概率记为：$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_{t-1})$和$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)$，称为xx时刻的置信度

已知
控制集：$\left\{{\mathbf{u}}_{1:t}\right\}$
观测集：$\left\{{\mathbf{z}}_{1:t}\right\}$
运动模型：$\mathrm{P}({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)$
观测模型：$\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)$
初始状态：$\mathrm{P}({\mathbf{x}}_0)$ 即 $\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_0)$

输入
$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_{t-1})$

输出
$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)$

---

大致流程就是将$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_{t-1})$带入到第1个方程中，求出的结果再通过结合第2个方程算出最终的$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)$
这里的运动模型和观测模型是需要设计的，即需要设计一套合理的$f(\cdot )$ 和 $h(\cdot )$、${\mathbf{\epsilon }}_t$ 和 ${\mathbf{\delta }}_t$

之后要说的卡尔曼滤波的创作者Kalman就是将它们假设成了线性高斯模型，即$f(\cdot )$ 和 $h(\cdot )$为线性函数、${\mathbf{\epsilon }}_t$ 和 ${\mathbf{\delta }}_t$是高斯噪声。具体内容可看第3节

2.5 算法流程

基本贝叶斯滤波算法流程如下

1.　Algorithm Bayes_filter($bel(x_{t-1}),u_t,z_t$):
2.　　 $\overline{bel}(x_t)=\int p(x_t|u_t,x_{t-1})bel(x_{t-1})\mathrm{d}{x}_{t-1}$
3.　　 $bel(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t)$
4.　　 return $bel(x_t)$

算法核心其实就是第2、3两行

---

伪代码第2行称为预测
$$\begin{array}{r}\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_{t-1})\\ \mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_t,{\mathbf{x}}_{t-1})\end{array}\}⟹\overline{\mathrm{b}\mathrm{e}\mathrm{l}}(x_t)$$
即 通过上一个状态的置信度和运动转移模型去预测当前可能的状态

---

伪代码第3行称为更新
$$\begin{array}{r}\overline{\mathrm{b}\mathrm{e}\mathrm{l}}({\mathbf{x}}_t)\\ \mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)\end{array}\}⟹\mathrm{b}\mathrm{e}\mathrm{l}(x_t)$$
即 使用当前的观测状态，去修正上一步预测的状态

---

2.6 推导

我们称所求的 $t$ 时刻的后验概率$\mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t})$为 $t$ 时刻的状态置信度，记为$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)$，那么
$$\begin{array}{rl}\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)& =\frac{\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t,{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})\mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})}{\mathrm{P}({\mathbf{z}}_t|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})}\\ & =\frac{\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)\mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})}{\mathrm{P}({\mathbf{z}}_t)}\\ & ={\mathbf{\eta }}_t\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)\mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})\end{array}$$
根据全概率公式以及上一小节的马尔科夫假设可得，上式中：
$$\begin{array}{rl}\mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})& =\int \mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1},{\mathbf{x}}_{t-1})\mathrm{P}({\mathbf{x}}_{t-1}|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})\mathrm{d}{\mathbf{x}}_{t-1}\\ & =\int \mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_t,{\mathbf{x}}_{t-1})\mathrm{P}({\mathbf{x}}_{t-1}|{\mathbf{u}}_{1:t-1},{\mathbf{z}}_{1:t-1})\mathrm{d}{\mathbf{x}}_{t-1}\\ & =\int \mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_t,{\mathbf{x}}_{t-1})\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_{t-1})\mathrm{d}{\mathbf{x}}_{t-1}\\ & \stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}\overline{\mathrm{b}\mathrm{e}\mathrm{l}}({\mathbf{x}}_t)\end{array}$$

所以得到贝叶斯滤波递推公式为：

$$\begin{gathered} \mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)={\mathbf{\eta }}_t\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)\int \mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_t,{\mathbf{x}}_{t-1})\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_{t-1})\mathrm{d}{\mathbf{x}}_{t-1} \\ 或 \\ \mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)={\mathbf{\eta }}_t\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)\overline{\mathrm{b}\mathrm{e}\mathrm{l}}({\mathbf{x}}_t) \end{gathered}$$

可以看到，算法流程伪代码第2、3两行和上面的贝叶斯递推公式描述的是一样的。

---

3. 卡尔曼滤波

3.1 线性高斯系统

可能实现贝叶斯滤波的最好的研究技术就是卡尔曼滤波，整个卡尔曼滤波就是一个线性高斯系统，各种假设与模型都是基于线性高斯而建立的。

3.1.1 状态转移概率

卡尔曼滤波认为状态转移概率服从高斯分布
即状态转移方程可以表示为一个带有随机高斯噪声的线性函数：
$${\mathbf{x}}_t={\mathbf{A}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{\epsilon }}_t$$
其中${\mathbf{\epsilon }}_t\sim N(0,{\mathbf{R}}_t)$

所以在${\mathbf{x}}_{t-1}$与${\mathbf{u}}_t$给定的情况下，${\mathbf{x}}_t\sim N({\mathbf{A}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t,{\mathbf{R}}_t)$
即
$$\mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_t,{\mathbf{x}}_{t-1})=\det (2\pi {\mathbf{R}}_t{)}^{-\frac{1}{2}}\exp \{-\frac{1}{2}({\mathbf{x}}_t-{\mathbf{A}}_t{\mathbf{x}}_{t-1}-{\mathbf{B}}_t{\mathbf{u}}_t{)}^{\mathrm{T}}{\mathbf{R}}_t^{-1}({\mathbf{x}}_t-{\mathbf{A}}_t{\mathbf{x}}_{t-1}-{\mathbf{B}}_t{\mathbf{u}}_t)\}$$

3.1.2 观测概率

卡尔曼滤波认为观测概率也服从高斯分布
即观测方程可以表示为一个带有随机高斯噪声的线性函数：
$${\mathbf{z}}_t={\mathbf{C}}_t{\mathbf{x}}_t+{\mathbf{\delta }}_t$$
其中${\mathbf{\delta }}_t\sim N(0,{\mathbf{Q}}_t)$

所以在${\mathbf{x}}_t$给定的情况下，${\mathbf{z}}_t\sim N({\mathbf{C}}_t{\mathbf{x}}_t,{\mathbf{Q}}_t)$
即
$$\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)=\det (2\pi {\mathbf{Q}}_t{)}^{-\frac{1}{2}}\exp \{-\frac{1}{2}({\mathbf{z}}_t-{\mathbf{C}}_t{\mathbf{x}}_t{)}^{\mathrm{T}}{\mathbf{Q}}_t^{-1}({\mathbf{z}}_t-{\mathbf{C}}_t{\mathbf{x}}_t)\}$$

3.1.3 初始置信度

卡尔曼滤波认为初始置信度仍然是服从高斯分布的
即
$$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_0)=\mathrm{P}({\mathbf{x}}_0)=\det (2\pi {\mathbf{\Sigma }}_0{)}^{-\frac{1}{2}}\exp \{-\frac{1}{2}({\mathbf{x}}_0-{\mathbf{\mu }}_0{)}^{\mathrm{T}}{\mathbf{\Sigma }}_0^{-1}({\mathbf{x}}_0-{\mathbf{\mu }}_0)\}$$

---

这三个高斯分布的假设可以保证之后的每一个置信度$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)$都是高斯分布的：$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)\sim N({\mathbf{\mu }}_t,{\mathbf{\Sigma }}_t)$

3.2 算法流程

下面给出卡尔曼滤波伪代码

1.　Algorithm Kalman_filter(${\mu }_{t-1},{\Sigma }_{t-1},u_t,z_t$):

2.　　${\overline{\mu }}_t=A_t{\mu }_{t-1}+B_t{u}_t$
3.　　${\overline{\Sigma }}_t=A_t{\Sigma }_{t-1}{A}_t^{\mathrm{T}}+R_t$

4.　　$K_t={\overline{\Sigma }}_t{C}_t^{\mathrm{T}}(C_t{\overline{\Sigma }}_t{C}_t^{\mathrm{T}}+Q_t{)}^{-1}$
5.　　${\mu }_t={\overline{\mu }}_t+K_t(z_t-C_t{\overline{\mu }}_t)$
6.　　${\Sigma }_t=(I-K_t{C}_t){\overline{\Sigma }}_t$

7.　　return ${\mu }_t,{\Sigma }_t$

算法伪代码中：
输入：上一时刻状态的均值与方差、当前时刻的控制量、当前时刻的观测值
输出：当前时刻状态的均值与方差

2~3步为预测，使用 前面状态$x_{t-1}$的均值与方差 去预测新状态的均值方差
4~6步为更新，使用新的观测数据，对上面预测的均值方差做出更新调整，得到修正后的 状态$x_t$的均值方差
第7步返回修正后$x_t$的均值与方差

这里出现了三个新的字母：${\overline{\mathbf{\mu }}}_t,{\overline{\mathbf{\Sigma }}}_t,{\mathbf{K}}_t$
其中前两个字母是通过预测步骤得到的状态均值方差，即${\overline{\mathbf{x}}}_t\sim N({\overline{\mathbf{\mu }}}_t,{\overline{\mathbf{\Sigma }}}_t)$，亦或是先验置信度的均值与方差：$\overline{\mathrm{b}\mathrm{e}\mathrm{l}}({\mathbf{x}}_t)\sim N({\overline{\mathbf{\mu }}}_t,{\overline{\mathbf{\Sigma }}}_t)$；
最后一个字母${\mathbf{K}}_t$代表的是卡尔曼增益(Kalman Gain)

3.3 推导

回顾运动观测方程：
$$\{\begin{array}{rl}{\mathbf{x}}_t& ={\mathbf{A}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{\epsilon }}_t\\ {\mathbf{z}}_t& ={\mathbf{C}}_t{\mathbf{x}}_t+{\mathbf{\delta }}_t\end{array}$$我们将两个式子中的${\mathbf{x}}_t$都改一下符号，以便它们与真实值${\mathbf{x}}_t$有所区分
$$\{\begin{array}{rlr}{\overline{\mathbf{x}}}_t& ={\mathbf{A}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{\epsilon }}_t& \cdot \cdot \cdot 3.1\\ {\mathbf{z}}_t& ={\mathbf{C}}_t{\tilde{\mathbf{x}}}_t+{\mathbf{\delta }}_t& \cdot \cdot \cdot 3.2\end{array}$$
下面分为预测、更新两部分详细讲解2~7行的公式如何得出

---

— 第1部分，预测 —

对于3.1式$${\overline{\mathbf{x}}}_t={\mathbf{A}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{\epsilon }}_t$$
${\overline{\mathbf{x}}}_t\sim N({\overline{\mathbf{\mu }}}_t,{\overline{\mathbf{\Sigma }}}_t)$
${\mathbf{x}}_{t-1}\sim N({\mathbf{\mu }}_{t-1},{\mathbf{\Sigma }}_{t-1})$
${\mathbf{u}}_t$是常量
${\mathbf{\epsilon }}_t\sim N(0,{\mathbf{R}}_t)$

而${\mathbf{x}}_{t-1}$与${\mathbf{\epsilon }}_t$是独立的，所以
$$\begin{array}{rl}{\overline{\mathbf{\mu }}}_t& ={\mathbf{A}}_t{\mathbf{\mu }}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+0\\ {\overline{\mathbf{\Sigma }}}_t& ={\mathbf{A}}_t{\mathbf{\Sigma }}_{t-1}{\mathbf{A}}_t^{\mathrm{T}}+{\mathbf{R}}_t\end{array}$$对应了算法伪代码第2、3行

相关公式：）
1. $\mathrm{E}(\mathbf{x}+\mathbf{y})=\mathrm{E}(\mathbf{x})+\mathrm{E}(\mathbf{y})$
2. $\mathrm{D}(a\mathbf{x})=a^2\mathrm{D}(\mathbf{x})$
3. $\mathrm{D}(\mathbf{A}\mathbf{x})=\mathbf{A}\mathrm{D}(\mathbf{x}){\mathbf{A}}^{\mathrm{T}}$
4. $\mathrm{D}(\mathbf{x}+\mathbf{y})=\mathrm{D}(\mathbf{x})+\mathrm{D}(\mathbf{y})$（$\mathbf{x},\mathbf{y}$独立）

另外，这两个公式用贝叶斯滤波知识也是可以推的，即
$$\begin{array}{rl}\overline{\mathrm{b}\mathrm{e}\mathrm{l}}({\mathbf{x}}_t)& =\mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})\\ & =\int \mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_t,{\mathbf{x}}_{t-1})\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_{t-1})\mathrm{d}{\mathbf{x}}_{t-1}\end{array}$$而右边的积分中，
$\mathrm{P}({\mathbf{x}}_t|{\mathbf{u}}_t,{\mathbf{x}}_{t-1})\sim \mathrm{N}({\mathbf{A}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t,{\mathbf{R}}_t)$
$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_{t-1})\sim \mathrm{N}({\mathbf{\mu }}_{t-1},{\mathbf{\Sigma }}_{t-1})$
写出它们俩的概率密度，相乘，然后积分，直接求出$\overline{\mathrm{b}\mathrm{e}\mathrm{l}}({\mathbf{x}}_t)$的概率密度，从而求出他的均值与方差: ${\overline{\mathbf{\mu }}}_t,{\overline{\mathbf{\Sigma }}}_t$
但此方法推导有些复杂，有兴趣的同学可以查阅相关资料——《概率机器人》3.2.2节

---

— 第2部分，更新 —

对于3.2式
$${\mathbf{z}}_t={\mathbf{C}}_t{\tilde{\mathbf{x}}}_t+{\mathbf{\delta }}_t$$
并没有与真实值${\mathbf{x}}_t$有关的变量，所以只能根据贝叶斯滤波原理来推导${\mathbf{x}}_t$的置信度
$$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)={\mathbf{\eta }}_t\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)\overline{\mathrm{b}\mathrm{e}\mathrm{l}}({\mathbf{x}}_t)$$由于$\mathrm{P}({\mathbf{z}}_t|{\mathbf{x}}_t)\sim \mathrm{N}({\mathbf{C}}_t{\mathbf{x}}_t,{\mathbf{Q}}_t)$，$\overline{\mathrm{b}\mathrm{e}\mathrm{l}}({\mathbf{x}}_t)\sim \mathrm{N}({\overline{\mathbf{\mu }}}_t,{\overline{\mathbf{\Sigma }}}_t)$
所以可得
$$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)={\mathbf{\eta }}^{\prime }{e}^{-{\mathbf{J}}_t}$$其中
$${\mathbf{J}}_t=\frac{1}{2}({\mathbf{z}}_t-{\mathbf{C}}_t{\mathbf{x}}_t{)}^{\mathrm{T}}{\mathbf{Q}}_t^{-1}({\mathbf{z}}_t-{\mathbf{C}}_t{\mathbf{x}}_t)+\frac{1}{2}({\mathbf{x}}_t-{\overline{\mathbf{\mu }}}_t{)}^{\mathrm{T}}{\overline{\mathbf{\Sigma }}}_t^{-1}({\mathbf{x}}_t-{\overline{\mathbf{\mu }}}_t)$$
${\mathbf{J}}_t$这仍然是关于${\mathbf{x}}_t$的二次型，说明$\mathrm{b}\mathrm{e}\mathrm{l}({\mathbf{x}}_t)$仍是高斯分布
接下来我们就需要找这个分布的均值 ${\mathbf{\mu }}_t$ 与方差 ${\mathbf{\Sigma }}_t$
对${\mathbf{J}}_t$关于${\mathbf{x}}_t$求一二阶导：
$$\begin{array}{rl}& \frac{\partial {\mathbf{J}}_t}{\partial {\mathbf{x}}_t}=-{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}({\mathbf{z}}_t-{\mathbf{C}}_t{\mathbf{x}}_t)+{\overline{\mathbf{\Sigma }}}_t^{-1}({\mathbf{x}}_t-{\overline{\mathbf{\mu }}}_t)\\ & \frac{{\partial }^2{\mathbf{J}}_t}{\partial {\mathbf{x}}_t^2}={\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t+{\overline{\mathbf{\Sigma }}}_t^{-1}\end{array}$$

相关公式：
1. $\frac{\partial \mathbf{A}\mathbf{x}}{\partial \mathbf{x}}={\mathbf{A}}^{\mathrm{T}}$
2. $\frac{\partial {\mathbf{x}}^{\mathrm{T}}\mathbf{A}\mathbf{x}}{\partial \mathbf{x}}=2\mathbf{A}\mathbf{x}$
3. 协方差矩阵是对称矩阵

根据多维高斯分布性质（1.6里有）可以求其均值方差，得到
$$\{\begin{array}{rl}& {\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}({\mathbf{z}}_t-{\mathbf{C}}_t{\mathbf{\mu }}_t)={\overline{\mathbf{\Sigma }}}_t^{-1}({\mathbf{\mu }}_t-{\overline{\mathbf{\mu }}}_t)\\ & {\mathbf{\Sigma }}_t^{-1}={\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t+{\overline{\mathbf{\Sigma }}}_t^{-1}\end{array}$$整理得^①
$$\{\begin{array}{rl}& {\mathbf{\mu }}_t={\overline{\mathbf{\mu }}}_t+{\mathbf{\Sigma }}_t{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}({\mathbf{z}}_t-{\mathbf{C}}_t{\overline{\mathbf{\mu }}}_t)\\ & {\mathbf{\Sigma }}_t^{-1}={\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t+{\overline{\mathbf{\Sigma }}}_t^{-1}\end{array}$$定义 ${\mathbf{\Sigma }}_t{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}={\mathbf{K}}_t$ 为卡尔曼增益
继续整理可得^②
$$\{\begin{array}{rl}& {\mathbf{K}}_t={\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t{)}^{-1}\\ & {\mathbf{\mu }}_t={\overline{\mathbf{\mu }}}_t+{\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{C}}_t{\overline{\mathbf{\mu }}}_t)\\ & {\mathbf{\Sigma }}_t=(\mathbf{I}-{\mathbf{K}}_t{\mathbf{C}}_t){\overline{\mathbf{\Sigma }}}_t\end{array}$$
对应了算法第4~6行

---

① 具体推导
$$\begin{array}{rl}{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}({\mathbf{z}}_t-{\mathbf{C}}_t{\mathbf{\mu }}_t)& ={\overline{\mathbf{\Sigma }}}_t^{-1}({\mathbf{\mu }}_t-{\overline{\mathbf{\mu }}}_t)\\ {\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{z}}_t-{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t{\mathbf{\mu }}_t& ={\overline{\mathbf{\Sigma }}}_t^{-1}{\mathbf{\mu }}_t-{\overline{\mathbf{\Sigma }}}_t^{-1}{\overline{\mathbf{\mu }}}_t\\ {\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{z}}_t+{\overline{\mathbf{\Sigma }}}_t^{-1}{\overline{\mathbf{\mu }}}_t& ={\overline{\mathbf{\Sigma }}}_t^{-1}{\mathbf{\mu }}_t+{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t{\mathbf{\mu }}_t\\ {\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{z}}_t+{\overline{\mathbf{\Sigma }}}_t^{-1}{\overline{\mathbf{\mu }}}_t& ={\mathbf{\Sigma }}_t^{-1}{\mathbf{\mu }}_t\\ {\mathbf{\Sigma }}_t{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{z}}_t+{\mathbf{\Sigma }}_t{\overline{\mathbf{\Sigma }}}_t^{-1}{\overline{\mathbf{\mu }}}_t& ={\mathbf{\mu }}_t\\ {\mathbf{K}}_t{\mathbf{z}}_t+{\mathbf{\Sigma }}_t{\overline{\mathbf{\Sigma }}}_t^{-1}{\overline{\mathbf{\mu }}}_t& ={\mathbf{\mu }}_t\\ {\mathbf{K}}_t{\mathbf{z}}_t+({\mathbf{\Sigma }}_t{\overline{\mathbf{\Sigma }}}_t^{-1}-\mathbf{I}){\overline{\mathbf{\mu }}}_t& ={\mathbf{\mu }}_t-{\overline{\mathbf{\mu }}}_t\\ {\mathbf{K}}_t{\mathbf{z}}_t+({\mathbf{\Sigma }}_t{\overline{\mathbf{\Sigma }}}_t^{-1}-{\mathbf{\Sigma }}_t{\mathbf{\Sigma }}_t^{-1}){\overline{\mathbf{\mu }}}_t& ={\mathbf{\mu }}_t-{\overline{\mathbf{\mu }}}_t\\ {\mathbf{K}}_t{\mathbf{z}}_t-{\mathbf{\Sigma }}_t({\mathbf{\Sigma }}_t^{-1}-{\overline{\mathbf{\Sigma }}}_t^{-1}){\overline{\mathbf{\mu }}}_t& ={\mathbf{\mu }}_t-{\overline{\mathbf{\mu }}}_t\\ {\mathbf{K}}_t{\mathbf{z}}_t-{\mathbf{\Sigma }}_t{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t{\overline{\mathbf{\mu }}}_t& ={\mathbf{\mu }}_t-{\overline{\mathbf{\mu }}}_t\\ {\mathbf{K}}_t{\mathbf{z}}_t-{\mathbf{K}}_t{\mathbf{C}}_t{\overline{\mathbf{\mu }}}_t& ={\mathbf{\mu }}_t-{\overline{\mathbf{\mu }}}_t\\ {\mathbf{\mu }}_t-{\overline{\mathbf{\mu }}}_t& ={\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{C}}_t{\overline{\mathbf{\mu }}}_t)\end{array}$$
② 具体推导
先推${\mathbf{K}}_t$，第4行公式中的${\mathbf{K}}_t$有个$({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t{)}^{-1}$，所以我们也要凑一下
${\mathbf{K}}_t={\mathbf{\Sigma }}_t{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}$
　　$={\mathbf{\Sigma }}_t$${\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}$$({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t)({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t{)}^{-1}$
　　$={\mathbf{\Sigma }}_t($${\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}$${\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+$${\mathbf{C}}_t^{\mathrm{T}}\underline{{\mathbf{Q}}_t^{-1}}$$\underline{{\mathbf{Q}}_t})({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t{)}^{-1}$
　　$={\mathbf{\Sigma }}_t({\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{C}}_t^{\mathrm{T}})({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t{)}^{-1}$
　　$={\mathbf{\Sigma }}_t({\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+\underline{{\overline{\mathbf{\Sigma }}}_t^{-1}{\overline{\mathbf{\Sigma }}}_t}{\mathbf{C}}_t^{\mathrm{T}})({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t{)}^{-1}$
　　$={\mathbf{\Sigma }}_t$$({\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t+{\overline{\mathbf{\Sigma }}}_t^{-1})$${\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t{)}^{-1}$
　　$={\mathbf{\Sigma }}_t$${\mathbf{\Sigma }}_t^{-1}$${\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t{)}^{-1}$
　　$={\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}({\mathbf{C}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^{\mathrm{T}}+{\mathbf{Q}}_t{)}^{-1}$
　　
再来推${\mathbf{\Sigma }}_t$
$$\begin{array}{rl}{\mathbf{K}}_t& ={\mathbf{\Sigma }}_t{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}\\ {\mathbf{K}}_t{\mathbf{C}}_t& ={\mathbf{\Sigma }}_t{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t\\ {\mathbf{K}}_t{\mathbf{C}}_t& ={\mathbf{\Sigma }}_t{\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t+{\mathbf{\Sigma }}_t{\overline{\mathbf{\Sigma }}}_t^{-1}-{\mathbf{\Sigma }}_t{\overline{\mathbf{\Sigma }}}_t^{-1}\\ {\mathbf{K}}_t{\mathbf{C}}_t& ={\mathbf{\Sigma }}_t({\mathbf{C}}_t^{\mathrm{T}}{\mathbf{Q}}_t^{-1}{\mathbf{C}}_t+{\overline{\mathbf{\Sigma }}}_t^{-1})-{\mathbf{\Sigma }}_t{\overline{\mathbf{\Sigma }}}_t^{-1}\\ {\mathbf{K}}_t{\mathbf{C}}_t& ={\mathbf{\Sigma }}_t{\mathbf{\Sigma }}_t^{-1}-{\mathbf{\Sigma }}_t{\overline{\mathbf{\Sigma }}}_t^{-1}\\ {\mathbf{K}}_t{\mathbf{C}}_t& =\mathbf{I}-{\mathbf{\Sigma }}_t{\overline{\mathbf{\Sigma }}}_t^{-1}\\ {\mathbf{\Sigma }}_t& =(\mathbf{I}-{\mathbf{K}}_t{\mathbf{C}}_t){\overline{\mathbf{\Sigma }}}_t\end{array}$$
在以上所有的推导中要注意${\mathbf{C}}_t$不一定可逆，所以不能去随便乘${\mathbf{C}}_t^{-1}$

---

至此，卡尔曼滤波推导完成。

4. 扩展卡尔曼滤波

(正在看… 后面有时间再写)

---

参考资料
Sebastian Thrun, Wolfram Burgard, Dieter Fox. Probabilistic Robotics [M].