卡尔曼滤波器KF

卡尔曼滤波器

卡尔曼滤波器是贝叶斯滤波器在线性高斯系统下的一个特例。

假设线性高斯系统下运动模型和观测模型如下：

$$\begin{array}{rl}& \text{运动方程：}{\mathbf{x}}_k={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k+{\mathbf{w}}_k,k=1,\cdots ,K\\ & \text{观测方程：}{\mathbf{y}}_k={\mathbf{C}}_k{x}_k+{\mathbf{n}}_k,k=0,\cdots ,K\end{array}$$

其中：

• ${\mathbf{x}}_k\in {\mathbb{R}}^N$ 表示系统状态
• ${\mathbf{v}}_k\in {\mathbb{R}}^N$ 表示输入
• ${\mathbf{w}}_k\in {\mathbb{R}}^N$ 表示过程噪声，${\mathbf{w}}_k\sim N(0,{\mathbf{Q}}_k)$
• ${\mathbf{y}}_k\in {\mathbb{R}}^M$ 表示测量
• ${\mathbf{n}}_k\in {\mathbb{R}}^M$ 表示测量噪声，${\mathbf{n}}_k\sim N(0,{\mathbf{R}}_k)$
• $k$ 为时间下标，最大值为 $K$

用 $\check{(\cdot )}$ 表示先验，用 $\widehat{(\cdot )}$ 表示后验，卡尔曼滤波器则可以表示为：

预测

$${\check{\mathbf{x}}}_k={\mathbf{A}}_{k-1}{\hat{\mathbf{x}}}_{k-1}+{\mathbf{v}}_k$$

$${\check{\mathbf{P}}}_k={\mathbf{A}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_{k-1}^{\mathrm{T}}+{\mathbf{Q}}_k$$

卡尔曼增益

$${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}{\left({\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}$$

更新

$${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\underset{\text{更新量 }}{\underbrace{\left({\mathbf{y}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k\right)}}$$

$${\hat{\mathbf{P}}}_k=\left(1-{\mathbf{K}}_k{\mathbf{C}}_k\right){\check{\mathbf{P}}}_k$$

通过贝叶斯推断推导

预测

由于是线性高斯系统，因此直接将 $k-1$ 时刻的后验分布通过线性运动模型传递，即可得到 $k$ 时刻的先验：

$$\begin{array}{rl}{\check{\mathbf{x}}}_k& =E\left[{\mathbf{x}}_k\right]\\ & =E\left[{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k+{\mathbf{w}}_k\right]\\ & ={\mathbf{A}}_{k-1}\underset{{\mathbf{x}}_{k-1}}{\underbrace{E\left[{\mathbf{x}}_{k-1}\right]}}+{\mathbf{v}}_k+\underset{0}{\underbrace{E\left[{\mathbf{w}}_k\right]}}\\ & ={\mathbf{A}}_{k-1}{\hat{\mathbf{x}}}_{k-1}+{\mathbf{v}}_k\end{array}$$

$$\begin{array}{rl}{\check{\mathbf{P}}}_k=& E\left[\left({\mathbf{x}}_k-E\left[{\mathbf{x}}_k\right]\right){\left({\mathbf{x}}_k-E\left[{\mathbf{x}}_k\right]\right)}^{\mathrm{T}}\right]\\ =& E[\left({\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k+{\mathbf{w}}_k-{\mathbf{A}}_{k-1}{\hat{\mathbf{x}}}_{k-1}-{\mathbf{v}}_k\right)\\ & \times {\left({\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k+{\mathbf{w}}_k-{\mathbf{A}}_{k-1}{\hat{\mathbf{x}}}_{k-1}-{\mathbf{v}}_k\right)}^{\mathrm{T}}]\\ =& {\mathbf{A}}_{k-1}\underset{{\hat{\mathbf{P}}}_{k-1}}{\underbrace{E\left[\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right){\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)}^{\mathrm{T}}\right]}}{\mathbf{A}}_{k-1}^{\mathrm{T}}+\underset{{\mathbf{Q}}_k}{\underbrace{E\left[{\mathbf{w}}_k{\mathbf{w}}_k^{\mathrm{T}}\right]}}\\ =& {\mathbf{A}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_{k-1}^{\mathrm{T}}+{\mathbf{Q}}_k\end{array}$$

更新

对于更新部分，我们将状态与 $k$ 时刻的测量写成联合高斯分布的形式：

$$\begin{array}{rl}p\left({\mathbf{x}}_k,{\mathbf{y}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)& =\mathcal{N}\left(\left[\begin{array}{c}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xy}\\ {\mathbf{\Sigma }}_{yx}& {\mathbf{\Sigma }}_{yy}\end{array}\right]\right)\\ & =\mathcal{N}\left(\left[\begin{array}{c}{\check{\mathbf{x}}}_k\\ {\mathbf{C}}_k{\check{\mathbf{x}}}_k\end{array}\right],\left[\begin{array}{cc}{\check{\mathbf{P}}}_k& {\check{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}\\ {\mathbf{C}}_k{\check{\mathbf{P}}}_k& {\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k\end{array}\right]\right)\end{array}$$

由高斯推断可以直接得到 $k$ 时刻的条件分布（即后验概率）：

$$p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)=\mathcal{N}(\underset{{\hat{\mathbf{x}}}_k}{\underbrace{{\mathbf{\mu }}_x+{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\left({\mathbf{y}}_k-{\mathbf{\mu }}_y\right)}},\underset{{\hat{\mathbf{P}}}_k}{\underbrace{{\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}}})$$

令 ${\mathbf{K}}_k={\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}={\check{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}{\left({\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}$（卡尔曼增益），则有：

$${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k\right)$$

$${\hat{\mathbf{P}}}_k=\left(1-{\mathbf{K}}_k{\mathbf{C}}_k\right){\check{\mathbf{P}}}_k$$

即证。