扩展卡尔曼滤波EKF

如果将置信度和噪声限制为高斯分布，并且对运动模型和观测进行线性化，计算贝叶斯滤波中的积分（以及归一化积），即可得到扩展卡尔曼滤波（EKF）。

为了推导EKF，首先假设 ${\mathbf{x}}_k$ 的置信度函数限制为高斯分布：

$$p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)=\mathcal{N}\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)$$

其中，${\hat{\mathbf{x}}}_k$ 为均值，${\hat{\mathbf{P}}}_k$ 为协方差。

并且假设噪声变量 ${\mathbf{w}}_k$ 和 ${\mathbf{n}}_k$ 也是高斯分布的：

$$\begin{array}{l}{\mathbf{w}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_k\right)\\ {\mathbf{n}}_k\sim \mathcal{N}\left(0,{\mathbf{R}}_k\right)\end{array}$$

对于以下运动和观测模型：

$$\begin{array}{rl}& {\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)\\ & {\mathbf{y}}_k=g\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)\end{array}$$

由于 $\mathbf{f}(\cdot )$ 和 $\mathbf{g}(\cdot )$ 是非线性函数，所以我们需要对其进行线性化。在当前状态的均值处展开，对运动和观测模型进行线性化：

$$\mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)\approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+{\mathbf{w}}_k^{\prime }$$

$$\mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)\approx {\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+{\mathbf{n}}_k^{\prime }$$

其中：

• ${\check{\mathbf{x}}}_k=\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0\right)$
• ${\mathbf{F}}_{k-1}={\frac{\partial \mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0}$
• ${\mathbf{w}}_k^{\prime }={\frac{\partial \mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)}{\partial {\mathbf{w}}_k}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0}{\mathbf{w}}_k$
• ${\check{\mathbf{y}}}_k=\mathbf{g}\left({\check{\mathbf{x}}}_k,0\right)$
• ${\mathbf{G}}_k={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k,0}$
• ${\mathbf{n}}_k^{\prime }={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{n}}_k}|}_{{\check{\mathbf{x}}}_k,0}{\mathbf{n}}_k$

给定过去的状态和最新输入，则当前状态 ${\mathbf{x}}_k$ 的统计学特性为：

$${\mathbf{x}}_k\approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+{\mathbf{w}}_k^{\prime }$$

$$E\left[{\mathbf{x}}_k\right]\approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+\underset{0}{\underbrace{E\left[{\mathbf{w}}_k^{\prime }\right]}}$$

$$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]\approx \underset{{\mathbf{Q}}_k^{\prime }}{\underbrace{E[{\mathbf{w}}_k^{\prime }{\mathbf{w}}_k^{{\prime }^{\mathrm{T}}]}}}$$

$$p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)\approx \mathcal{N}\left({\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right),{\mathbf{Q}}_k^{\prime }\right)$$

给定当前状态，则当前观测 ${\check{\mathbf{y}}}_k$ 的统计学特性为：

$${\mathbf{y}}_k\approx {\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+{\mathbf{n}}_k^{\prime }$$

$$E\left[{\mathbf{y}}_k\right]\approx {\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+\underset{0}{\underbrace{E\left[{\mathbf{n}}_k^{\prime }\right]}}$$

$$E\left[\left({\mathbf{y}}_k-E\left[{\mathbf{y}}_k\right]\right){\left({\mathbf{y}}_k-E\left[{\mathbf{y}}_k\right]\right)}^{\mathrm{T}}\right]\approx \underset{{\mathbf{R}}_k^{\prime }}{\underbrace{E\left[{\mathbf{n}}_k^{\prime }{\mathbf{n}}_k^{\prime \mathrm{T}}\right]}}$$

$$p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)\approx \mathcal{N}\left({\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{R}}_k^{\prime }\right)$$

由贝叶斯滤波器有：

$$\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}$$

将线性化之后的运动和观测模型代入到贝叶斯滤波器中，有：

$$\begin{array}{l}\underset{\mathcal{N}\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)}{\underbrace{p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)}}=\eta \underset{\mathcal{N}\left({\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{R}}_k^{\prime }\right)}{\underbrace{p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)}}\\ \times \int \underset{\mathcal{N}\left({\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right),{\mathbf{Q}}_k^{\prime }\right)}{\underbrace{p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)}}\underset{\mathcal{N}\left({\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1}\right)}{\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}$$

将服从高斯分布的变量传入到非线性函数中，积分之后仍然服从高斯分布：

$$\begin{array}{l}\underset{\mathcal{N}\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)}{\underbrace{p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)}}=\eta \underset{\mathcal{N}\left({\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{R}}_k^{\prime }\right)}{\underbrace{p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)}}\\ \times \underset{\mathcal{N}\left({\check{\mathbf{x}}}_k,{\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^{\mathrm{T}}+{\mathbf{Q}}_k^{\prime }\right)}{\underbrace{\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}$$

再利用高斯概率密度函数的归一化积的性质，有：

$$\begin{array}{rl}\underset{\mathcal{N}\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)}{\underbrace{p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)}}=& \underset{\mathcal{N}\left({\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-{\check{\mathbf{y}}}_k\right),\left(1-{\mathbf{K}}_k{\mathbf{G}}_k\right)\left({\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^{\mathrm{T}}+{\mathbf{Q}}_k^{\prime }\right)\right)}{\underbrace{\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}$$

其中，${\mathbf{K}}_k$ 为卡尔曼增益。比较上面式子的左右两侧，有：

预测

$${\check{\mathbf{x}}}_k=\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0\right)$$

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

卡尔曼增益

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

更新

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

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