滤波估计理论（三）——扩展卡尔曼滤波器（Extended Kalman Filter）

在经典Kalman滤波的推导中，我们假设系统的动态模型和量测模型皆是线性的，因而可以基于Gaussian分布的线性化性质，直接推导出线性变换后的分布。但在实际生活中，线性模型是一个强假设，大多数的系统并不是线性的，其动态模型和量测模型通常通过一个非线性函数进行映射：
$$\begin{array}{rl}{\mathbf{x}}_k& =f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_{k-1}\right)+{\mathbf{q}}_{k-1}\\ {\mathbf{z}}_k& =h\left({\mathbf{x}}_k\right)+{\mathbf{r}}_k\end{array}$$

由于Gaussian分布并没有任何的分线性映射性质，如何求解上述模型成为了一个难题。自然而然地，如果我们能有某些线性映射的组合去近似表示该非线性映射，那么这一难题就可以得到解决。

矢量函数的泰勒级数

我们知道任何一元函数都可以表示为无限连加的形式，即泰勒级数（Taylor Series），而对于矢量函数同样如此，这里直接给出矢量函数泰勒级数展开定义：

定理1：对于多元函数$\mathbf{y}=\mathbf{g}(\mathbf{x}),x\in {\mathbb{R}}^n,y\in {\mathbb{R}}^m$，令$\mathbf{x}=\mathbf{\mu }+\delta \mathbf{x}$，则$\mathbf{g}(\mathbf{x})$其在$\mathbf{\mu }$点处的泰勒级数为：
$$\mathbf{g}(\mathbf{x})=\mathbf{g}(\mathbf{\mu }+\delta \mathbf{x})\approx \mathbf{g}(\mathbf{\mu })+\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}+\frac{1}{2}\sum _{i=1}^m[\delta {\mathbf{x}}^T{\mathbf{H}}_i(\mathbf{\mu })\delta \mathbf{x}]{\mathbf{u}}_i+\cdots \text{\hspace{1em}(1)}$$

式中${\mathbf{u}}_i=[0,\cdots ,1,\cdots ,0{]}^T$为第$i$行为$1$的单位矢量；$\mathbf{J}(\mathbf{\mu })$表示$\mathbf{g}(\mathbf{x})$对变量$\mathbf{x}$的Jacobian矩阵，对应位置处元素为：
$$[\mathbf{J}(\mathbf{\mu }){]}_{j,j^{\prime }}={\frac{\partial g_j(\mathbf{x})}{\partial x_{j^{\prime }}}|}_{\mathbf{x}=\mathbf{\mu }}$$

式中$g_j(\mathbf{x})$为矢量函数$\mathbf{g}(x)=[g_1(\mathbf{x}),\cdots ,g_m(\mathbf{x}){]}^T$中第$j$行所表示的实值函数；$x_{j^{\prime }}$为变量$\mathbf{x}=[x_1,\cdots ,x_n{]}^T$中第$j^{\prime }$行对应的元素。

${\mathbf{H}}_i(\mathbf{\mu })$表示对变量$\mathbf{x}$的Hessian矩阵，对应位置处元素为：
$$[{\mathbf{H}}_i(\mathbf{\mu }){]}_{j,j^{\prime }}={\frac{{\partial }^2{g}_i(\mathbf{x})}{\partial x_j\partial x_{j^{\prime }}}|}_{\mathbf{x}=\mathbf{\mu }}$$

式中$g_i(\mathbf{x})$、$x_j$和$x_{j^{\prime }}$的含义参照Jacobian矩阵。

注：这里需要注意的是与Jacobian矩阵不同，Hessian要求被求导的函数必须是一个实值函数，因此这里我们说${\mathbf{H}}_i(\mathbf{\mu })$而非$\mathbf{H}(\mathbf{\mu })$是Hessian矩阵。而Jacobian矩阵的被求导函数既可以为矢量函数也可以为实值函数，当Jacobian矩阵的求导函数为实值函数时，Jacobian矩阵将退化为梯度矩阵。

高斯分布非线性变换的线性近似

假设现在有一个高斯随机变量$\mathbf{x}\sim N(\mathbf{\mu },\mathbf{P})$和对应的非线性变换$\mathbf{y}=\mathbf{g}(\mathbf{x})$，利用式（1）对$\mathbf{g}(\mathbf{x})$进行泰勒展开并取其前可获得其对应的线性近似如下：
$$\mathbf{g}(\mathbf{x})\approx \mathbf{g}(\mathbf{\mu })+\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}$$

进而可以获得线性近似后的分布如下：
$$\begin{array}{rl}E[\mathbf{g}(\mathbf{x})]& \approx E[\mathbf{g}(\mathbf{\mu })+\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}]\\ & =\mathbf{g}(\mathbf{\mu })+\mathbf{J}(\mathbf{\mu })E[\delta \mathbf{x}]\\ & =\mathbf{g}(\mathbf{\mu })\\ Cov[\mathbf{g}(\mathbf{x})]& \approx E[(\mathbf{g}(\mathbf{x})-E[\mathbf{g}(\mathbf{x})])(\mathbf{g}(\mathbf{x})-E[\mathbf{g}(\mathbf{x})]{)}^T]\\ & \approx E[(\mathbf{g}(\mathbf{x})-\mathbf{g}(\mathbf{\mu }))(\mathbf{g}(\mathbf{x})-\mathbf{g}(\mathbf{\mu }){)}^T]\\ & =E[(\mathbf{g}(\mathbf{\mu })+\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}-\mathbf{g}(\mathbf{\mu }))(\mathbf{g}(\mathbf{\mu })+\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}-\mathbf{g}(\mathbf{\mu }){)}^T]\\ & =E[(\mathbf{J}(\mathbf{\mu })\delta \mathbf{x})(\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}{)}^T]\\ & =\mathbf{J}(\mathbf{\mu })E[\delta \mathbf{x}\delta {\mathbf{x}}^T]{\mathbf{J}}^T(\mathbf{\mu })\\ & =\mathbf{J}(\mathbf{\mu })\mathbf{P}{\mathbf{J}}^T(\mathbf{\mu })\end{array}\text{\hspace{1em}(2)}$$

记$\tilde{\mathbf{g}}(\mathbf{x})=[\begin{array}{cc}\mathbf{x}& \mathbf{g}(\mathbf{x})\end{array}{]}^T$，进而我们可以得到$\mathbf{x}$和$\mathbf{g}(\mathbf{x})$的联合分布为：
$$\begin{array}{rl}E[\tilde{\mathbf{g}}(\mathbf{\mu })]& \approx \left[\begin{array}{c}\mathbf{\mu }\\ \mathbf{g}(\mathbf{\mu })\end{array}\right]\\ Cov[\tilde{\mathbf{g}}(\mathbf{\mu })]& \approx \tilde{\mathbf{J}}(\mathbf{\mu })\mathbf{P}\widetilde{{\mathbf{J}}^T}(\mathbf{\mu })\\ & =\left[\begin{array}{c}\mathbf{I}\\ \mathbf{J}(\mathbf{\mu })\end{array}\right]\mathbf{P}{\left[\begin{array}{c}\mathbf{I}\\ \mathbf{J}(\mathbf{\mu })\end{array}\right]}^T\\ & =\left[\begin{array}{cc}\mathbf{P}& \mathbf{P}{\mathbf{J}}^T(\mathbf{\mu })\\ \mathbf{J}(\mathbf{\mu })\mathbf{P}& \mathbf{J}(\mathbf{\mu })\mathbf{P}{\mathbf{J}}^T(\mathbf{\mu })\end{array}\right]\end{array}\text{\hspace{1em}(3)}$$
由于$\mathbf{g}(\mathbf{x})$是由$\mathbf{x}$变换而来，其分布相当于条件分布$p(\mathbf{g}(\mathbf{x})|\mathbf{x})$，式（3）实际上和我们在Kalman滤波推导中提到过的由条件分布求解联合分布的公式是完全一致的。

一阶近似线性近似EKF

加性噪声模型

在状态空间模型中，很多时候我们会假设控制变量${\mathbf{u}}_k$为零，而只考虑系统本身的状态变量${\mathbf{x}}_k$和噪声${\mathbf{q}}_k$；又或者如量测方程一般，本身就并不包含控制变量。此时无论是预测方程还是量测方程都为$\mathbf{y}=\mathbf{g}(\mathbf{x})+\mathbf{q}$形式，由于噪声变量$\mathbf{q}$是附加在原状态变量上的，这一类模型统称为加性噪声模型。

在加性噪声模型下我们有：
$$\begin{array}{rl}\mathbf{y}& =\mathbf{g}(\mathbf{x})+\mathbf{q}\\ \mathbf{x}& \sim N(\mathbf{\mu },\mathbf{P})\\ \mathbf{q}& \sim M(0,\mathbf{Q})\end{array}$$

对联合分布$\tilde{\mathbf{g}}(\mathbf{x})=[\begin{array}{cc}\mathbf{x}& \mathbf{g}(\mathbf{x})\end{array}{]}^T$取一阶泰勒近似有：
$$\tilde{\mathbf{g}}(\mathbf{x})\approx \left[\begin{array}{c}\mathbf{\mu }+\mathbf{I}\delta \mathbf{x}\\ \mathbf{g}(\mathbf{\mu })+\mathbf{q}+\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}\end{array}\right]$$

对其求取均值，其中已知$\mathbf{q}$和所有$\delta \mathbf{x}$项均值均为$0$，有：
$$\begin{array}{rl}E[\tilde{\mathbf{g}}(\mathbf{x})]& \approx \left[\begin{array}{c}\mathbf{\mu }+E[\mathbf{I}\delta \mathbf{x}]\\ \mathbf{g}(\mathbf{\mu })+E[\mathbf{q}]+E[\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}]\end{array}\right]\\ & =\left[\begin{array}{c}\mathbf{\mu }\\ \mathbf{g}(\mathbf{\mu })\end{array}\right]\end{array}$$

将上述两式同时带入方差的定义中有：
$$\begin{array}{rl}Cov[\tilde{\mathbf{g}}(\mathbf{x})]& \approx E[\left(\tilde{\mathbf{g}}(\mathbf{x})-E[\tilde{\mathbf{g}}(\mathbf{x})]\right){\left(\tilde{\mathbf{g}}(\mathbf{x})-E[\tilde{\mathbf{g}}(\mathbf{x})]\right)}^T]\\ & =E\left[\left[\begin{array}{c}\mathbf{\mu }+\mathbf{I}\delta \mathbf{x}-\mathbf{\mu }\\ \mathbf{g}(\mathbf{\mu })+\mathbf{q}+\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}-\mathbf{g}(\mathbf{\mu })\end{array}\right]{\left[\begin{array}{c}\mathbf{\mu }+\mathbf{I}\delta \mathbf{x}-\mathbf{\mu }\\ \mathbf{g}(\mathbf{\mu })+\mathbf{q}+\mathbf{J}(\mathbf{\mu })\delta \mathbf{x}-\mathbf{g}(\mathbf{\mu })\end{array}\right]}^T\right]\\ & =\left[\begin{array}{cc}E[\delta \mathbf{x}\delta {\mathbf{x}}^T]& E[\delta \mathbf{x}\delta {\mathbf{x}}^T]{\mathbf{J}}^T(\mathbf{\mu })\\ \mathbf{J}(\mathbf{\mu })E[\delta \mathbf{x}\delta {\mathbf{x}}^T]& \mathbf{J}(\mathbf{\mu })E[\delta \mathbf{x}\delta {\mathbf{x}}^T]{\mathbf{J}}^T(\mathbf{\mu })+E[\delta \mathbf{q}\delta {\mathbf{q}}^T]\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{P}& \mathbf{P}{\mathbf{J}}^T(\mathbf{\mu })\\ \mathbf{J}(\mathbf{\mu })\mathbf{P}& \mathbf{J}(\mathbf{\mu })\mathbf{P}{\mathbf{J}}^T(\mathbf{\mu })+\mathbf{Q}\end{array}\right]\end{array}$$

现在再回到我们的状态估计问题中来，在加性噪声模型下，待估计系统的状态空间模型为：
$$\{\begin{array}{rl}{\mathbf{x}}_k& =\mathbf{f}({\mathbf{x}}_{k-1})+{\mathbf{q}}_{k-1}\\ {\mathbf{z}}_k& =\mathbf{h}({\mathbf{x}}_k)+{\mathbf{r}}_k\end{array}$$

类比Kalman滤波的推导过程，假设我们有了$k$时刻的先验分布$p({\mathbf{x}}_{k-1}|{\mathbf{z}}_{1:k-1})\sim N({\mathbf{\mu }}_{k-1},{\mathbf{P}}_{k-1})$和过程噪声$p({\mathbf{q}}_{k-1})\sim N(0,{\mathbf{Q}}_{k-1})$，那么根据状态转移方程我们很容易得到联合分布$p({\mathbf{x}}_k,{\mathbf{x}}_{k-1}|{\mathbf{z}}_{1:k-1})$为：
$$p({\mathbf{x}}_k,{\mathbf{x}}_{k-1}|{\mathbf{z}}_{1:k-1})\sim N(\mathbf{f}({\mathbf{\mu }}_{k-1})，\mathbf{F}({\mathbf{\mu }}_{k-1}){\mathbf{P}}_{k-1}{\mathbf{F}}^T({\mathbf{\mu }}_{k-1}))+{\mathbf{Q}}_{k-1})$$

式中，$\mathbf{F}(\mathbf{\mu })$为对应的Jacobian矩阵。同样我们简记$p({\mathbf{x}}_k,{\mathbf{x}}_{k-1}|{\mathbf{z}}_{1:k-1})\sim N({\hat{\mathbf{\mu }}}_k,{\hat{\mathbf{P}}}_k)$，根据量测方程我们可以写出条件概率$p({\mathbf{z}}_k|{\mathbf{x}}_k)\sim N({\mathbf{H}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)$，进而得到联合概率$p({\mathbf{x}}_k,{\mathbf{z}}_k|{\mathbf{z}}_{1:k-1})$为：
$$p({\mathbf{x}}_k,{\mathbf{z}}_k|{\mathbf{z}}_{1:k-1})\sim N(\left[\begin{array}{c}{\hat{\mathbf{\mu }}}_k\\ \mathbf{h}({\hat{\mathbf{\mu }}}_k)\end{array}\right],\left[\begin{array}{cc}{\hat{\mathbf{P}}}_k& {\hat{\mathbf{P}}}_k{\mathbf{H}}^T({\hat{\mathbf{\mu }}}_k)\\ \mathbf{H}({\hat{\mathbf{\mu }}}_k){\hat{\mathbf{P}}}_k& \mathbf{H}({\hat{\mathbf{\mu }}}_k){\hat{\mathbf{P}}}_k{\mathbf{H}}^T({\hat{\mathbf{\mu }}}_k)+{\mathbf{R}}_k\end{array}\right])$$

进而我们可以得到条件概率$p({\mathbf{x}}_k,{\mathbf{z}}_k|{\mathbf{z}}_{1:k-1})$为：
$$\begin{array}{rl}& p({\mathbf{x}}_k|{\mathbf{z}}_k,{\mathbf{z}}_{1:k-1})\sim N({\tilde{\mathbf{\mu }}}_k,{\tilde{\mathbf{P}}}_k)\\ & {\tilde{\mathbf{\mu }}}_k={\hat{\mathbf{\mu }}}_k+{\hat{\mathbf{P}}}_k{\mathbf{H}}^T({\hat{\mathbf{\mu }}}_k)[\mathbf{H}({\hat{\mathbf{\mu }}}_k){\hat{\mathbf{P}}}_k{\mathbf{H}}^T({\hat{\mathbf{\mu }}}_k)+{\mathbf{R}}_k{]}^{-1}({\mathbf{z}}_k-\mathbf{h}({\hat{\mathbf{\mu }}}_k)\\ & {\tilde{\mathbf{P}}}_k={\hat{\mathbf{P}}}_k-{\hat{\mathbf{P}}}_k{\mathbf{H}}^T({\hat{\mathbf{\mu }}}_k)[\mathbf{H}({\hat{\mathbf{\mu }}}_k){\hat{\mathbf{P}}}_k{\mathbf{H}}^T({\hat{\mathbf{\mu }}}_k)+{\mathbf{R}}_k{]}^{-1}\mathbf{H}({\hat{\mathbf{\mu }}}_k){\hat{\mathbf{P}}}_k\end{array}$$

至此我们就得到了加性噪声模型下的一阶近似EKF算法，整理如下：
$$一阶近似EKF（加性）\{\begin{array}{rl}一步预测& \{\begin{array}{rl}{\hat{\mathbf{x}}}_k& =\mathbf{f}({\mathbf{\mu }}_{k-1})\\ {\hat{\mathbf{P}}}_k& =\mathbf{F}({\mathbf{\mu }}_{k-1}){\mathbf{P}}_{k-1}{\mathbf{F}}^T({\mathbf{\mu }}_{k-1}))+{\mathbf{Q}}_{k-1}\end{array}\\ 量测更新& \{\begin{array}{rl}\mathbf{K}& ={\hat{\mathbf{P}}}_k{\mathbf{H}}^T({\hat{\mathbf{\mu }}}_k)[\mathbf{H}({\hat{\mathbf{\mu }}}_k){\hat{\mathbf{P}}}_k{\mathbf{H}}^T({\hat{\mathbf{\mu }}}_k)+{\mathbf{R}}_k{]}^{-1}\\ {\tilde{\mathbf{x}}}_k& ={\hat{\mathbf{\mu }}}_k+\mathbf{K}({\mathbf{z}}_k-\mathbf{h}({\hat{\mathbf{\mu }}}_k))\\ {\tilde{\mathbf{P}}}_k& ={\hat{\mathbf{P}}}_k-\mathbf{K}\mathbf{H}({\hat{\mathbf{\mu }}}_k){\hat{\mathbf{P}}}_k\end{array}\end{array}$$

非加性噪声模型

而如果在状态空间模型中控制变量${\mathbf{u}}_k$不为零，此时预测方程会变为$\mathbf{y}=\mathbf{g}(\mathbf{x},\mathbf{q})$形式。此时噪声变量$\mathbf{q}$是和状态变量$\mathbf{x}$同为$\mathbf{g}(\mathbf{x})$的自变量，两者显然并不具有可加性，因此这一类模型统称为非加性噪声模型。

对于非加性噪声模型来说，其泰勒级数在式（1）的基础上需要进行一定改动，这里直接给出结论：
定理2：假设$\mathbf{g}(\mathbf{x},\mathbf{u})\in {\mathbb{R}}^n$为关于自变量为$\mathbf{x}\in {\mathbb{R}}^n$和$\mathbf{u}\in {\mathbb{R}}^m$的矢量函数，则$\mathbf{g}(\mathbf{x},\mathbf{u})$在点$({\mathbf{x}}_0,{\mathbf{u}}_0)$处的泰勒级数为：
$$\mathbf{g}(\mathbf{x},\mathbf{u})=\mathbf{g}({\mathbf{x}}_0,{\mathbf{u}}_0)+{\mathbf{J}}_{\mathbf{x}}({\mathbf{x}}_0,{\mathbf{u}}_0)\delta \mathbf{x}+{\mathbf{J}}_{\mathbf{u}}({\mathbf{x}}_0,{\mathbf{u}}_0)\delta \mathbf{u}+\cdots$$

那么，对于非线性变换$\mathbf{y}=\mathbf{g}(\mathbf{x},\mathbf{q})$，$\mathbf{x}\sim N(\mathbf{\mu },\mathbf{P})$，$\mathbf{q}\sim N(0,\mathbf{Q})$，联合分布$\tilde{\mathbf{g}}(\mathbf{x})=[\begin{array}{cc}\mathbf{x}& \mathbf{g}(\mathbf{x},\mathbf{q})\end{array}{]}^T$，可以求得均值为：
$$\begin{array}{rl}E[\tilde{\mathbf{g}}(\mathbf{x})]& =\left[\begin{array}{c}E[\mathbf{x}]\\ E[\mathbf{g}(\mathbf{\mu },0)+{\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu },0)\delta \mathbf{x}+{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu },0)\delta \mathbf{q}]\end{array}\right]\\ & =\left[\begin{array}{c}\mathbf{\mu }\\ \mathbf{g}(\mathbf{\mu })+{\mathbf{J}}_{\mathbf{x}}E[(\mathbf{\mu })\delta \mathbf{x}]+{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })E[\delta \mathbf{q}]\end{array}\right]\\ & =\left[\begin{array}{c}\mathbf{\mu }\\ \mathbf{g}(\mathbf{\mu })\end{array}\right]\end{array}$$
同样，参考加性噪声模型的方法求解方法有：
$$\begin{array}{rl}Cov[\tilde{\mathbf{g}}(\mathbf{x})]& \approx E[\left(\tilde{\mathbf{g}}(\mathbf{x})-E[\tilde{\mathbf{g}}(\mathbf{x})]\right){\left(\tilde{\mathbf{g}}(\mathbf{x})-E[\tilde{\mathbf{g}}(\mathbf{x})]\right)}^T]\\ & =E\left[\left[\begin{array}{c}\delta \mathbf{x}\\ {\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })\delta \mathbf{x}+{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })\delta \mathbf{q}\end{array}\right]{\left[\begin{array}{c}\delta \mathbf{x}\\ {\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })\delta \mathbf{x}+{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })\delta \mathbf{q}\end{array}\right]}^T\right]\\ & =\left[\begin{array}{cc}E[\delta \mathbf{x}\delta {\mathbf{x}}^T]& E[\delta \mathbf{x}\delta {\mathbf{x}}^T]{\mathbf{J}}_{\mathbf{x}}^T(\mathbf{\mu })+E[\delta \mathbf{x}\delta {\mathbf{q}}^T]{\mathbf{J}}_{\mathbf{q}}^T(\mathbf{\mu })\\ {\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })E[\delta \mathbf{x}\delta {\mathbf{x}}^T]+{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })E[\delta \mathbf{q}\delta {\mathbf{x}}^T]& \begin{array}{rl}& {\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })E[\delta \mathbf{x}\delta {\mathbf{x}}^T]{\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })+{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })E[\delta \mathbf{q}\delta {\mathbf{x}}^T]{\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })+\\ & {\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })E[\delta \mathbf{x}\delta {\mathbf{q}}^T]{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })+{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })E[\delta \mathbf{q}\delta {\mathbf{q}}^T]{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })\end{array}\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{P}& \mathbf{P}{\mathbf{J}}_{\mathbf{x}}^T(\mathbf{\mu })\\ {\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })\mathbf{P}& {\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })\mathbf{P}{\mathbf{J}}_{\mathbf{x}}(\mathbf{\mu })+{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })\mathbf{Q}{\mathbf{J}}_{\mathbf{q}}(\mathbf{\mu })\end{array}\right]\end{array}$$
上式最后一步化简中利用了$\mathbf{x}$和$\mathbf{q}$不相关的，互协方差$E(\delta \mathbf{x}\delta {\mathbf{q}}^T)=E(\delta \mathbf{q}\delta {\mathbf{x}}^T)=0$的性质。

在有了非加性噪声模型下的Gaussian变换公式后，我们同样可以按照Kalman滤波的推导方法推导如下所示非加性噪声模型下的一阶近似EKF
$$\{\begin{array}{rl}{\mathbf{x}}_k& =\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{q}}_{k-1})\\ {\mathbf{z}}_k& =\mathbf{h}({\mathbf{x}}_k,{\mathbf{r}}_k)\end{array}$$

具体的推导步骤和加性噪声模型完全一致，这里不再赘述，直接给出结论：
$$一阶近似EKF（非加性）\{\begin{array}{rl}一步预测& \{\begin{array}{rl}{\hat{\mathbf{x}}}_k& =\mathbf{f}({\mathbf{\mu }}_{k-1},0)\\ {\hat{\mathbf{P}}}_k& ={\mathbf{F}}_{\mathbf{x}}({\mathbf{\mu }}_{k-1}){\mathbf{P}}_{k-1}{\mathbf{F}}_{\mathbf{x}}^T({\mathbf{\mu }}_{k-1})+{\mathbf{F}}_{\mathbf{q}}({\mathbf{\mu }}_{k-1}){\mathbf{Q}}_{k-1}{\mathbf{F}}_{\mathbf{q}}^T({\mathbf{\mu }}_{k-1})\end{array}\\ 量测更新& \{\begin{array}{rl}\mathbf{S}& ={\mathbf{H}}_{\mathbf{x}}({\mathbf{\mu }}_{k-1}){\mathbf{P}}_{k-1}{\mathbf{H}}_{\mathbf{x}}^T({\mathbf{\mu }}_{k-1})+{\mathbf{H}}_{\mathbf{r}}({\mathbf{\mu }}_{k-1}){\mathbf{Q}}_{k-1}{\mathbf{H}}_{\mathbf{r}}^T({\mathbf{\mu }}_{k-1})\\ \mathbf{K}& ={\hat{\mathbf{P}}}_k{\mathbf{H}}_{\mathbf{x}}^T({\hat{\mathbf{\mu }}}_k){\mathbf{S}}^{-1}\\ {\tilde{\mathbf{x}}}_k& ={\hat{\mathbf{\mu }}}_k+\mathbf{K}({\mathbf{z}}_k-\mathbf{h}({\hat{\mathbf{\mu }}}_k,0))\\ {\tilde{\mathbf{P}}}_k& ={\hat{\mathbf{P}}}_k-\mathbf{K}{\mathbf{H}}_{\mathbf{x}}({\hat{\mathbf{\mu }}}_k){\hat{\mathbf{P}}}_k\end{array}\end{array}$$

式中，${\mathbf{F}}_{\mathbf{i}}({\mathbf{\mu }}_{k-1},0),\mathbf{i}=\mathbf{x},\mathbf{q}$和${\mathbf{H}}_{\mathbf{i}}({\hat{\mathbf{\mu }}}_k,0),\mathbf{i}=\mathbf{x},\mathbf{q}$分别为状态转移函数$\mathbf{f}(\mathbf{x},\mathbf{q})$和量测函数$\mathbf{h}(\mathbf{x},\mathbf{r})$在对应点处的Jacobian矩阵。

总结

除了上述两种一阶近似EKF之外，还有一种取泰勒级数二次项的二阶近似EKF，但其公式推导复杂，计算资源消耗较大，一般应用较少。对于大多数工业应用来说，一阶近似EKF的性能已完全能够满足我们的需求，因此这里对于二阶EKF略过不提，感兴趣的可自行推导。