扩展卡尔曼滤波

扩展卡尔曼滤波就是贝叶斯滤波添加一些近似的实际应用：将置信度和噪声限制为高斯分布，并且对运动和观测模型进行线性化计算贝叶斯滤波中的积分(以及归一化积) 。
假设${\mathbf{x}}_k$ 的概率密度函数限制为高斯分布：
$$p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{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}{rl}{\mathbf{w}}_k& \sim \mathcal{N}(0,{\mathbf{Q}}_{\mathbf{k}})\\ {\mathbf{n}}_k& \sim \mathcal{N}(0,{\mathbf{R}}_{\mathbf{k}})\end{array}$$
高斯PDF通过非线性函数转换后，可能会成为非高斯的。

运动模型线性化：

$$\begin{array}{r}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_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 }\end{array}$$
其中
$$\begin{array}{rl}{\check{\mathbf{x}}}_k& =f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0\right)\\ {\mathbf{F}}_{k-1}& ={\frac{\partial f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\omega }_k\right)}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0}\\ {\mathbf{w}}_k^{\prime }& ={\frac{\partial f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\mathbf{w}}_k\right)}{\partial {\mathbf{w}}_k}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0}{\mathbf{w}}_k={\mathbf{L}}_k{\mathbf{w}}_k\end{array}\text{\hspace{1em}(state predict)}$$
给定过去的状态和最新输入，则当前状态${\mathbf{x}}_k$的统计学特性为：
$$\begin{array}{rl}& E\left({\mathbf{x}}_k\right)\approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\underset{E({\mathbf{x}}_{\mathbf{k}-1})-{\hat{\mathbf{x}}}_{k-1}=0}{\underbrace{E({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1})}}+\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)}^T\right]\approx \underset{{\mathbf{Q}}_k^{\prime }}{\underbrace{E\left[{\mathbf{w}}_k^{\prime }{\mathbf{w}}_k^{\prime T}\right]}}\\ & p\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{u}}_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)\end{array}$$

观测模型线性化：

$$\begin{array}{r}{\mathbf{y}}_k=\mathbf{h}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)={\check{\mathbf{y}}}_k+{\mathbf{H}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+{\mathbf{n}}_k^{\prime }\end{array}$$
其中
$$\begin{array}{rl}{\check{\mathbf{y}}}_k& =\mathbf{h}\left({\check{\mathbf{x}}}_k,0\right)\\ {\mathbf{H}}_k& ={\frac{\partial \mathbf{h}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k,0}\\ {\mathbf{n}}_k^{\prime }& ={\frac{\partial \mathbf{h}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{n}}_k}|}_{{\check{\mathbf{x}}}_k,0}{\mathbf{n}}_k\end{array}$$
给定当前状态，则当前状态观测${\mathbf{y}}_k$的统计学特性为：
$$\begin{array}{r}E\left({\mathbf{y}}_k\right)\approx {\check{\mathbf{y}}}_k+{\mathbf{H}}_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)}^T\right]\approx \underset{{\mathbf{R}}_k^{\prime }}{\underbrace{E\left[{\mathbf{n}}_k^{\prime }{{\mathbf{n}}^{\prime }}_k^T\right]}}\\ p\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)\approx \mathcal{N}\left({\check{\mathbf{y}}}_k+{\mathbf{H}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{R}}_k^{\prime }\right)\end{array}$$

代入贝叶斯滤波框架

由贝叶斯滤波，可知：
$$\begin{array}{rl}\stackrel{k时刻状态估计后验概率}{\overbrace{p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k}\right)}}& =\eta \stackrel{k时刻观测模型}{\overbrace{p\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)}}\stackrel{k时刻状态估计先验概率}{\overbrace{p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}}\\ \underset{\mathcal{N}\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)}{\underbrace{p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k}\right)}}& =\stackrel{更新步}{\overbrace{\eta \underset{\mathcal{N}\left({\mathbf{y}}_k+{\mathbf{H}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{R}}_k^{\prime }\right)}{\underbrace{p\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)}}\times \stackrel{预测步}{\overbrace{\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|{\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)}}\underset{\mathcal{N}\left({\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1}\right)}{\underbrace{p\left({\mathbf{x}}_{k-1}|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k-1},{\mathbf{y}}_{0:k-1}\right)}}\mathrm{d}{\mathbf{x}}_{k-1}}}}}\end{array}$$

由高斯分布的非线性变换可知：
$$\begin{array}{rl}\underset{\mathcal{N}({\mathbf{\mu }}_y,\mathbf{R}+\mathbf{G}{\mathbf{\Sigma }}_{xx}{\mathbf{G}}^T)}{\underbrace{p\left(\mathbf{y}\right)}}& ={\int }_{-\infty }^{+\infty }\underset{\mathcal{N}({\mathbf{\mu }}_y+\mathbf{G}\left(\mathbf{x}-{\mathbf{\mu }}_x\right),\mathbf{R})}{\underbrace{p\left(\mathbf{y}|\mathbf{x}\right)}}\underset{\mathcal{N}({\mathbf{\mu }}_x,{\mathbf{\Sigma }}_{xx})}{\underbrace{p\left(\mathbf{x}\right)}}\mathrm{d}\mathbf{x}\end{array}$$
令
$$\begin{array}{rl}\mathbf{x}\to {\mathbf{x}}_{k-1}& \mathbf{y}\to {\mathbf{x}}_k\\ {\mathbf{\mu }}_x\to {\hat{\mathbf{x}}}_{k-1}& {\mathbf{\mu }}_y\to {\check{\mathbf{x}}}_k\\ \mathbf{G}\to {\mathbf{F}}_{k-1}& \mathbf{R}\to {\mathbf{Q}}_k^{\prime }\\ {\mathbf{\Sigma }}_{xx}\to {\hat{\mathbf{P}}}_{k-1}& {\mathbf{\Sigma }}_{yy}\to {\hat{\mathbf{P}}}_k\end{array}$$
于是可以得到积分号内两个高斯分布的积分结果(即预测步得到的第k步先验估计)
$$\underset{{\mathbf{x}}_k\sim \mathcal{N}({\check{\mathbf{x}}}_k,{\mathbf{Q}}_k^{\prime }+{\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T)}{\underbrace{p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}}=\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|{\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)}}\underset{\mathcal{N}\left({\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1}\right)}{\underbrace{p\left({\mathbf{x}}_{k-1}|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k-1},{\mathbf{y}}_{0:k-1}\right)}}\mathrm{d}{\mathbf{x}}_{k-1}$$
$$\underset{{\mathbf{x}}_k\sim \mathcal{N}\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)}{\underbrace{p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k}\right)}}=\eta \underset{{\mathbf{y}}_k\sim \mathcal{N}\left({\check{\mathbf{y}}}_k+{\mathbf{H}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{R}}_k^{\prime }\right)}{\underbrace{p\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)}}\times \underset{{\mathbf{x}}_k\sim \mathcal{N}({\check{\mathbf{x}}}_k,{\mathbf{Q}}_k^{\prime }+{\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T)}{\underbrace{p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}}$$
先验协方差矩阵
$$\begin{array}{r}{\check{\mathbf{P}}}_k={\mathbf{Q}}_k^{\prime }+{\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_k{\mathbf{F}}_{k-1}^T\end{array}\text{\hspace{1em}(cov predict)}$$

将$p\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)$看做以${\mathbf{x}}_k$为变量的函数：
$$\begin{array}{rl}p\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)& =\rho \exp \left({\left({\mathbf{y}}_k-\left({\check{\mathbf{y}}}_k+{\mathbf{H}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)\right)\right)}^T{{\mathbf{R}}^{\prime }}_k^{-1}\left({\mathbf{y}}_k-\left({\check{\mathbf{y}}}_k+{\mathbf{H}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)\right)\right)\right)\\ & =\rho \exp \left({\left({\mathbf{H}}_k{\mathbf{x}}_k-\left({\mathbf{H}}_k{\check{\mathbf{x}}}_k+({\mathbf{y}}_k-{\check{\mathbf{y}}}_k)\right)\right)}^T{{\mathbf{R}}^{\prime }}_k^{-1}\left({\mathbf{H}}_k{\mathbf{x}}_k-\left({\mathbf{H}}_k{\check{\mathbf{x}}}_k+({\mathbf{y}}_k-{\check{\mathbf{y}}}_k)\right)\right)\right)\\ & =\rho \exp \left({\left({\mathbf{x}}_k-\left({\check{\mathbf{x}}}_k+{\mathbf{H}}_k^{-1}({\mathbf{y}}_k-{\check{\mathbf{y}}}_k)\right)\right)}^T{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}{\mathbf{H}}_k\left({\mathbf{x}}_k-\left({\check{\mathbf{x}}}_k+{\mathbf{H}}_k^{-1}({\mathbf{y}}_k-{\check{\mathbf{y}}}_k)\right)\right)\right)\end{array}$$
如果${\mathbf{H}}_k$可逆，则可以进行如下转换：
$$\underset{{\mathbf{x}}_k\sim \mathcal{N}\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)}{\underbrace{p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k}\right)}}=\eta \underset{{\mathbf{x}}_k\sim \mathcal{N}\left({\check{\mathbf{x}}}_k+{\mathbf{H}}_k^{-1}\left({\mathbf{y}}_k-{\check{\mathbf{y}}}_k\right),{\left({\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}{\mathbf{H}}_k\right)}^{-1}\right)}{\underbrace{p\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)}}\times \underset{{\mathbf{x}}_k\sim \mathcal{N}({\check{\mathbf{x}}}_k,{\mathbf{Q}}_k^{\prime }+{\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T)}{\underbrace{p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}}$$
但是${\mathbf{H}}_k$可能不可逆，更严谨的做法是根据高斯分布随机变量线性变换的归一化积公式，得

$$\begin{array}{rlr}{\hat{\mathbf{P}}}_k^{-1}& ={\check{\mathbf{P}}}_k^{-1}+{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}{\mathbf{H}}_k& (1)\\ {\hat{\mathbf{P}}}_k^{-1}{\hat{\mathbf{x}}}_k& ={\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k+{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}\left({\mathbf{H}}_k{\check{\mathbf{x}}}_k+\left({\mathbf{y}}_k-{\check{\mathbf{y}}}_k\right)\right)& (2)\end{array}$$
由矩阵求逆定理的(2)式${\left(\mathbf{D}+\mathbf{C}\mathbf{A}\mathbf{B}\right)}^{-1}\equiv {\mathbf{D}}^{-1}-{\mathbf{D}}^{-1}\mathbf{C}{\left({\mathbf{A}}^{-1}+\mathbf{B}{\mathbf{D}}^{-1}\mathbf{C}\right)}^{-1}\mathbf{B}{\mathbf{D}}^{-1}$
$$\begin{array}{r}\begin{array}{rl}{\hat{\mathbf{P}}}_k& ={\left({\check{\mathbf{P}}}_k^{-1}+{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}{\mathbf{H}}_k\right)}^{-1}\\ & ={\check{\mathbf{P}}}_k-{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{R}}_k^{\prime }+{\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T\right)}^{-1}{\mathbf{H}}_k{\check{\mathbf{P}}}_k\\ & =\left(\mathbf{I}-{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}^{\mathbf{T}}{\left({{\mathbf{R}}^{\prime }}_{\mathbf{k}}+{\mathbf{H}}_{\mathbf{k}}{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}^{\mathbf{T}}\right)}^{-1}{\mathbf{H}}_{\mathbf{k}}\right){\check{\mathbf{P}}}_k\end{array}\end{array}$$
归一化积(1)式代入(2)式化简得
$$\begin{array}{rl}\mathbf{I}& ={\hat{\mathbf{P}}}_k\left({\check{\mathbf{P}}}_k^{-1}+{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}{\mathbf{H}}_k\right)\\ {\hat{\mathbf{x}}}_k& ={\hat{\mathbf{P}}}_k\left({\check{\mathbf{P}}}_k^{-1}+{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}{\mathbf{H}}_k\right){\check{\mathbf{x}}}_k+{\hat{\mathbf{P}}}_k{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}\left({\mathbf{y}}_k-{\check{\mathbf{y}}}_k\right)\\ & ={\check{\mathbf{x}}}_k+{\hat{\mathbf{P}}}_k{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}\left({\mathbf{y}}_k-{\check{\mathbf{y}}}_k\right)\\ & ={\check{\mathbf{x}}}_k+{\left({\check{\mathbf{P}}}_k^{-1}+{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}{\mathbf{H}}_k\right)}^{-1}{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}\left({\mathbf{y}}_k-{\check{\mathbf{y}}}_k\right)\end{array}$$
由矩阵求逆定理的(4)式${\left(\mathbf{D}+\mathbf{C}\mathbf{A}\mathbf{B}\right)}^{-1}\mathbf{C}\mathbf{A}\equiv {\mathbf{D}}^{-1}\mathbf{C}{\left({\mathbf{A}}^{-1}+\mathbf{B}{\mathbf{D}}^{-1}\mathbf{C}\right)}^{-1}$
$${\left({\check{\mathbf{P}}}_k^{-1}+{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}{\mathbf{H}}_k\right)}^{-1}{\mathbf{H}}_k^T{{\mathbf{R}}^{\prime }}_k^{-1}={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{R}}_k^{\prime }+{\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T\right)}^{-1}$$
结合前面的预测方程(state predict)和(cov predict)

设卡尔曼增益为
$$\begin{array}{r}{\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{R}}_k^{\prime }+{\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T\right)}^{-1}\end{array}$$
则更新步：
$$\begin{array}{rl}{\hat{\mathbf{P}}}_k& =\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}\right)\underset{{\mathbf{Q}}_k^{\prime }+{\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_k{\mathbf{F}}_{k-1}^T}{\underbrace{{\check{\mathbf{P}}}_k}}\\ {\hat{\mathbf{x}}}_k& =\underset{\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0\right)}{\underbrace{{\check{\mathbf{x}}}_k}}+{\mathbf{K}}_k({\mathbf{y}}_k-\underset{\mathbf{h}\left({\check{\mathbf{x}}}_k,0\right)}{\underbrace{{\check{\mathbf{y}}}_k}})\end{array}$$

1. 通过非线性的运动和观测模型来传递估计的均值
2. 噪声协方差${\mathbf{Q}}_k^{\prime }$和${\mathbf{R}}_k^{\prime }$中包含了雅可比矩阵，这是因为我们允许噪声应用于非线性模型中。

EKF并不能保证在一般的非线性系统中能够充分的发挥作用。EKF的主要问题在于，其线性化的工作点是估计状态的均值，而不是真实状态。

广义高斯滤波

一般来说，我们先从$k-1$时刻的高斯后验开始：
$$p\left({\mathbf{x}}_{k-1}|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k-1},{\mathbf{y}}_{0:k-1}\right)=\mathcal{N}\left({\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1}\right)$$

通过非线性运动模型$\mathbf{f}(\cdot )$在时间上向前递推，以得到在$k$时刻的高斯先验：
$$p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)=\mathcal{N}\left({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k\right)$$
这是预测步骤，结合了最新的输入${\mathbf{u}}_k$。

对于校正步骤，我们采用联合高斯概率密度函数，分解与推断的方法，写出$k$时刻状态和最新测量的联合高斯分布：
$$\begin{array}{r}p\left({\mathbf{x}}_k,{\mathbf{y}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)=\mathcal{N}\left(\left[\begin{array}{c}{\mathbf{\mu }}_{x,k}\\ {\mathbf{\mu }}_{y,k}\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx,k}& {\mathbf{\Sigma }}_{xy,k}\\ {\mathbf{\Sigma }}_{yx,k}& {\mathbf{\Sigma }}_{yy,k}\end{array}\right]\right)\end{array}$$

则${\mathbf{x}}_k$的条件概率密度函数：
$$\begin{array}{r}p\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k}\right)=\mathcal{N}(\underset{{\hat{\mathbf{x}}}_k}{\underbrace{{\mathbf{\mu }}_{x,k}+{\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}\left({\mathbf{y}}_k-{\mathbf{\mu }}_{y,k}\right)}},\underset{\widehat{{\mathbf{P}}_k}}{\underbrace{{\mathbf{\Sigma }}_{xx,k}-{\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}{\mathbf{\Sigma }}_{yx,k}}})\end{array}$$
将${\hat{\mathbf{x}}}_k$定义为均值，${\hat{\mathbf{P}}}_k$定义为协方差，可以写出广义高斯滤波中校正步骤的方程：
$$\begin{array}{rl}{\mathbf{K}}_k& ={\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}\\ {\hat{\mathbf{P}}}_k& ={\check{\mathbf{P}}}_k-{\mathbf{K}}_k{\mathbf{\Sigma }}_{xy,k}^T\\ {\hat{\mathbf{x}}}_k& ={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-{\mathbf{\mu }}_{y,k}\right)\end{array}\text{\hspace{1em}(GGF update)}$$

其中
$$\begin{array}{rl}{\mathbf{\mu }}_{x,k}& =E({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1})={\check{\mathbf{x}}}_k\\ {\mathbf{\Sigma }}_{xx,k}& =E\left(({\mathbf{x}}_k-{\mathbf{\mu }}_{x,k})({\mathbf{x}}_k-{\mathbf{\mu }}_{x,k}{)}^T|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)={\check{\mathbf{P}}}_k\end{array}$$

对非线性观测模型在任意一点${\mathbf{x}}_{op,k}$进行线性化，可得：
$$\begin{array}{r}{\mathbf{y}}_k=\mathbf{h}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)={\mathbf{y}}_{op,k}+{\mathbf{H}}_k\left({\mathbf{x}}_k-{\mathbf{x}}_{op,k}\right)+{\mathbf{n}}_k^{\prime }\end{array}$$
其中
$$\begin{array}{rl}{\mathbf{y}}_{op,k}& =\mathbf{h}\left({\mathbf{x}}_{op,k},0\right)\\ {\mathbf{H}}_k& ={\frac{\partial \mathbf{h}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{x}}_k}|}_{{\mathbf{x}}_{op,k},0}\\ {\mathbf{n}}_k^{\prime }& ={\frac{\partial \mathbf{h}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{n}}_k}|}_{{\mathbf{x}}_{op,k},0}{\mathbf{n}}_k\end{array}$$
则
$$\begin{array}{rl}& \begin{array}{rl}{\mathbf{\mu }}_{y,k}& =E({\mathbf{y}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1})\\ & =\underset{\mathrm{C}\mathrm{o}\mathrm{n}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{t}}{\underbrace{E\left({\mathbf{y}}_{op,k}-{\mathbf{H}}_k{\mathbf{x}}_{op,k}|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}}+\underset{{\mathbf{H}}_{k}E\left({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}{\underbrace{E\left({\mathbf{H}}_k{\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}}+\underset{0}{\underbrace{E\left({\mathbf{n}}_k^{\prime }\right)}}\\ & ={\mathbf{y}}_{op,k}-{\mathbf{H}}_k{\mathbf{x}}_{op,k}+{\mathbf{H}}_k{\check{\mathbf{x}}}_k\\ & ={\mathbf{y}}_{op,k}+{\mathbf{H}}_k\left({\check{\mathbf{x}}}_k-{\mathbf{x}}_{op,k}\right)\end{array}\\ & \begin{array}{rl}{\mathbf{\Sigma }}_{yy,k}& =E\left(\left[{\mathbf{y}}_k-{\mathbf{\mu }}_{y,k}\right]\cdot {\left[{\mathbf{y}}_k-{\mathbf{\mu }}_{y,k}\right]}^T|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\\ & =E\left(\underset{{\mathbf{H}}_{k}E\left[\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right){\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)}^T\right]{\mathbf{H}}_k^T}{\underbrace{{\mathbf{H}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right){\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)}^T{\mathbf{H}}_k^T}}+\underset{E({\mathbf{n}}_k^{\prime }{\mathbf{n}}_k^{\prime T})}{\underbrace{{\mathbf{n}}_k^{\prime }{\mathbf{n}}_k^{\prime T}}}+\underset{E({\mathbf{n}}_k^{\prime })E(\cdot )=0}{\underbrace{{\mathbf{n}}_k^{\prime }{\left[{\mathbf{H}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)\right]}^T}}+\underset{E(\cdot )E({\mathbf{n}}_k^{\prime })=0}{\underbrace{{\mathbf{H}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right){\mathbf{n}}_k^{\prime }}}|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\\ & ={\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k^{\prime }\end{array}\\ & \begin{array}{rl}{\mathbf{\Sigma }}_{xy}& =E\left[\left({\mathbf{x}}_k-{\mathbf{\mu }}_{x,k}\right){\left({\mathbf{y}}_k-{\mathbf{\mu }}_{y,k}\right)}^T|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right]\\ & =E\left[\left({\mathbf{x}}_k-{\mathbf{\mu }}_{x,k}\right){\left({\mathbf{H}}_k\left({\mathbf{x}}_k-{\mathbf{\mu }}_{x,k}\right)\right)}^T|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right]\\ & =E\left[\left({\mathbf{x}}_k-{\mathbf{\mu }}_{x,k}\right){\left({\mathbf{x}}_k-{\mathbf{\mu }}_{x,k}\right)}^T|{\check{\mathbf{x}}}_0,{\mathbf{u}}_{1:k},{\mathbf{y}}_{0:k-1}\right]{\mathbf{H}}_k^T\\ & ={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T\end{array}\\ & {\mathbf{\Sigma }}_{yx}={\mathbf{\Sigma }}_{xy}^T={\mathbf{H}}_k{\check{\mathbf{P}}}_k\end{array}$$

将上面四式 $\Uparrow$ 带入GGF update式得：
$$\begin{array}{rl}{\mathbf{K}}_k& ={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{R}}_k^{\prime }+{\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T\right)}^{-1}\\ {\hat{\mathbf{P}}}_k& =\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{k}}{\mathbf{G}}_{\mathbf{k}}\right){\check{\mathbf{P}}}_k\\ {\hat{\mathbf{x}}}_k& ={\check{\mathbf{x}}}_k-{\mathbf{K}}_k\left({\mathbf{y}}_k-{\mathbf{y}}_{op,k}-{\mathbf{G}}_k\left({\check{x}}_k-{\mathbf{x}}_{op,k}\right)\right)\end{array}\text{\hspace{1em}(iterate update)}$$
上式与扩展卡尔曼滤波的增益和校正方程非常相似，唯一的区别在于线性化的工作点。如果我们将线性化的工作点设置为预测先验的均值，即${\mathbf{x}}_{op,k}={\check{\mathbf{x}}}_k$，那么两者完全相同。

如果我们迭代计算iterate update式，并且在每一次迭代中将工作点设置为上一次迭代的后验均值，将得到更好的结果：
$${\hat{\mathbf{x}}}_k\to {\mathbf{x}}_{op,k}$$
在第一次迭代中，令${\mathbf{x}}_{op,k}={\check{\mathbf{x}}}_k$。在迭代的过程中，若${\mathbf{x}}_{op,k}$的改变足够小就停止迭代。注意，在卡尔曼增益和后验状态收敛之后，协方差方程只需计算一次。

EKF、IEKF与MAP的关系

IEKF的结果对应于后验概率的(局部)极大值。由于EKF的校正部分并没有迭代，它可能远离局部极大值。
IEKF的均值对应于MAP的模(最大值)。