卡尔曼滤波器

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

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

运动方程： x k = A k − 1 x k − 1 + v k + w k , k = 1 , ⋯   , K 观测方程： y k = C k x k + n k , k = 0 , ⋯   , K

​运动方程：xk​=Ak−1​xk−1​+vk​+wk​,k=1,⋯,K观测方程：yk​=Ck​xk​+nk​,k=0,⋯,K​

其中：

• ${\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$ 时刻的先验：

x ˇ k = E [ x k ] = E [ A k − 1 x k − 1 + v k + w k ] = A k − 1 E [ x k − 1 ] ⏟ x k − 1 + v k + E [ w k ] ⏟ 0 = A k − 1 x ^ k − 1 + v k

xˇk​​=E[xk​]=E[Ak−1​xk−1​+vk​+wk​]=Ak−1​xk−1​ E[xk−1​]​​+vk​+0 E[wk​]​​=Ak−1​x^k−1​+vk​​

P ˇ k = E [ ( x k − E [ x k ] ) ( x k − E [ x k ] ) T ] = E [ ( A k − 1 x k − 1 + v k + w k − A k − 1 x ^ k − 1 − v k ) × ( A k − 1 x k − 1 + v k + w k − A k − 1 x ^ k − 1 − v k ) T ] = A k − 1 E [ ( x k − 1 − x ^ k − 1 ) ( x k − 1 − x ^ k − 1 ) T ] ⏟ P ^ k − 1 A k − 1 T + E [ w k w k T ] ⏟ Q k = A k − 1 P ^ k − 1 A k − 1 T + Q k

Pˇk​====​E[(xk​−E[xk​])(xk​−E[xk​])T]E[(Ak−1​xk−1​+vk​+wk​−Ak−1​x^k−1​−vk​)×(Ak−1​xk−1​+vk​+wk​−Ak−1​x^k−1​−vk​)T]Ak−1​P^k−1​ E[(xk−1​−x^k−1​)(xk−1​−x^k−1​)T]​​Ak−1T​+Qk​ E[wk​wkT​]​​Ak−1​P^k−1​Ak−1T​+Qk​​

更新

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

p ( x k , y k ∣ x ˇ 0 , v 1 : k , y 0 : k − 1 ) = N ( [ μ x μ y ] , [ Σ x x Σ x y Σ y x Σ y y ] ) = N ( [ x ˇ k C k x ˇ k ] , [ P ˇ k P ˇ k C k T C k P ˇ k C k P ˇ k C k T + R k ] )

p(xk​,yk​∣xˇ0​,v1:k​,y0:k−1​)​=N([μx​μy​​],[Σxx​Σyx​​Σxy​Σyy​​])=N([xˇk​Ck​xˇk​​],[Pˇk​Ck​Pˇk​​Pˇk​CkT​Ck​Pˇk​CkT​+Rk​​])​

由高斯推断可以直接得到 $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$$

即证。