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

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

• 背景说明
• 扩展卡尔曼滤波
• 扩展卡尔曼滤波-过程和量测噪声可叠加类型：
• 扩展卡尔曼滤波-过程和量测噪声不可叠加类型：
• 统计线性化滤波器
• 统计线性化滤波器-噪声可叠加类型
• 统计线性化滤波器-噪声不可叠加类型
• 无迹卡尔曼滤波
• 噪声可叠加的无迹卡尔曼滤波
• 噪声不可叠加的无迹卡尔曼滤波

背景说明

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

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)}$x∼N(m,P)y=g(x)

对$\bm{g(x)}$g(x)进行泰勒展开，设$\bm{x = m + \delta x}$x=m+δx，且$\bm{\delta x \sim N(m,P)}$δx∼N(m,P)。其中$\bm{G_{x}(m),G^i_{xx}(m)}$Gx​(m),Gxxi​(m)分别是$\bm{g}$g的雅可比矩阵和$\bm{Hessian}$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}}+...$g(x)=g(m+δx)≈g(m)+Gx​(m)δx+∑21​δxGxxi​δxei​+...

利用展开式中的前两项逼近$\bm{g(x)}$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)}$g(x)=g(m+δx)≈g(m)+Gx​(m)δxE[g(x)]=g(m)E[(g(x)−E[g(x)])(g(x)−E[g(x)])T]=Gx​(m)PGxT​(m)

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

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

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

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

EKF模型为：

式中，$\bm{x_k \in R^n}$xk​∈Rn为状态量，$\bm{y_k \in R^m}$yk​∈Rm为量测量，$\bm{q_{k-1} \sim N(0, Q_{k-1})}$qk−1​∼N(0,Qk−1​)为高斯过程噪声，$\bm{r_k \sim N(0,R_k)}$rk​∼N(0,Rk​)为量测噪声，$\bm{f(\cdot)}$f(⋅)为系统的动态模型，$\bm{h(\cdot)}$h(⋅)为系统的量测模型。

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

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

不可叠加变换通式为：

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

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

EKF模型为：

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

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

统计线性化滤波器

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

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

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

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

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

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

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

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

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

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

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

无迹卡尔曼滤波

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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