卡尔曼滤波（Kalman Filter）

为什么需要卡尔曼滤波？

看上图，这其实是一个典型的测量模型，我们设y是观测到的值，x是隐变量。举个例子，x表示火箭燃料温度，可惜的是，燃料内部的温度太高，我们没有办法直接测量，只能测量他火箭外围的温度y，因此每一步的测量都伴随着随机误差，那么如何仅使用观测到的数据y来预测真实的x，这就是卡尔曼滤波(filter)所做的事情。

State Space Model

这个图模型有两类概率，第一类是$p(y_y|x_t)$，称为measurement probability,或者emission probability,另外还有$p(x_t|x_{t-1})$，称为转移概率，再加上一个初始概率$p(x_1)$，就可以完全表示这个state space model了。通过定义这几个概率的形式，就可以得到不同的模型：

1. HMM模型：(i)$p(x_t|x_{t-1})=A_{x_{t-1,t}}$,A一个离散的转移矩阵，x是离散的。(ii)$P(y_t|x_t)$是任意的。(iii)$p(x_1)=\pi$
2. 线性高斯SSM：(i)$p(x_t|x_{t-1})=\mathcal{N}(A{x}_{t-1}+B,Q)$ (ii)$P(y_t|x_t)=\mathcal{N}(H{x}_{t-1}+C,R)$。(iii)$p(x_1)=\mathcal{N}({\mu }_0,{\sigma }_0)$

对于上述dynamic模型或者State Space Model(SSM)来说，主要有4种任务：

1. Evaluation:$p(y_1,y_2,...,y_t)$
2. 参数学习:$\arg \underset{\theta }{\max }\log p(y_1,y_2,...,y|\theta )$
3. State decoding: $p(x_1,x_2,...,x_t|y_1,y_2,...,y_t)$
4. Filtering:$p(x_t|y_1,y_2,...,y_t)$

其实HMM和线性高斯SSM都可以做这4种任务，但是，HMM可能会更多的涉及第1,2类任务，而线性高斯SSM更多是涉及滤波任务，现在我们要讲的卡尔曼滤波就是做第4个任务filtering.

现在，我们形式化写一下线性高斯SSM的定义：

$$\begin{gathered} x_t=A{x}_{t-1}+B+w,w\sim N(0,Q) \\ {y}_t=H{x}_t+C+v,v\sim N(0,R) \\ cov(x_{t-1},w)=0,cov(x_t,v)=0,cov(w,v)=0 \end{gathered}$$

于是$p(x_t|x_{t-1})=\mathcal{N}(A{x}_{t-1}+B,Q),P(y_t|x_t)=\mathcal{N}(H{x}_{t-1}+C,R)$,为了推导的方便，我们暂时把B,C去掉。

卡尔曼滤波数学推导

现在我们看看怎么做滤波。考虑这个滤波的概率分布：

$$\begin{array}{rl}\underset{filteringatt}{\underbrace{p(x_t|y_1,y_2,...,y_t)}}& =\frac{p(x_t,y_1,y_2,...,y_t)}{p(y_1,y_2,...,y_t)}\\ & =\frac{p(y_t|x_t,y_1,y_2,...,y_{t-1})p(x_t,y_1,y_2,...,y_{t-1})}{p(y_1,y_2,...,y_t)}\\ & =\frac{p(y_t|x_t)p(x_t|y_1,y_2,...,y_{t-1})p(y_1,y_2,...,y_{t-1})}{p(y_1,y_2,...,y_t)}\\ & \propto p(y_t|x_t)\underset{Predictionatt}{\underbrace{p(x_t|y_1,y_2,...,y_{t-1})}}\end{array}$$

那个正比是因为观测值y是确定的，所以p(y)可以看作常数，于是我们发现，一个滤波，其实是prediction和一个生成概率的乘积，而如果我们把prediction展开来写：

$$\underset{predictionatt}{\underbrace{p(x_t|y_1,y_2,...,y_{t-1})}}=\int p(x_t|x_{t-1})\underset{filteringatt-1}{\underbrace{p(x_{t-1}|y_1,y_2,...,y_{t-1})}}d{x}_{t-1}=N\left(AE(x_{t-1}|y_1,y_2,...,y_{t-1}),A{\hat{\Sigma }}_{t-1}{A}^T+Q\right)$$

神奇的事情发生了，那就是我们的prediction恰好可以用上一时刻的滤波来算，这就形成了一个递归的过程，只要我们迭代地来算，那么整个滤波的都可以算出来！所以，基本套路就是，

1. 计算t时刻的prediction，然后t时刻的prediction就用到t-1时刻的filtering，
2. 计算t时刻的filtering，而t时刻的filtering又用到了t时刻的prediction。

那么这个迭代的过程就是从t=1开始，往下迭代计算：

$$\begin{array}{rlr}t=1:& (Filter)& p(x_1|y_1)\sim N\left({\hat{\mu }}_1,{\hat{\sigma }}_1\right)\\ t=2:& (Predict)& p(x_2|y_1)\sim N\left({\overline{\mu }}_2,{\overline{\sigma }}_2\right)\\ & (Filter)& p(x_2|y_1,y_2)\sim N\left({\hat{\mu }}_2,{\hat{\sigma }}_2\right)\\ t=3:& (Predict)& p(x_3|y_1,y_2)\sim N\left({\overline{\mu }}_3,{\overline{\sigma }}_3\right)\\ & (Filter)& p(x_3|y_1,y_2,y_3)\sim N\left({\hat{\mu }}_3,{\hat{\sigma }}_3\right)\end{array}$$

首先记住一个事实，只要随机变量组成的联合分布是高斯分布，那么这些变量的边缘概率分布，或者条件概率分布，都是服从高斯分布的。在这里，显然由于SSM服从线性高斯的模型，所以上面的这些条件概率分布都是服从高斯分布的。此外再记一个事实，当联合分布是高斯分布的时候，条件概率分布的高斯分布是这样的：

$$\begin{gathered} \left(\begin{array}{c}{\mathbf{x}}_1\\ {\mathbf{x}}_2\end{array}\right)\sim N\left(\left(\begin{array}{c}{\mathbf{\mu }}_1\\ {\mathbf{\mu }}_2\end{array}\right),\left(\begin{array}{cc}{\mathbf{\Sigma }}_{11}& {\mathbf{\Sigma }}_{12}\\ {\mathbf{\Sigma }}_{21}& {\mathbf{\Sigma }}_{22}\end{array}\right)\right) \\ p({\mathbf{x}}_1|{\mathbf{x}}_2)\sim N\left({\mathbf{\mu }}_1+{\mathbf{\Sigma }}_{12}{\mathbf{\Sigma }}_{22}^{-1}({\mathbf{x}}_2-{\mathbf{\mu }}_2),{\mathbf{\Sigma }}_{11}-{\mathbf{\Sigma }}_{12}{\mathbf{\Sigma }}_{22}^{-1}{\mathbf{\Sigma }}_{21}\right) \end{gathered}$$

那好现在，我们回顾一下，我们的目标就是要推导出一下两个公式的具体形式：

$$\begin{gathered} prediction:p(x_t|y_1,y_2,...,y_{t-1}) \\ filtering:p(x_t|y_1,y_2,...,y_t) \end{gathered}$$

我们先考虑prediction，想要预测t时刻f的真实状态$x_t$，那么根据SSM的定义，自然是需要t-1时刻的真实值$x_{t-1}$来预测的，而这个真实值是用t-1时刻的filtering得到的。那么t-1时刻的真实值(filter)是怎样的呢？设t-1时刻的filter为$p(x_{t-1}|y_1,y_2,...,y_{t-1})\sim N\left({\hat{\mu }}_{t-1},{\hat{\sigma }}_{t-1}\right)$,对其做重参数化：

$$x_{t-1}|y_1,y_2,...,y_{t-1}=E(x_{t-1}|y_1,...,y_{t-1})+\Delta x_{t-1},\Delta x_{t-1}\sim N\left(0,{\hat{\sigma }}_{t-1}\right)$$

因为这个分布是高斯分布，所以我们将他可以写成是均值加上一个随机变量来表示。于是t时刻的预测值可以通过下面的公式进行计算：

$$\begin{array}{rl}{x}_t|y_1,y_2,...,y_{t-1}& =A{x}_{t-1}+w\\ & =A(E(x_{t-1}|y_1,...,y_{t-1})+\Delta x_{t-1})+w\\ & =\underset{E(x_t|y_1,...,y_{t-1})}{\underbrace{AE(x_{t-1}|y_1,...,y_{t-1})}}+\underset{\Delta x_t}{\underbrace{A\Delta x_{t-1}+w}}\end{array}$$

好了现在t时刻prediction有了，那么t时刻filtering怎么得到呢？刚才介绍的用联合高斯分布来求条件概率分布的技巧就可以用上了，我们发现，如果能够写出这个联合分布的形式：

$$p(x_t,y_t|y_1,y_2,...,y_{t-1})$$

那不就能够用公式算出$p(x_t|y_1,y_2,...,y_{t-1},y_t)$的形式了吗？所以为了知道这个联合分布，我们还需要知道分布$p(y_t|y_1,y_2,...,y_{t-1})$的形式，于是同样的套路：

$$\begin{array}{rl}{y}_t|y_1,y_2,...,y_{t-1}& =H{x}_t+v\\ & =H(A{x}_{t-1}+w)+v\\ & =H(AE(x_{t-1}|y_1,...,y_{t-1})+A\Delta x_{t-1}+w)+v\\ & =\underset{E(y_t|y_1,y_2,...,y_{t-1})}{\underbrace{HAE(x_{t-1}|y_1,...,y_{t-1})}}+\underset{\Delta y_t}{\underbrace{HA\Delta x_{t-1}+Hw+v}}\end{array}$$

现在我们已经知道了两个高斯的概率形式了：

$$\begin{gathered} p(x_t|y_1,y_2,...,y_{t-1})\sim N\left(AE(x_{t-1}|y_1,...,y_{t-1}),\underset{{\overline{\Sigma }}_t}{\underbrace{E\left(\Delta x_t\Delta x_t^T\right)}}\right) \\ p(y_t|y_1,y_2,...,y_{t-1})\sim N\left(HAE(x_{t-1}|y_1,...,y_{t-1}),\underset{{\hat{\Sigma }}_t}{\underbrace{E\left(\Delta y_t\Delta y_t^T\right)}}\right) \end{gathered}$$

所以

$$p(x_t,y_t|y_1,y_2,...,y_{t-1})=N\left(\left(\begin{array}{c}AE(x_{t-1}|y_1,...,y_{t-1})\\ HAE(x_{t-1}|y_1,...,y_{t-1})\end{array}\right),\left(\begin{array}{cc}E\left(\Delta x_t\Delta x_t^T\right)& E\left(\Delta x_t\Delta y_t^T\right)\\ E\left(\Delta y_t^T\Delta x_t^T\right)& E\left(\Delta y_t\Delta y_t^T\right)\end{array}\right)\right)$$

现在，我们终于能够算$p(x_t|y_1,y_2,...,y_t)$了，剩下的问题就是的他协方差是什么，我们可以化简一下：

$$\begin{array}{rl}\Delta x_t\Delta x_t^T& =(A\Delta x_{t-1}+w)(A\Delta x_{t-1}+w{)}^T\\ & =(A\Delta x_{t-1}+w)\left(\Delta x_{t-1}^T{A}^T+w^T\right)\\ & =A\Delta x_{t-1}\Delta x_{t-1}^T{A}^T+\underset{cov=0}{\underbrace{w\Delta x_{t-1}^T{A}^T+w\Delta x_{t-1}^T{A}^T}}+w{w}^T\\ E\left(\Delta x_t\Delta x_t^T\right)& =AE\left(\Delta x_{t-1}\Delta x_{t-1}^T\right)A^T+E\left(w{w}^T\right)\\ & =A{\hat{\Sigma }}_{t-1}{A}^T+Q\end{array}$$

同理，对于y

$$\begin{array}{rl}\Delta y_t\Delta y_t^T& =(HA\Delta x_{t-1}+Hw+v)(HA\Delta x_{t-1}+Hw+v{)}^T\\ & =(HA\Delta x_{t-1}+Hw+v)\left(\Delta x_{t-1}^T{A}^T{H}^T+w^T{H}^T+v^T\right)\\ & =HA\Delta x_{t-1}\Delta x_{t-1}^T{A}^T{H}^T+Hw{w}^T{H}^T+v{v}^T+\underset{cov=0}{\underbrace{...}}\\ E\left(\Delta y_t\Delta y_t^T\right)& =HA\left(\Delta x_{t-1}\Delta x_{t-1}^T\right)A^T{H}^T+HE\left(w{w}^T\right)H^T+E\left(v{v}^T\right)\\ & =HA{\hat{\Sigma }}_{t-1}{A}^T{H}^T+HQ{H}^T+R\end{array}$$

对于交叉项：

$$\begin{array}{rl}\Delta x_t\Delta y_t^T& =(A\Delta x_{t-1}+w)(HA\Delta x_{t-1}+Hw+v{)}^T\\ & =(A\Delta x_{t-1}+w)\left(\Delta x_{t-1}^T{A}^T{H}^T+w^T{H}^T+v^T\right)\\ & =A\Delta x_{t-1}\Delta x_{t-1}^T{A}^T{H}^T+w{w}^T{H}^T+\underset{cov=0}{\underbrace{...}}\\ E\left(\Delta y_t\Delta y_t^T\right)& =A\left(\Delta x_{t-1}\Delta x_{t-1}^T\right)A^T{H}^T+E\left(w{w}^T\right)H^T\\ & =\left(A{\hat{\Sigma }}_{t-1}{A}^T+Q\right)H^T\\ & ={\overline{\Sigma }}_t{H}^T\end{array}$$

至此推导完成，我们可以不停地迭代计算卡尔曼滤波了！总结一下，基本流程就是$filterin{g}_1\to predictio{n}_2\to filterin{g}_2\to predictio{n}_3\to ...$

附录：多元高斯分布

假设$\mathbf{x}=({\mathbf{x}}_1,{\mathbf{x}}_2)$是联合高斯分布，并且参数为：

$$\mathbf{\mu }=\left(\begin{array}{c}{\mathbf{\mu }}_1\\ {\mathbf{\mu }}_2\end{array}\right),\mathbf{\Sigma }=\left(\begin{array}{cc}{\mathbf{\Sigma }}_{11}& {\mathbf{\Sigma }}_{12}\\ {\mathbf{\Sigma }}_{21}& {\mathbf{\Sigma }}_{22}\end{array}\right),\mathbf{\Lambda }={\mathbf{\Sigma }}^{-1}=\left(\begin{array}{cc}{\mathbf{\Lambda }}_{11}& {\mathbf{\Lambda }}_{12}\\ {\mathbf{\Lambda }}_{21}& {\mathbf{\Lambda }}_{22}\end{array}\right)$$

那么他们的边缘分布为

$$\begin{array}{rl}p({\mathbf{x}}_1)& =\mathcal{N}({\mathbf{x}}_1|{\mathbf{\mu }}_1,{\mathbf{\Sigma }}_{11})\\ p({\mathbf{x}}_2)& =\mathcal{N}({\mathbf{x}}_2|{\mathbf{\mu }}_2,{\mathbf{\Sigma }}_{22})\end{array}$$

以及其条件分布为：

$$\begin{array}{rl}p({\mathbf{x}}_1|{\mathbf{x}}_2)& =\mathcal{N}({\mathbf{x}}_1|{\mathbf{\mu }}_{1|2},{\mathbf{\Sigma }}_{1|2})\\ {\mathbf{\mu }}_{1|2}& ={\mathbf{\mu }}_1+{\mathbf{\Sigma }}_{12}{\mathbf{\Sigma }}_{22}^{-1}({\mathbf{x}}_2-{\mathbf{\mu }}_2)\\ & ={\mathbf{\mu }}_1-{\mathbf{\Lambda }}_{11}^{-1}{\mathbf{\Lambda }}_{12}({\mathbf{x}}_2-{\mathbf{\mu }}_2)\\ & ={\mathbf{\Sigma }}_{1|2}({\mathbf{\Lambda }}_{11}{\mathbf{\mu }}_1-{\mathbf{\Lambda }}_{12}({\mathbf{x}}_2-{\mathbf{\mu }}_2))\\ {\mathbf{\Sigma }}_{1|2}& ={\mathbf{\Sigma }}_{11}-{\mathbf{\Sigma }}_{12}{\mathbf{\Sigma }}_{22}^{-1}{\mathbf{\Sigma }}_{21}\end{array}$$

以上条件分布非常重要！
这个东西是怎么推出来的呢？他的推导比较直接，那就是利用概率分解：

$$p({\mathbf{x}}_1,{\mathbf{x}}_2)=p({\mathbf{x}}_1|{\mathbf{x}}_2)p({\mathbf{x}}_2)$$

只要我们把$p({\mathbf{x}}_1,{\mathbf{x}}_2)$和$p({\mathbf{x}}_2)$都写出来，自然就知道$p({\mathbf{x}}_1|{\mathbf{x}}_2)$是什么了。首先对于$p({\mathbf{x}}_1,{\mathbf{x}}_2)$的分布如下：

$$E=\exp \left\{-\frac{1}{2}{\left(\begin{array}{c}{\mathbf{x}}_1-{\mathbf{\mu }}_1\\ {\mathbf{x}}_2-{\mathbf{\mu }}_2\end{array}\right)}^T{\left(\begin{array}{cc}{\mathbf{\Sigma }}_{11}& {\mathbf{\Sigma }}_{12}\\ {\mathbf{\Sigma }}_{21}& {\mathbf{\Sigma }}_{22}\end{array}\right)}^{-1}\left(\begin{array}{c}{\mathbf{x}}_1-{\mathbf{\mu }}_1\\ {\mathbf{x}}_2-{\mathbf{\mu }}_2\end{array}\right)\right\}$$

于是我们用公式将逆矩阵展开得到：

E = exp ⁡ { − 1 2 ( x 1 − μ 1 x 2 − μ 2 ) T ( I 0 Σ 22 − 1 Σ 21 I ) ( ( Σ / Σ 22 ) − 1 0 0 Σ 22 − 1 ) × ( I − Σ 12 Σ 22 − 1 0 I ) ( x 1 − μ 1 x 2 − μ 2 ) } = exp ⁡ { − 1 2 ( x 1 − μ 1 − Σ 12 Σ 22 − 1 ( x 2 − μ 2 ) ) ⏟ μ 1 ∣ 2 T ( Σ / Σ 22 ⏟ Σ 1 ∣ 2 ) − 1 ( x 1 − μ 1 − Σ 12 Σ 22 − 1 ( x 2 − μ 2 ) ) ⏟ μ 1 ∣ 2 } × exp ⁡ { − 1 2 ( x 2 − μ 2 ) T Σ 22 − 1 ( x 2 − μ 2 ) } = p ( x 1 ∣ x 2 ) p ( x 2 ) \begin{aligned} E= & \exp\{-\frac{1}{2}\left(\begin{array}{ c } \mathbf{x}_{1} -\boldsymbol{\mu }_{1}\\ \mathbf{x}_{2} -\boldsymbol{\mu }_{2} \end{array}\right)^{T}\left(\begin{array}{ c c } \mathbf{I} & \mathbf{0}\\ \mathbf{\Sigma }^{-1}_{22}\boldsymbol{\Sigma }_{21} & \mathbf{I} \end{array}\right)\left(\begin{array}{ c c } (\boldsymbol{\Sigma } /\boldsymbol{\Sigma }_{22})^{-1} & \mathbf{0}\\ \mathbf{0} & \mathbf{\Sigma }^{-1}_{22} \end{array}\right)\\ & \times \left(\begin{array}{ c c } \mathbf{I} & -\boldsymbol{\Sigma }_{12}\boldsymbol{\Sigma }^{-1}_{22}\\ \mathbf{0} & \mathbf{I} \end{array}\right)\left(\begin{array}{ l } \mathbf{x}_{1} -\boldsymbol{\mu }_{1}\\ \mathbf{x}_{2} -\boldsymbol{\mu }_{2} \end{array}\right)\}\\ = & \exp\left\{-\frac{1}{2}\underbrace{\left(\mathbf{x}_{1} -\boldsymbol{\mu }_{1} -\boldsymbol{\Sigma }_{12}\boldsymbol{\Sigma }^{-1}_{22}(\mathbf{x}_{2} -\boldsymbol{\mu }_{2})\right)}_{\boldsymbol{\mu }_{1|2}}^{T}\left(\underbrace{\boldsymbol{\Sigma } /\mathbf{\Sigma }_{22}}_{\mathbf{\Sigma }_{1|2}}\right)^{-1}\underbrace{\left(\mathbf{x}_{1} -\boldsymbol{\mu }_{1} -\mathbf{\Sigma }_{12}\boldsymbol{\Sigma }^{-1}_{22}(\mathbf{x}_{2} -\boldsymbol{\mu }_{2})\right)}_{\boldsymbol{\mu }_{1|2}}\right\}\\ & \times \exp\left\{-\frac{1}{2}(\mathbf{x}_{2} -\boldsymbol{\mu }_{2})^{T}\boldsymbol{\Sigma }^{-1}_{22}(\mathbf{x}_{2} -\boldsymbol{\mu }_{2})\right\}\\ = & p(\mathbf{x}_{1} |\mathbf{x}_{2}) p(\mathbf{x}_{2}) \end{aligned} E===​exp{−21​(x1​−μ1​x2​−μ2​​)T(IΣ22−1​Σ21​​0I​)((Σ/Σ22​)−10​0Σ22−1​​)×(I0​−Σ12​Σ22−1​I​)(x1​−μ1​x2​−μ2​​)}exp⎩⎪⎨⎪⎧​−21​T (x1​−μ1​−Σ12​Σ22−1​(x2​−μ2​))​​⎝⎜⎛​Σ1∣2​ Σ/Σ22​​​⎠⎟⎞​−1μ1∣2​ (x1​−μ1​−Σ12​Σ22−1​(x2​−μ2​))​​⎭⎪⎬⎪⎫​×exp{−21​(x2​−μ2​)TΣ22−1​(x2​−μ2​)}p(x1​∣x2​)p(x2​)​

最终我们发现，条件高斯分布的期望和方差分别就是

$$\begin{array}{rl}{\mathbf{\mu }}_{1|2}& ={\mathbf{\mu }}_1+{\mathbf{\Sigma }}_{12}{\mathbf{\Sigma }}_{22}^{-1}({\mathbf{x}}_2-{\mathbf{\mu }}_2)\\ {\mathbf{\Sigma }}_{1|2}& =\mathbf{\Sigma }/{\mathbf{\Sigma }}_{22}={\mathbf{\Sigma }}_{11}-{\mathbf{\Sigma }}_{12}{\mathbf{\Sigma }}_{22}^{-1}{\mathbf{\Sigma }}_{21}\end{array}$$

参考资料

文本思路主要参考了徐亦达老师的课程：徐亦达 卡尔曼滤波

https://en.wikipedia.org/wiki/Kalman_filter