【自动驾驶】学习卡尔曼滤波（三）——无迹卡尔曼滤波

参考资料

• 无损卡尔曼滤波
• 卡尔曼滤波
• 卡尔曼滤波算法原理
• 从贝叶斯滤波到无迹卡尔曼滤波

文章大部分内容主要来自参考资料，旨在于更方便学习记忆查找，不作其他用途。

在上一篇整理总结了扩展卡尔曼滤波。扩展卡尔曼滤波通过一阶泰勒展开得到近似的线性状态转移矩阵与观测矩阵。EKF的一个最大的问题就是求解雅可比矩阵计算量比较大，因此这里再介绍另一种可用于非线性系统的卡尔曼滤波——无迹卡尔曼滤波（Unscented Kalman Filter，UKF）。

1. 无迹变换

在应用KF时面临的主要问题就是非线性过程模型和观测模型的处理。

对于我们想要估计的状态，在k时刻满足均值为$x_{k|k}$,方差为$P_{k|k}$的一个高斯分布，KF能够使用的前提就是所处理的状态是满足高斯分布的，对于非线性的情况下，EKF是寻找一个线性函数来近似这个非线性函数，而UKF就是去找一个与真实分布近似的高斯分布。

UKF的基本思路就是： 近似非线性函数的概率分布要比近似非线性函数本身要容易

如何去找一个与真实分布近似的高斯分布呢？——找一个与真实分布有着相同的均值和协方差的高斯分布。

如何去找这样的均值和方差呢？——无迹变换。

1.1 无迹变换基本概念

无迹变换的核心理念：

• 近似概率分布比近似任意的非线性函数或变换要相对容易。

无迹变换要解决的问题是：已知某随机变量（多维情形下是随机向量）的概率分布（均值和方差），求其经过某非线性函数 $g(\cdot )$变换后的概率分布。

基于上述思想，无迹变换的主要步骤为：

1. 根据某种规则对随机变量的概率分布进行确定性采样，并为采样点分配权重（均值权重和方差权重），采样点我们通常称之为 sigma 点；

2. 将每一个 sigma 点进行非线性变换，得到新的 sigma 点；

3. 对非线性变换后的新的 sigma 点进行加权求和，分别计算加权均值和加权方差，用加权均值和加权方差近似表征随机变量经非线性变换后的概率分布。

无迹变换主要包括两种形式：一般形式的无迹变换和比例无迹变换。两种无迹变换的区别主要体现在采样规则和权重计算上。比例无迹变换是对前者做出的改进。

1.2 一般形式的无迹变换

假设存在$n$维随机向量 $X$，其服从均值 ${\mu }_x$ 和协方差 ${\Sigma }_x$的正态分布：
$$X\sim \mathcal{N}\left({\mu }_x,{\Sigma }_x\right)$$
将 $X$ 经过非线性函数 $g(\cdot )$ 进行变换，得到随机向量 $Y$，我们使用一般形式的无迹变换估计 $Y$ 的概率分布。一般形式的无迹变换最早由 Julier 等人提出，其主要操作流程如下所述：

• 步骤一：参数选择与权重计算

  一般形式的无迹变换只涉及一个外部引入参数$\kappa$，各 sigma 点（共$2n+1$ 个）的权重分配如下：
  $$W^{(i)}=\{\begin{array}{ll}\frac{\kappa }{n+\kappa }& i=0\\ \frac{1}{2(n+\kappa )}& i=1,\cdots ,2n\end{array}$$

  其中, $\kappa \in \mathbb{R}$, 表征了 sigma 点相对均值的散布程度， $\kappa$ 越大，非均值处的 sigma 点距离均值越远，且所占权重越小, 而均值处 sigma 点所占权重则相对越 大。对于高斯问题， $n+\kappa =3$ 是一个比较好的选择，对于非高斯问题（无迹变换也适用于非高斯问题）， $\kappa$ 应该选择其它更恰当的值。

• 步骤二：确定性采样

  通常情况下, sigma 点位于均值处及对称分布于主轴的协方差处（每维两个）。按照如下方法采样得到 $2n+1$ 个 sigma 点, 构成 $n\times (2n+1)$ 的点集矩阵 $\mathcal{X}$ :
  $${\mathcal{X}}^{(i)}=\{\begin{array}{ll}{\mu }_x& i=0\\ {\mu }_x+{\left(\sqrt{(n+\kappa ){\Sigma }_x}\right)}^{(i-1)}& i=1,\cdots ,n\\ {\mu }_x-{\left(\sqrt{(n+\kappa ){\Sigma }_x}\right)}^{(i-n-1)}& i=n+1,\cdots ,2n\end{array}$$
  其中, ${\left(\sqrt{(n+\kappa ){\Sigma }_x}\right)}^{(i-1)}$ 表示矩阵 $(n+\kappa ){\Sigma }_x$ 作 Cholesky分解后下三角矩阵的第 $i-1$ 列, ${\left(\sqrt{(n+\kappa ){\Sigma }_x}\right)}^{(i-n-1)}$ 同理。

  Cholesky分解
  
  当 A 是一个 Symmetric positive definite matrix的时候，就可以分解成 lower triangle 矩阵 L 和它的转置也就是 upper triangle

• 步骤三：sigma 点非线性变换

  将每个 sigma 点（即 $\mathcal{X}$ 的每一列）进行 $g(\cdot )$ 的非线性变换，得到变换后的新的点集矩阵 $\mathcal{Y}$ :
  $${\mathcal{Y}}^{(i)}=g\left({\mathcal{X}}^{(i)}\right)i=0,\cdots ,2n$$

• 步骤四：加权计算近似均值与近似协方差
  $$\begin{array}{c}{\mu }_y=\sum _{i=0}^{i=2n}{W}^{(i)}{\mathcal{Y}}^{(i)}\\ {\Sigma }_y=\sum _{i=0}^{i=2n}{W}^{(i)}\left[{\mathcal{Y}}^{(i)}-{\mu }_y\right]{\left[{\mathcal{Y}}^{(i)}-{\mu }_y\right]}^T\end{array}$$

1.3 比例无迹变换

当参数 $\kappa <0$ 时, 权重 $W^{(0)}=\frac{\kappa }{n+\kappa }<0$, 加权算得 的近似协方差可能存在非半正定的情况。为应对该问题, Julier 等人后来提出无迹变换的改进形式一一比例无迹变换，并由 Merwe 等人对其进行了简化。

比例无迹变换与一般形 式的无迹变换的主要区别体现在参数选取与 sigma 点权重计算上，其主要操作流程如下所述：

• 步骤一：参数选择与权重计算

  比例无迹变换引入了四个外部参数： $\alpha 、\beta 、\kappa$ 和 $\lambda$, 各 sigma 点 $(2n+1$ 个 $)$ 的权重 分配如下：
  $$\begin{array}{c}{W}_m^{(i)}=\{\begin{array}{ll}\frac{\lambda }{n+\lambda }& i=0\\ \frac{1}{2(n+\lambda )}& i=1,\cdots ,2n\end{array}\\ W_c^{(i)}=\{\begin{array}{ll}\frac{\lambda }{n+\lambda }+1-{\alpha }^2+\beta & i=0\\ \frac{1}{2(n+\lambda )}& i=1,\cdots ,2n\end{array}\end{array}$$
  其中, $W_m^{(i)}$ 表示计算近似均值时 sigma 点的权重, $W_c^{(i)}$ 表示计算近似协方差时 sigma 点的权重。参数 $\lambda$ 满足:
  $$\lambda ={\alpha }^2(n+\kappa )-n$$

  参数$\beta$用于引入随机变量概率分布的高阶矩信息，如果分布是精确的高斯分布，则 $\beta =2$ 是最优选择。

  参数 $\alpha$ 和 $\kappa$ 为确定 sigma 点分布在均值多远的范围内的比例参数。 $\alpha$ 满足 $10^{-4}\le \alpha \le 1$, 为避免强非线性系统中的非局部效应问题, $\alpha$ 通常取一个较小值; $\kappa$ 满 足 $\kappa \ge 0$ ，通常取 $\kappa =3-n$ 或 $\kappa =0$ 。

  下图呈现了当 $\alpha$ 取值分别为 $0.3$ 和 1 时， sigma 点的分布情况，从图中可以发现， $\alpha$ 取值越大，非均值处的 sigma 点距离均值越远。
  

• 步骤三：sigma 点非线性变换

  将每个 sigma 点（即 $\mathcal{X}$ 的每一列）进行 $g(\cdot )$ 的非线性变换, 得到变换后的新的点集矩 阵 $\mathcal{Y}$ :
  $${\mathcal{Y}}^{(i)}=g\left({\mathcal{X}}^{(i)}\right)i=0,\cdots ,2n$$

• 步骤四：加权计算近似均值与近似协方差
  $$\begin{array}{c}{\mu }_y=\sum _{i=0}^{i=2n}{W}_m^{(i)}{\mathcal{Y}}^{(i)}\\ {\Sigma }_y=\sum _{i=0}^{i=2n}{W}_c^{(i)}\left[{\mathcal{Y}}^{(i)}-{\mu }_y\right]{\left[{\mathcal{Y}}^{(i)}-{\mu }_y\right]}^T\end{array}$$

2. 无迹卡尔曼滤波

只介绍可加性噪声条件下的无迹卡尔曼滤波算法。

UKF同样也分为预测和更新。

假设系统的状态方程和观测方程为：
从上式中我们可以发现, 过程噪声 $Q_k$ 和观测噪声 $R_k$ 是以线性可加项的形式存在于系统状态方程和观测方程中的，这种形式的表述基于这样的前提假设：过程噪声 $Q_k$ 对系统状态转移过程的影响是线性的，观测噪声 $R_k$ 对系统观测过程的影响也是线性的。很明显, 此种情况下, $Q_k$ 与 $X_k$ 是同维的, $R_k$ 与 $Y_k$ 是同维的。

假设有如下已知条件：

基于以上已知条件与符号定义，我们总结一下可加性噪声条件下的无迹卡尔曼滤波算法步骤（使用比例无迹变换）。

2.1 初始化

• 选定滤波初值
  根据观测量初值 $z_0$ 及观测函数 $h(\cdot )$ 计算对应的状态量初始均值 ${\mu }_{X_0}^{+}$, 设定状态量协方差 初值 ${\Sigma }_{X_0\text{ 。 }}^{+}$

• 选定无迹变换参数
  设定比例无迹变换参数 $\alpha 、\beta 、\kappa$ 和 $\lambda$ 的参数值, 若使用一般形式的无迹变换, 仅需设定 $\kappa$ 的取值。

• sigma 点权重计算
  根据无迹变换参数取值及权重计算公式计算各 sigma 点权重。
  $$\begin{array}{c}{W}_m^{(i)}=\{\begin{array}{ll}\frac{\lambda }{n+\lambda }& i=0\\ \frac{1}{2(n+\lambda )}& i=1,\cdots ,2n\end{array}\\ W_c^{(i)}=\{\begin{array}{ll}\frac{\lambda }{n+\lambda }+1-{\alpha }^2+\beta & i=0\\ \frac{1}{2(n+\lambda )}& i=1,\cdots ,2n\end{array}\end{array}$$
  对 $k=1,2,3,\cdots$, 执行——

2.2 sigma采样

对 $k-1$ 时刻状态量 $X_{k-1}$ 的后验概率分布进行 sigma 采样
$${\mathcal{X}}_{k-1}^{+(i)}=\{\begin{array}{ll}{\mu }_{X_{k-1}}^{+}& i=0\\ {\mu }_{X_{k-1}}^{+}+\gamma {\left(\sqrt{{\Sigma }_{X_{k-1}}^{+}}\right)}^{(i-1)}& i=1,\cdots ,n\\ {\mu }_{X_{k-1}}^{+}-\gamma {\left(\sqrt{{\Sigma }_{X_{k-1}}^{+}}\right)}^{(i-n-1)}& i=n+1,\cdots ,2n\end{array}$$
其中, $\gamma =\sqrt{(n+\lambda )}$ 。

2.3 预测步

1. 状态转移非线性变换

$${\mathcal{X}}_k^{-\ast (i)}=f\left({\mathcal{X}}_{k-1}^{+(i)},u_k\right),i=0,1,2,\cdots ,2n$$

2. 加权计算 k 时刻状态量的先验概率分布

$$\begin{array}{c}{\mu }_{X_k}^{-}=\sum _{i=0}^{i=2n}{W}_m^{(i)}{\mathcal{X}}_k^{-\ast (i)}\\ {\Sigma }_{X_k}^{-}=\sum _{i=0}^{i=2n}{W}_c^{(i)}\left[{\mathcal{X}}_k^{-\ast (i)}-{\mu }_{X_k}^{-}\right]{\left[{\mathcal{X}}_k^{-\ast (i)}-{\mu }_{X_k}^{-}\right]}^T+{\Sigma }_Q\end{array}$$

由于过程噪声是线性可加的，所以此处${\Sigma }_Q$ 直接加在了加权协方差的末尾。

2.4 对 k 时刻状态量的先验概率分布进行 sigma 采样

$${\mathcal{X}}_k^{-(i)}=\{\begin{array}{ll}{\mu }_{X_k}^{-}& i=0\\ {\mu }_{X_k}^{-}+\gamma {\left(\sqrt{{\Sigma }_{X_k}^{-}}\right)}^{(i-1)}& i=1,\cdots ,n\\ {\mu }_{X_k}^{-}-\gamma {\left(\sqrt{{\Sigma }_{X_k}^{-}}\right)}^{(i-n-1)}& i=n+1,\cdots ,2n\end{array}$$
某些时候，为降低运算量会省略此步, 而在下一步骤中直接使用2.2节中的 sigma 点集 ${\mathcal{X}}_k^{-\ast (i)}$, 这样做在一定程度上会降低精度。

更新步

1. 观测非线性变换

$${\mathcal{Z}}_k^{(i)}=h\left({\mathcal{X}}_k^{-(i)}\right),i=0,1,2,\cdots ,2n$$

2. 加权计算 $k$ 时刻观测量 $Z_k$ 的概率分布

$$\begin{array}{c}{\mu }_{Z_k}=\sum _{i=0}^{i=2n}{W}_m^{(i)}{\mathcal{Z}}_k^{(i)}\\ {\Sigma }_{Z_k}=\sum _{i=0}^{i=2n}{W}_c^{(i)}\left[{\mathcal{Z}}_k^{(i)}-{\mu }_{Z_k}\right]{\left[{\mathcal{Z}}_k^{(i)}-{\mu }_{Z_k}\right]}^T+{\Sigma }_R\end{array}$$
由于观测噪声是线性可加的，所以此处 ${\Sigma }_R$ 直接加在了加权协方差的末尾。

3. 计算状态量与观测量的互协方差

$${\Sigma }_{X_k{Z}_k}=\sum _{i=0}^{i=2n}{W}_c^{(i)}\left[{\mathcal{X}}_k^{-(i)}-{\mu }_{X_k}^{-}\right]{\left[{\mathcal{Z}}_k^{(i)}-{\mu }_{Z_k}\right]}^T$$

4. 计算卡尔曼增益

$$K_k={\Sigma }_{X_k{Z}_k}{\Sigma }_{Z_k}^{-1}$$

5. 计算 k 时刻状态量的后验概率分布

$$\begin{array}{c}{\mu }_{X_k}^{+}={\mu }_{X_k}^{-}+K_k\left(z_k-{\mu }_{Z_k}\right)\\ {\Sigma }_{X_k}^{+}={\Sigma }_{X_k}^{-}-K_k{\Sigma }_{Z_k}{K}_k^T\end{array}$$

可加性噪声条件下的无迹卡尔曼滤波算法流程可总结为：

3. 应用——移动机器人应用

该部分介绍与前文一致。也可以参考这个文档。

python实现——见于github代码仓库。