贝叶斯滤波器

贝叶斯滤波器

定义以下运动和观测模型：

$$\begin{array}{rl}& \text{运动方程：}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right),k=1,\cdots ,K\\ & \text{观测方程：}{\mathbf{y}}_k=g\left({\mathbf{x}}_k,{\mathbf{n}}_k\right),k=0,\cdots ,K\end{array}$$

变量和符号定义如下：

• ${\mathbf{x}}_k\in {\mathbb{R}}^N$ 表示系统状态
• ${\mathbf{x}}_0\in {\mathbb{R}}^N$ 表示初始状态
• ${\mathbf{v}}_k\in {\mathbb{R}}^N$ 表示输入
• ${\mathbf{w}}_k\in {\mathbb{R}}^N$ 表示过程噪声
• ${\mathbf{y}}_k\in {\mathbb{R}}^M$ 表示测量
• ${\mathbf{n}}_k\in {\mathbb{R}}^M$ 表示测量噪声
• $k$ 为时间下标，最大值为 $K$
• 函数 $f(\cdot )$ 为非线性的运动模型
• 函数 $g(\cdot )$ 为非线性的观测模型

贝叶斯滤波仅使用过去以及当前的测量，通过构造一个完整的概率密度函数PDF，来计算当前状态 ${\mathbf{x}}_k$ 的置信度：

$$P\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)\text{\hspace{1em}(1)}$$

其中， ${\check{\mathbf{x}}}_0$ 表示 0 时刻系统状态的先验。

贝叶斯滤波器可以用下面公式表示：

$$\begin{array}{rl}& \underset{\text{后验置信度 }}{\underbrace{p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)}}\\ =& \eta \underset{\text{利用 }\mathbf{g}(\cdot )\text{ 进行更新 }}{\underbrace{p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)}}\int \underset{\text{利用 }\mathbf{f}(\cdot )\text{ 进行预测 }}{\underbrace{p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)}}\underset{\text{先验置信度 }}{\underbrace{p\left({\mathbf{x}}_{k-1}\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k-1},{\mathbf{y}}_{0:k-1}\right)}}\mathrm{d}{\mathbf{x}}_{k-1}\end{array}\text{\hspace{1em}(2)}$$

推导过程

对（1）式进行贝叶斯展开，有：

$$\begin{array}{rl}& p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)\\ & \\ & =\frac{p\left({\mathbf{x}}_k,{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)}{p\left({\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)}\\ & \\ & =\frac{p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k,{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)p\left({\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}{p\left({\mathbf{y}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)p\left({\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}\\ & \\ & =\frac{p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k,{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}{p\left({\mathbf{y}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}\end{array}\text{\hspace{1em}(3)}$$

由于 ${\mathbf{y}}_k$ 的状态只与 ${\mathbf{x}}_k$ 相关，所以上式可化简为：

$$\begin{array}{rl}& p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)\\ & \\ & =\frac{p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}{p\left({\mathbf{y}}_k\right)}\end{array}\text{\hspace{1em}(4)}$$

令 $p^{-1}\left({\mathbf{y}}_k\right)=\eta$，则有：

$$\begin{array}{rl}& p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)=\eta p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\end{array}\text{\hspace{1em}(5)}$$

由全概率公式和贝叶斯公式有：

$$\begin{array}{rl}& p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\\ & \\ =& \int p\left({\mathbf{x}}_k,{\mathbf{x}}_{k-1}\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}\\ & \\ =& \int p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)p\left({\mathbf{x}}_{k-1}\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}\end{array}\text{\hspace{1em}(6)}$$

然后，由一阶马尔可夫性假设（ k 时刻的状态只和 k - 1 时刻状态有关）有：

$$p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)=p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)\text{\hspace{1em}(7)}$$

并且 k - 1 时刻的状态与 k 时刻的输入无关，即：

$$p\left({\mathbf{x}}_{k-1}\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)=p\left({\mathbf{x}}_{k-1}\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k-1},{\mathbf{y}}_{0:k-1}\right)\text{\hspace{1em}(8)}$$

结合 (5)、(6)、(7)、(8) 式，即可得到：

$$\begin{array}{rl}& p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)\\ =& \eta p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)\int p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)p\left({\mathbf{x}}_{k-1}\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k-1},{\mathbf{y}}_{0:k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}\end{array}\text{\hspace{1em}(9)}$$

即证。