扩展卡尔曼滤波，无迹卡尔曼滤波和统计线性化滤波器

背景说明

由于在工程应用中，系统的动态模型和量测模型往往不是线性的，此时，卡尔曼滤波不再适用。所以本文介绍三种模型构造高斯逼近的方法。

1. 基于泰勒基数展开的扩张卡尔曼滤波（extend Kalman filter，EKF）；
2. 基于统计线性化的统计线性化滤波器（statistically linearized filter，SLF）；
3. 基于无迹变换的无迹卡尔曼滤波（unscented Kalman filter，UKF）。

无迹卡尔曼滤波被认为是统计线性化滤波的逼近。ULF优于EKF在于：SLF是更全局的逼近。不足在于：非线性函数的期望值必须进行闭型解算。UKF不要求模型函数可微，不要计算雅可比矩阵，但UKF的精度低于SLF，因为SLF采用了更多的数据；且UKF计算步骤较多。

扩展卡尔曼滤波

将高斯随机变量性转换在另一个随机变量y：
$$\begin{gathered} \mathbf{x}\sim \mathbf{N}(\mathbf{m},\mathbf{P}) \\ \mathbf{y}=\mathbf{g}(\mathbf{x}) \end{gathered}$$

对$\mathbf{g}(\mathbf{x})$进行泰勒展开，设$\mathbf{x}=\mathbf{m}+\mathbf{\delta }\mathbf{x}$，且$\mathbf{\delta }\mathbf{x}\sim \mathbf{N}(\mathbf{m},\mathbf{P})$。其中${\mathbf{G}}_{\mathbf{x}}(\mathbf{m}),{\mathbf{G}}_{\mathbf{x}\mathbf{x}}^{\mathbf{i}}(\mathbf{m})$分别是$\mathbf{g}$的雅可比矩阵和$\mathbf{H}\mathbf{e}\mathbf{s}\mathbf{s}\mathbf{i}\mathbf{a}\mathbf{n}$矩阵。
$$\mathbf{g}(\mathbf{x})=\mathbf{g}(\mathbf{m}+\mathbf{\delta }\mathbf{x})\approx \mathbf{g}(\mathbf{m})+{\mathbf{G}}_{\mathbf{x}}(\mathbf{m})\mathbf{\delta }\mathbf{x}+\sum \frac{1}{2}\mathbf{\delta }\mathbf{x}{\mathbf{G}}_{\mathbf{x}\mathbf{x}}^{\mathbf{i}}\mathbf{\delta }\mathbf{x}{\mathbf{e}}_{\mathbf{i}}+...$$

利用展开式中的前两项逼近$\mathbf{g}(\mathbf{x})$,且可求起均值和方差分别为：
$$\begin{gathered} \mathbf{g}(\mathbf{x})=\mathbf{g}(\mathbf{m}+\mathbf{\delta }\mathbf{x})\approx \mathbf{g}(\mathbf{m})+{\mathbf{G}}_{\mathbf{x}}(\mathbf{m})\mathbf{\delta }\mathbf{x} \\ \mathbf{E}[\mathbf{g}(\mathbf{x})]=\mathbf{g}(\mathbf{m}) \\ \mathbf{E}[(\mathbf{g}(\mathbf{x})-\mathbf{E}[\mathbf{g}(\mathbf{x})])(\mathbf{g}(\mathbf{x})-\mathbf{E}[\mathbf{g}(\mathbf{x})]{)}^{\mathbf{T}}]={\mathbf{G}}_{\mathbf{x}}(\mathbf{m})\mathbf{P}{\mathbf{G}}_{\mathbf{x}}^{\mathbf{T}}(\mathbf{m}) \end{gathered}$$

通常需要求$\mathbf{x},\mathbf{y}$的联合协方差阵，该联合协方差阵可通过增光变换得到：

扩展卡尔曼滤波-过程和量测噪声可叠加类型：

叠加变换的通式为：【注意】该变换通式在后面的统计线性化滤波器和无迹变换滤波器中均一致。

则经过线性逼近的高斯逼近的$\mathbf{x},\mathbf{y}$联合分布为：

EKF模型为：

式中，${\mathbf{x}}_{\mathbf{k}}\in {\mathbf{R}}^{\mathbf{n}}$为状态量，${\mathbf{y}}_{\mathbf{k}}\in {\mathbf{R}}^{\mathbf{m}}$为量测量，${\mathbf{q}}_{\mathbf{k}-1}\sim \mathbf{N}(0,{\mathbf{Q}}_{\mathbf{k}-1})$为高斯过程噪声，${\mathbf{r}}_{\mathbf{k}}\sim \mathbf{N}(0,{\mathbf{R}}_{\mathbf{k}})$为量测噪声，$\mathbf{f}(\cdot )$为系统的动态模型，$\mathbf{h}(\cdot )$为系统的量测模型。

一阶可叠加卡尔曼滤波的预测与更新过程如下：

扩展卡尔曼滤波-过程和量测噪声不可叠加类型：

不可叠加变换通式为：

通过增广变换得到关于$\mathbf{x},\mathbf{y}$的均值和协方差为：

非叠加变换的线性逼近为：

EKF模型为：

相关的变量和函数均与可叠加变换的一致。

一阶不可叠加卡尔曼滤波的预测和更新过程如下：

统计线性化滤波器

对于统计线性化滤波器，只是把一阶泰勒级数逼近换成统计线性化。变换模型一致：
$$\begin{gathered} \mathbf{x}\sim \mathbf{N}(\mathbf{m},\mathbf{P}) \\ \mathbf{y}=\mathbf{g}(\mathbf{x}) \end{gathered}$$
在统计线性化中，可得到函数的线性近似：
$$\begin{gathered} \mathbf{g}(\mathbf{x})\simeq \mathbf{b}+\mathbf{A}\mathbf{\delta }\mathbf{x} \\ \mathbf{\delta }\mathbf{x}=\mathbf{x}-\mathbf{m} \end{gathered}$$

使均方根误差最小，可得：

对$\mathbf{g}(\mathbf{x})$的逼近中，$\mathbf{b}$恰好为均值，而逼近得到的协方差阵为：

统计线性化滤波器-噪声可叠加类型

其中，关于$\mathbf{x},\mathbf{y}$的增广矩阵的均值和方差求法与EKF一致：

关于$\mathbf{x}$的变换通式是一致的。
基于高斯逼近的统计线性化得到的$\mathbf{x},\mathbf{y}$联合分布为：

该统计线性化滤波器的预测和更新过程如下：

统计线性化滤波器-噪声不可叠加类型

通过增广变换得到关于$(x,y)$均值和协方差：

关于$\mathbf{x}$的变换通式为不可叠加的变换通式；
基于高斯逼近的统计线性化得到的$\mathbf{x},\mathbf{y}$的分布如下：

不可叠加噪声的统计线性化（卡尔曼）滤波器的预测和更新步骤如下：

无迹卡尔曼滤波

噪声可叠加的无迹卡尔曼滤波

可叠加的变换通式均一致。
基于高斯逼近的无迹变换的$\mathbf{x},\mathbf{y}$的分布为：

式中，子矩阵的计算表达式：

1. 构造$2\mathbf{n}+1$个$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点：
   
   式中，$\mathbf{\lambda }$为比例系数，且由算法系数$\mathbf{\alpha },\mathbf{\kappa }$决定（这两个系数应该是在相关算法中确定）：
   
2. 将$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点带入非线性函数$\mathbf{g}(\cdot )$中：
   
3. 计算子矩阵：
   
   其中，常值权值${\mathbf{W}}_{\mathbf{i}}^{(\mathbf{m})},{\mathbf{W}}_{\mathbf{i}}^{(\mathbf{c})}$的定义为：
   

噪声可叠加的无迹卡尔曼滤波预测和更新步骤：
系统的动态模型和量测模型与先前一致。
预测部分：
1). 构造$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点：

2). 将$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点带入系统动态模型中：

3). 计算预测均值${\mathbf{m}}_{\mathbf{k}}^{-}$和预测协方差${\mathbf{P}}_{\mathbf{k}}^{-}$:

更新部分：
1). 构造$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点：

2). 将$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点带入系统量测模型中：

3). 计算量测量预测均值${\mathbf{m}}_{\mathbf{k}}^{-}$和预测协方差${\mathbf{P}}_{\mathbf{k}}^{-}$，以及状态与量测量之间的互协方差${\mathbf{C}}_{\mathbf{k}}$:

4). 结合量测量${\mathbf{y}}_{\mathbf{k}}$，计算滤波增益${\mathbf{K}}_{\mathbf{k}}$，滤波状态平均值${\mathbf{m}}_{\mathbf{k}}$和方差${\mathbf{P}}_{\mathbf{k}}$：

噪声不可叠加的无迹卡尔曼滤波

不可叠加的变换通式如EKF中的一致。
基于高斯逼近的无迹变换的$\mathbf{x},\mathbf{y}$的分布如下：

式中，子矩阵的计算方法如下：
设$\mathbf{x},\mathbf{q}$的维数分别为$\mathbf{n},{\mathbf{n}}_{\mathbf{q}}$，且${\mathbf{n}}^{{}^{\prime }}=\mathbf{n}+{\mathbf{n}}_{\mathbf{q}}$.
1). 构造增广随机变量 $\tilde{\mathbf{x}}=(\mathbf{x},\mathbf{q})$的$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点：

式中，${\mathbf{\lambda }}^{{}^{\prime }}$与$\mathbf{\lambda }$的定义式一致，另外，增广均值和协方差定义式为：

2). 将$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点带入非线性方程：

3). 计算预测均值${\mathbf{\mu }}_{\mathbf{U}}$，预测协方差${\mathbf{S}}_{\mathbf{U}}$和状态量和量测量之间的互协方差${\mathbf{C}}_{\mathbf{U}}$：

噪声不可叠加的无迹卡尔曼滤波：

预测步骤：
1). 构造增广随机变量 $({\mathbf{x}}_{\mathbf{k}-1},{\mathbf{q}}_{\mathbf{k}-1})$的$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点：

2). 将$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点带入动态模型中：

3). 计算预测均值${\mathbf{m}}_{\mathbf{k}}^{-}$，预测协方差${\mathbf{P}}_{\mathbf{k}}^{-}$：

更新步骤：
1). 构造增广随机变量 $({\mathbf{x}}_{\mathbf{k}},{\mathbf{q}}_{\mathbf{k}})$的$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点：

2). 将$\mathbf{s}\mathbf{i}\mathbf{g}\mathbf{m}\mathbf{a}$点带入量测模型中：

3). 计算预测均值${\mathbf{\mu }}_{\mathbf{k}}$，预测协方差${\mathbf{S}}_{\mathbf{k}}$和状态量和量测量之间的互协方差${\mathbf{C}}_{\mathbf{k}}$：

4). 结合量测量${\mathbf{y}}_{\mathbf{k}}$，计算滤波增益${\mathbf{K}}_{\mathbf{k}}$，滤波状态平均值${\mathbf{m}}_{\mathbf{k}}$和方差${\mathbf{P}}_{\mathbf{k}}$：

本文参考贝叶斯滤波与平滑，作者萨日伽。太累了，不想去一一检验错误，如有疑问，请留言。