贝叶斯滤波和贝叶斯平滑(Kalman滤波，RTS平滑)

贝叶斯滤波(Bayesian filtering equations)

贝叶斯滤波的目的是在观测到信号${\mathbf{y}}_{1:k}$后，得到当前第$k$个时刻的状态${\mathbf{x}}_k$的后验边际分布：
$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})$$

贝叶斯滤波方程

定理(Bayesian filtering equations)
The recursive equations (the Bayesian filter) for computing the predicted distribution $p({\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})$ and the filtering distribution $p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})$ at the time $k$ are given by the following Bayesian filtering eqautions.

• Initialization: 迭代从先验分布$p({\mathbf{x}}_0)$开始

• Prediction: 给定动态模型(dynamic model)$p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})$，状态${\mathbf{x}}_k$的预测分布(predictive distribution)为：
  $$p({\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})=\int p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})p({\mathbf{x}}_{k-1}|{\mathbf{y}}_{1:k-1})\text{d}{\mathbf{x}}_{k-1}\text{\hspace{1em}(1)}$$

• Update: 给定前$k$次观测向量${\mathbf{y}}_{1:k}$，状态${\mathbf{x}}_k$的后验分布为
  $$p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})=\frac{1}{Z_k}p({\mathbf{y}}_k|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})\text{\hspace{1em}(2)}$$其中$Z_k$是归一化系数：
  $$Z_k=\int \frac{1}{Z_k}p({\mathbf{y}}_k|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})\text{d}{\mathbf{x}}_k\text{\hspace{1em}(3)}$$

根据之前的博客概率状态空间模型的马尔可夫性质和观测向量的条件独立性，不难证明上述等式，这里省略。

Kalman滤波

在经典Kalman滤波中，动态模型(dynamic model)和测量模型(measurement model)都是线性高斯的：
$$\begin{array}{rl}\text{dynamic model: }& {\mathbf{x}}_k={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{q}}_{k-1}\\ \text{measurement model: }& {\mathbf{y}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{r}}_k\end{array}\text{\hspace{1em}(4)}$$

其中

• ${\mathbf{x}}_k\in {\mathbb{R}}^n$ 是$k$时刻的状态
• ${\mathbf{y}}_k\in {\mathbb{R}}^m$ 是$k$时刻的测量向量
• ${\mathbf{q}}_{k-1}\sim \mathcal{N}(0,{\mathbf{Q}}_{k-1})$是过程噪声(process noise)
• ${\mathbf{r}}_k\sim \mathcal{N}(0,{\mathbf{R}}_k)$是测量噪声
• ${\mathbf{x}}_0\sim \mathcal{N}({\mathbf{m}}_0,{\mathbf{P}}_0)$是状态的先验分布
• ${\mathbf{A}}_{k-1}$是动态模型中的变换矩阵(transition matrix)
• ${\mathbf{H}}_k$是测量矩阵

用概率的形式表示模型(4)为：
$$\begin{array}{rl}p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})& =\mathcal{N}({\mathbf{x}}_k|{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1},{\mathbf{Q}}_{k-1})\\ p({\mathbf{y}}_k|{\mathbf{x}}_k)& =\mathcal{N}({\mathbf{x}}_k|{\mathbf{H}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)\end{array}\text{\hspace{1em}(5)}$$

定理(Kalman Filter)
The Bayesian filtering equations for the linear filtering model (4) can be evaluated in closed form and the resulting distributions are Gaussian:
$$\begin{array}{rl}p({\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})& =\mathcal{N}({\mathbf{x}}_k|{\mathbf{m}}_k^{-},{\mathbf{P}}_k^{-})\\ p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})& =\mathcal{N}({\mathbf{x}}_k|{\mathbf{m}}_k^{},{\mathbf{P}}_k)\\ p({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1})& =\mathcal{N}({\mathbf{y}}_k|{\mathbf{H}}_k{\mathbf{m}}_k^{-},{\mathbf{S}}_k)\end{array}\text{\hspace{1em}(6)}$$

上述分布的参数的迭代可以被描述为

• prediction step:
  $$\begin{array}{rl}{\mathbf{m}}_k^{-}& ={\mathbf{A}}_{k-1}{\mathbf{m}}_{k-1}\\ {\mathbf{P}}_k^{-}& ={\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_{k-1}\end{array}\text{\hspace{1em}(7)}$$

• update step
  $$\begin{array}{rl}{\mathbf{v}}_k& ={\mathbf{y}}_k-{\mathbf{H}}_k{\mathbf{m}}_k^{-}\\ {\mathbf{S}}_k& ={\mathbf{H}}_k{\mathbf{P}}_k^{-1}{\mathbf{H}}_k^T+{\mathbf{R}}_k\\ {\mathbf{K}}_k& ={\mathbf{P}}_k^{-1}{\mathbf{H}}_k^T+{\mathbf{R}}_k\\ {\mathbf{m}}_k& ={\mathbf{m}}_k^{-}+{\mathbf{K}}_k{\mathbf{v}}_k\\ {\mathbf{P}}_k& ={\mathbf{P}}_k^{-1}-{\mathbf{K}}_k{\mathbf{S}}_k{\mathbf{K}}_k^T\end{array}\text{\hspace{1em}(8)}$$

上述迭代过程从先验分布的均值${\mathbf{m}}_0$和协方差${\mathbf{P}}_0$开始。
证明：
（1）prediction相关
联合高斯分布与条件高斯分布的相关性质(之前写的博客)总结了联合高斯分布的表达式，据此，我们可以得到
$$\begin{array}{rl}& p({\mathbf{x}}_{k-1},{\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})\\ & =p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})p({\mathbf{x}}_{k-1}|{\mathbf{y}}_{1:k-1})\\ & =\mathcal{N}({\mathbf{x}}_k|{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1},{\mathbf{Q}}_{k-1})\cdot \mathcal{N}({\mathbf{x}}_{k-1}|{\mathbf{m}}_{k-1},{\mathbf{P}}_{k-1})\\ & =\mathcal{N}\left(\left[\begin{array}{c}{\mathbf{x}}_{k-1}\\ {\mathbf{x}}_k\end{array}\right]|{\mathbf{m}}^{\prime },{\mathbf{P}}^{\prime }\right)\end{array}$$

其中
$$\begin{array}{rl}{\mathbf{m}}^{\prime }& =\left[\begin{array}{c}{\mathbf{m}}_{k-1}\\ {\mathbf{A}}_{k-1}{\mathbf{m}}_{k-1}\end{array}\right]\\ {\mathbf{P}}^{\prime }& =\left[\begin{array}{cc}{\mathbf{P}}_{k-1}& {\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T\\ {\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}& {\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_{k-1}\end{array}\right]\end{array}$$

根据上述博客总结中的边际概率表达式，我们可以写出
$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})=\mathcal{N}({\mathbf{x}}_k|{\mathbf{m}}_k^{-},{\mathbf{P}}_k^{-})$$

其中
$${\mathbf{m}}_k^{-}={\mathbf{A}}_{k-1}{\mathbf{m}}_{k-1},{\mathbf{P}}_k^{-}={\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_{k-1}$$

（2）Update相关
类似地，我们可以得到
$$\begin{array}{rl}& p({\mathbf{x}}_k,{\mathbf{y}}_k|{\mathbf{y}}_{1:k-1})\\ & =p({\mathbf{y}}_k|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})\\ & =\mathcal{N}({\mathbf{y}}_k|{\mathbf{H}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)\cdot \mathcal{N}({\mathbf{x}}_k|{\mathbf{m}}_k^{-},{\mathbf{P}}_k^{-})\\ & =\mathcal{N}\left(\left[\begin{array}{c}{\mathbf{x}}_k\\ {\mathbf{y}}_k\end{array}\right]|{\mathbf{m}}^{\prime \prime },{\mathbf{P}}^{\prime \prime }\right)\end{array}$$

其中
$$\begin{array}{rl}{\mathbf{m}}^{\prime \prime }& =\left[\begin{array}{c}{\mathbf{m}}_k^{-}\\ {\mathbf{H}}_k{\mathbf{m}}_k^{-}\end{array}\right]\\ {\mathbf{P}}^{\prime \prime }& =\left[\begin{array}{cc}{\mathbf{P}}_k^{-}& {\mathbf{P}}_k^{-}{\mathbf{H}}_k^T\\ {\mathbf{H}}_k{\mathbf{P}}_k^{-}& {\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k\end{array}\right]\end{array}$$

进一步，关于$p({\mathbf{x}}_k,{\mathbf{y}}_k|{\mathbf{y}}_{1:k-1})$的边际概率分布为
$$\begin{array}{rl}p({\mathbf{x}}_k|{\mathbf{y}}_k,{\mathbf{y}}_{1:k-1})& =p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})\\ & =\mathcal{N}({\mathbf{x}}_k|{\mathbf{m}}_k,{\mathbf{P}}_k)\end{array}$$

其中
$$\begin{array}{rl}{\mathbf{m}}_k& ={\mathbf{m}}_k^{-}+{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)}^{-1}\left({\mathbf{y}}_k-{\mathbf{H}}_k{\mathbf{m}}_k^{-1}\right)\\ {\mathbf{P}}_k& ={\mathbf{P}}_k^{-}-{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)}^{-1}{\mathbf{H}}_k{\mathbf{P}}_k^{-}\end{array}$$
上述迭代过程，也可以被写为式(7,8)的形式。完成证明。

贝叶斯平滑(Bayesian smoothing)

贝叶斯平滑的目的是在观测到信号${\mathbf{y}}_{1:T}$后，计算之前第$k$个时刻的状态${\mathbf{x}}_k$的后验边际概率：
$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:T}),T>k$$

贝叶斯最优平滑方程

定理(Bayesian optimal smoothing equations)
The backward recursive equations (the Bayesian smoother) for computing the smoothed distributions $p({\mathbf{x}}_k|{\mathbf{y}}_{1:T})$ for any $k<T$ are given by the following Bayesian (fixed-interval) smooth equations:
$$\begin{array}{rl}p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})& =\int p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})\text{d}{\mathbf{x}}_k\\ p({\mathbf{x}}_k|{\mathbf{y}}_{1:T})& =p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})\int \left[\frac{p({\mathbf{x}}_{k+1}|{\mathbf{x}}_K)p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})}\right]\text{d}{\mathbf{x}}_{k+1}\end{array}\text{\hspace{1em}(9)}$$

证明：注意式(1)的第一个等式，积分项里边本应为：$p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k,{\mathbf{y}}_{1:k})p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})$，但是因为马尔可夫性，所以$p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k,{\mathbf{y}}_{1:k})=p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)$。

回顾上一篇博客概率状态空间模型的马尔可夫性质，我们知道$p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})=p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k})$，借助Bayes公式，我们有
$$\begin{array}{rl}p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})& =p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k})\\ & =\frac{p({\mathbf{x}}_k,{\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})}\\ & =\frac{p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k,{\mathbf{y}}_{1:k})p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})}\\ & =\frac{p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})}\end{array}$$

给定${\mathbf{y}}_{1:T}$，${\mathbf{x}}_k$和${\mathbf{x}}_{k+1}$的联合分布为：
$$\begin{array}{rl}p({\mathbf{x}}_k,{\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})& =p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})\\ & =p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k})p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})\\ & =\frac{p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})}{p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})}\end{array}$$

其中$p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})$是时刻$k+1$的平滑分布(smoothed distribution)，关于${\mathbf{x}}_k$的边际分布就是对上式关于${\mathbf{x}}_{k+1}$积分，得到式(1)。

Rauch-Tung-Striebel smoother

Rauch-Tung-Striebel smoother(RTSS)的平滑解：
$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:T})=\mathcal{N}({\mathbf{x}}_k|{\mathbf{m}}_k^s,{\mathbf{P}}_k^s)\text{\hspace{1em}(10)}$$

定理(RTS smoother)
The backward recursion equations for the (fixed interval) Rauch-Tung-Striebel smoother are given as
$$\begin{array}{rl}{\mathbf{m}}_{k+1}^{-}& ={\mathbf{A}}_k{\mathbf{m}}_k\\ {\mathbf{P}}_{k+1}^{-}& ={\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T+{\mathbf{Q}}_k\\ {\mathbf{G}}_k& ={\mathbf{P}}_k{\mathbf{A}}_k^T[{\mathbf{P}}_{k+1}^{-}{]}^{-1}\\ {\mathbf{m}}_k^s& ={\mathbf{m}}_k^{}+{\mathbf{G}}_k[{\mathbf{m}}_{k+1}^s-{\mathbf{m}}_{k+1}^{-}]\\ {\mathbf{P}}_k^s& ={\mathbf{P}}_k^{}+{\mathbf{G}}_k[{\mathbf{P}}_{k+1}^s-{\mathbf{P}}_{k+1}^{-}]{\mathbf{G}}_k^T\end{array}\text{\hspace{1em}(11)}$$

其中${\mathbf{m}}_k$和${\mathbf{P}}_k$由Kalman滤波得到。迭代过程从时刻$T$开始，且${\mathbf{m}}_T^s={\mathbf{m}}_T$，${\mathbf{P}}_T^s={\mathbf{P}}_T$

联合高斯分布与条件高斯分布的相关性质(之前写的博客)总结了联合高斯分布的表达式，据此，给定${\mathbf{y}}_{1:k}$，${\mathbf{x}}_k$和${\mathbf{x}}_{k+1}$的联合概率密度函数为
$$\begin{array}{rl}& p({\mathbf{x}}_k,{\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:k})\\ & =p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{y}}_{1:k})\\ & =\mathcal{N}({\mathbf{x}}_{k+1}|{\mathbf{A}}_k{\mathbf{x}}_k,{\mathbf{Q}}_k)\cdot \mathcal{N}({\mathbf{x}}_k|{\mathbf{m}}_k,{\mathbf{P}}_k)\\ & =\mathcal{N}\left(\left[\begin{array}{c}{\mathbf{x}}_k\\ {\mathbf{x}}_{k+1}\end{array}\right]|{\tilde{\mathbf{m}}}_1,{\tilde{\mathbf{P}}}_1\right)\end{array}$$

其中
$$\begin{array}{rl}{\tilde{\mathbf{m}}}_1& =\left[\begin{array}{c}{\mathbf{m}}_k\\ {\mathbf{A}}_k{\mathbf{m}}_k\end{array}\right]\\ {\tilde{\mathbf{P}}}_1& =\left[\begin{array}{cc}{\mathbf{P}}_k& {\mathbf{P}}_k{\mathbf{A}}_k^T\\ {\mathbf{A}}_k{\mathbf{P}}_k& {\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T+{\mathbf{Q}}_k\end{array}\right]\end{array}$$

根据状态的马尔可夫性，我们有
$$p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})=p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k})$$

根据上述博客总结中的边际概率表达式，我们可以写出
$$\begin{array}{rl}p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})& =p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:k})\\ & =\mathcal{N}({\mathbf{x}}_k|{\tilde{\mathbf{m}}}_2,{\tilde{\mathbf{P}}}_2)\end{array}$$

其中
$$\begin{array}{rl}{\mathbf{G}}_k& ={\mathbf{P}}_k{\mathbf{A}}_k^T{\left({\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T+{\mathbf{Q}}_k\right)}^{-1}\\ {\tilde{\mathbf{m}}}_2& ={\mathbf{m}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_{k+1}-{\mathbf{A}}_k{\mathbf{m}}_k\right)\\ {\tilde{\mathbf{P}}}_2& ={\mathbf{P}}_k-{\mathbf{G}}_k\left({\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T+{\mathbf{Q}}_k\right){\mathbf{G}}_k^T\end{array}$$

给定所有数据${\mathbf{y}}_{1:T}$，${\mathbf{x}}_k$和${\mathbf{x}}_{k+1}$的联合分布为
$$\begin{array}{rl}p({\mathbf{x}}_{k+1},{\mathbf{x}}_{\mathbf{k}}|{\mathbf{y}}_{1:T})& =p({\mathbf{x}}_k|{\mathbf{x}}_{k+1},{\mathbf{y}}_{1:T})p({\mathbf{x}}_{k+1}|{\mathbf{y}}_{1:T})\\ & =\mathcal{N}({\mathbf{x}}_k|{\tilde{\mathbf{m}}}_2,{\tilde{\mathbf{P}}}_2)\mathcal{N}({\mathbf{x}}_{k+1}|{\mathbf{m}}_{k+1}^s,{\mathbf{P}}_{k+1}^s)\\ & =\mathcal{N}\left(\left[\begin{array}{c}{\mathbf{x}}_{k+1}\\ {\mathbf{x}}_k\end{array}\right]|{\tilde{\mathbf{m}}}_3,{\tilde{\mathbf{P}}}_3\right)\end{array}$$

其中
$$\begin{gathered} {\tilde{\mathbf{m}}}_3=\left[\begin{array}{c}{\mathbf{m}}_{k+1}^s\\ {\mathbf{m}}_k+{\mathbf{G}}_k({\mathbf{m}}_{k+1}^s-{\mathbf{A}}_k{\mathbf{m}}_k)\end{array}\right] \\ {\tilde{\mathbf{P}}}_3=\left[\begin{array}{cc}{\mathbf{P}}_{k+1}^s& {\mathbf{P}}_{k+1}^s{\mathbf{G}}_k^T\\ {\mathbf{G}}_k{\mathbf{P}}_{k+1}^s& {\mathbf{G}}_k{\mathbf{P}}_{k+1}^s{\mathbf{G}}_k^T+{\tilde{\mathbf{P}}}_2\end{array}\right] \end{gathered}$$

进一步，关于${\mathbf{x}}_k$的边际概率分布为
$$p({\mathbf{x}}_k|{\mathbf{y}}_{1:T})=\mathcal{N}({\mathbf{x}}_k|{\mathbf{m}}_k^s,{\mathbf{P}}_k^s)$$

其中
$$\begin{array}{rl}{\mathbf{m}}_k^s& ={\mathbf{m}}_k+{\mathbf{G}}_k({\mathbf{m}}_{k+1}^s-{\mathbf{A}}_k{\mathbf{m}}_k)\\ {\mathbf{P}}_k^s& ={\mathbf{P}}_k+{\mathbf{G}}_k({\mathbf{P}}_k^s-{\mathbf{A}}_k{\mathbf{P}}_k{\mathbf{A}}_k^T-{\mathbf{Q}}_k){\mathbf{G}}_k^T\end{array}$$

这样也就完成了对式(11)，关于RTS平滑的证明。