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

背景说明

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

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

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

扩展卡尔曼滤波

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

对 $\bm{g(x)}$进行泰勒展开，设 $\bm{x = m + \delta x}$，且 $\bm{\delta x \sim N(m,P)}$。其中 $\bm{G_{x}(m),G^i_{xx}(m)}$分别是 $\bm{g}$的雅可比矩阵和 $\bm{Hessian}$矩阵。
$\bm{g(x) = g(m + \delta x) \approx g(m) + G_{x}(m)\delta x + \sum{\frac{{1}}{2}}\delta xG^i_{xx}\delta xe_{i}}+...$

利用展开式中的前两项逼近 $\bm{g(x)}$,且可求起均值和方差分别为：
$\bm{g(x) = g(m + \delta x) \approx g(m) + G_{x}(m)\delta x} \\ \bm{E[g(x)] = g(m)} \\ \bm{E[(g(x) - E[g(x)])(g(x) - E[g(x)])^T] = G_x(m)PG^T_x(m)}$

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

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

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

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

EKF模型为：

式中， $\bm{x_k \in R^n}$为状态量， $\bm{y_k \in R^m}$为量测量， $\bm{q_{k-1} \sim N(0, Q_{k-1})}$为高斯过程噪声， $\bm{r_k \sim N(0,R_k)}$为量测噪声， $\bm{f(\cdot)}$为系统的动态模型， $\bm{h(\cdot)}$为系统的量测模型。

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

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

不可叠加变换通式为：

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

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

EKF模型为：

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

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

统计线性化滤波器

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

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

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

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

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

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

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

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

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

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

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

无迹卡尔曼滤波

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

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

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

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

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

2). 将 $\bm{sigma}$点带入系统动态模型中：

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

更新部分：
1). 构造 $\bm{sigma}$点：

2). 将 $\bm{sigma}$点带入系统量测模型中：

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

4). 结合量测量 $\bm{y_k}$，计算滤波增益 $\bm{K_k}$，滤波状态平均值 $\bm{m_k}$和方差 $\bm{P_k}$：

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

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

式中，子矩阵的计算方法如下：
设 $\bm{x,q}$的维数分别为 $\bm{n,n_q}$，且 $\bm{n^{'} =n\ + n_q}$.
1). 构造增广随机变量 $\bm{ \widetilde{x} = (x,q)}$的 $\bm{sigma}$点：

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

2). 将 $\bm{sigma}$点带入非线性方程：

3). 计算预测均值 $\bm{\mu_U}$，预测协方差 $\bm{S_U}$和状态量和量测量之间的互协方差 $\bm{C_U}$：

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

预测步骤：
1). 构造增广随机变量 $\bm{ (x_{k-1},q_{k-1})}$的 $\bm{sigma}$点：

2). 将 $\bm{sigma}$点带入动态模型中：

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

更新步骤：
1). 构造增广随机变量 $\bm{ (x_k,q_k)}$的 $\bm{sigma}$点：

2). 将 $\bm{sigma}$点带入量测模型中：

3). 计算预测均值 $\bm{\mu_k}$，预测协方差 $\bm{S_k}$和状态量和量测量之间的互协方差 $\bm{C_k}$：

4). 结合量测量 $\bm{y_k}$，计算滤波增益 $\bm{K_k}$，滤波状态平均值 $\bm{m_k}$和方差 $\bm{P_k}$：

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