摘要——卡尔曼滤波器为估计工程系统的状态提供了一项重要技术。由于非线性卡尔曼滤波器有多种变体，因此缺乏针对特定研究和工程应用的滤波器选择指南。 这就需要深入讨论不同非线性卡尔曼滤波器的复杂性。 实际状态估计应用特别感兴趣的是扩展卡尔曼滤波器 (EKF) 和无迹卡尔曼滤波器 (UKF)。 本教程分为三篇独立的文章。 第一部分给出了 EKF 和 UKF 的一般比较，并提供了滤波器选择指南。 第二部分介绍了有关 EKF 和 UKF 实现的详细信息，包括方程式、技巧和示例代码。 第三部分研究了用于确定系统的假定噪声特性的不同技术以及这些非线性卡尔曼滤波器的调谐程序。

一、引言
无迹卡尔曼滤波器 (UKF) 是一种通用的工程工具，一旦了解它就可以为许多实际问题提供良好的非线性估计结果（Julier 和 Uhlmann，1997）。 UKF 已在各种应用中得到有效实施，例如姿态估计（Rhudy 等人，2011 年；Gross 等人，2012 年；Rhudy 等人，2013a）、风估计（Rhudy 等人，2013c）和空速估计（Gururajan 等人，2013a；Gururajan 等人，2013b）。 由于缺少雅可比矩阵计算，UKF也比扩展卡尔曼滤波器更容易构造和修改
(EKF)（Kalman 和 Bucy，1961）在滤波器设计的原型阶段（Rhudy 和 Gu，2013）。尽管现有的工作详细介绍了 UKF 的实现，例如 （Julier 和 Uhlmann，1997 年；van der Merwe 等人，2004 年），这些技术作品是用高级语言编写的，对于那些不太熟悉无味转换 (UT) 的人来说，理解起来可能有点困难。本指南旨在 澄清技术文献，使 UKF 成为各种用户更容易使用的工具。

本文的其余部分安排如下。 首先，讨论了线性和非线性随机状态估计问题，包括线性和扩展卡尔曼滤波器的定义。 然后针对加性和非加性噪声假设提供并讨论了 UKF 的理论方程。 最后，给出了一个示例问题来帮助说明 UKF 在实际情况中的应用。 此外，附录中提供了加法和非加法 UKF 的详细概述，以及所考虑示例问题的完整 MATLAB 代码。

线性随机状态估计

一个一般的离散时间线性状态空间系统可以写成

UKF

Unscented Transformation

Unscented Transformation (UT)是UKF的核心技术，用于处理非线性变换y=f(x)中的非线性，其中x和y是L×1向量，f是L×1向量值函数 . 这里，x 是一个随机变量，通常假设其服从均值 x 和协方差 Px 的正态分布（高斯分布）。 UT 为 EKF 中使用的雅可比矩阵的分析线性化提供了统计替代方法。 有关这些线性化技术的详细分析比较，请参阅（Rhudy 等人，2013b）。 UT 使用一小组确定性选择的点，称为 sigma 点，这些点是根据先验条件选择的，即这些点是从假设的先验分布中选择的。 这些点的分布或先验分布的置信度是根据为 UT 选择的尺度参数确定的。 尺度参数有不同的表示和符号，但最终这些表示是等效的，并且影响sigma的分布以及用于重建后验（变换后）统计的权重向量。

UT 的缩放可以完全由三个缩放参数表示。 主要尺度参数 $\alpha$ 决定了 sigma 点的分布。 该参数可以在 $10^{-4}$ 和 1 之间变化。较小的 $\alpha$ 会导致更严格（更接近）的 sigma 点选择，而较大的 $\alpha$ 会提供更广泛的 sigma 点分布。 次要缩放参数 $\beta$ 用于包括有关先验分布的信息（对于高斯分布，β= 2 是最优的）。 第三尺度参数 κ 通常设置为 0，有关更多信息，请参见 (Julier and Uhlmann, 1997)。 使用这三个缩放参数，定义了一个附加缩放参数 λ 和权重向量 $W^m$（均值）和$W^c$（协方差）

$$\begin{array}{rl}{\chi }_i& =\bar{x}\\ {\chi }_0& =\bar{x}+(\sqrt{(L+\lambda )P_x}{)}_{i}i=1,...,L\\ {\chi }_i& =\bar{x}-(\sqrt{(L+\lambda )P_x}{)}_{i-L}i=L+1,...,2L\\ W_0^{(m)}& =\frac{\lambda }{L+\lambda }\\ W_0^{(c)}& =\frac{\lambda }{L+\lambda }+(1-{\alpha }^2+\beta )\\ W_i^{(m)}& =W_i^{(c)}=\frac{1}{2(L+\lambda )}i=1,...,2L\end{array}$$

其中$L$是状态向量的长度。 然后使用随机变量 x 的参数 $\lambda$、先验均值 $\bar{x}$ 和协方差 $P_x$ 生成 $2L+1$ sigma点，如
$$\chi =[\bar{\mathbf{x}}\bar{\mathbf{x}}+\sqrt{L+\lambda }\sqrt{{\mathbf{P}}_x}\sqrt{L-\lambda }\sqrt{{\mathbf{P}}_x}]$$

其中 $\chi$ 是 sigma 点的 L× (2L+1) 矩阵，其中该矩阵的每一列代表一个 sigma 点。 请注意，在 上式中，向量和矩阵的和定义为将向量添加到矩阵的每一列。 或者，L×1 列向量 $\bar{x}$ 可以乘以 1×L 行向量，得到一个 L×L 矩阵，可以使用标准矩阵加法。 还要注意 (9) 包含矩阵的平方根。 虽然有多种计算矩阵平方根的方法，但在性能和计算效率方面推荐的方法是 Cholesky 方法（Rhudy 等人，2011）。 该方法使用 Cholesky 分解来计算下三角矩阵，然后可以将其用作矩阵平方根的表示，即

$${\mathbf{P}}_x=(\sqrt{{\mathbf{P}}_x})(\sqrt{{\mathbf{P}}_x}{)}^T$$

$\sqrt{{\mathbf{P}}_x}$是下三角矩阵。 请注意，此表示不同于主矩阵平方根，后者采用不带转置的 (10) 形式，并且通常是非三角形的。 一种不同的处理矩阵平方根的方法，称为“平方根 UKF (SR-UKF)”（van der Merwe 和 Wan，2001），它提高了计算复杂性，但可以获得不同的性能结果。

一旦生成了 sigma 点，每个点都通过非线性函数，即 sigma 点矩阵的每一列 $\chi$ 都通过非线性传播，如

其中上标 (i) 表示矩阵的第 i 列，ψ 是变换后的 sigma 点矩阵。 然后，使用 (8) 中定义的权重向量，使用这些转换后的 sigma 点的加权平均值来估计后验均值和协方差，如

其中y是y的估计均值，Py是y的估计协方差矩阵。 这些值表示后验统计，或非线性变换后估计的统计特性。

Standard (Unaugmented) UKF

随机估计问题中使用的一个常见假设是过程和测量噪声项是可加的，如
$$\begin{gathered} x_{k+1}=f(x_k,u_k)+w_k \\ {z}_{k+1}=h(x_{k+1},u_{k+1})+V_{k+1} \end{gathered}$$

使用这种噪声假设，可以应用标准 UKF 方程而不需要增加状态向量，这将在下一节中讨论，另请参见（Wu 等人，2005 年）。

sigma点的维度和状态向量相同的，从假设的初始条件 x0 和 P0 开始，递归执行 UKF。 UKF 使用第 IIA 节中描述的无味转换 (UT)。 请注意，缩放参数通常假定在整个 UKF 中保持不变，即，过滤器中的每个时间步长都使用相同的 UT 缩放比例。 这些缩放参数以及权重向量可以在执行过滤器之前定义一次。在每个离散时间步骤 k，首先根据先验状态估计 $x_{k-1}$ 和协方差 $P_{k-1}$ 生成一组sigma 点，如：
$$\begin{array}{rl}{\chi }_0& =\bar{x}\\ {\chi }_0& =\bar{x}+(\sqrt{(L+\lambda )P_x}{)}_{i}i=1,...,L\\ {\chi }_0& =\bar{x}-(\sqrt{(L+\lambda )P_x}{)}_{i-L}i=L+1,...,2L\\ W_0^{(m)}& =\frac{\lambda }{L+\lambda }\\ W_0^{(c)}& =\frac{\lambda }{L+\lambda }+(1-{\alpha }^2+\beta )\\ W_i^{(m)}& =W_i^{(c)}=\frac{1}{2(L+\lambda )}i=1,...,2L\end{array}$$

$$\lambda ={\alpha }^2(L+\kappa )-L$$
接下来，每个 sigma 点都通过非线性状态预测函数 f。 由于假定过程噪声是可加的且为零均值，因此可以从预测函数中省略它，如：
$${\chi }_{k+1|k,i}=f({\chi }_{k,i},u_k),0,1,...,2L$$
使用转换后sigma点计算新的分布的均值和方差：

先验状态和协方差估计：
$$\begin{gathered} {\hat{x}}_{k+1|k}=\sum _{i=0}^{2L}{w}_i^m{\chi }_{k+1|k,i} \\ {\hat{P}}_{k+1|k}=Q_k+\sum _{i=0}^{2L}{w}_i^c({\chi }_{k+1|k,i}-{\hat{x}}_{k+1|k})({\chi }_{k+1|k,i}-{\hat{x}}_{k+1|k}{)}^T \end{gathered}$$

将先验映射到测量空间然后算出均值和方差

不用再产生sigma points了，我们可以直接使用预测出来的sigma点集，并且可以忽略掉处理噪声部分。那么对先验的非线性映射就可以表示为如下的sigma point预测（即预测非线性变换以后的均值和协方差）：

测量模型：
$$z_{k+1|k}=h({\chi }_{k+1|k,i},u_{k+1}),0,1,...,2L$$
其中 z 是输出 sigma 点的矩阵。 然后使用这些输出sigma点来计算预测输出、输出协方差矩阵以及状态和输出之间的交叉协方差

预测测量均值：
$${\hat{z}}_{k+1|k}=\sum _{i=1}^{2L}{w}_i^m{z}_{k+1|k,i}$$
预测测量协方差：
$$S_{k|k-1}=R_k+\sum _{i=0}^{2L}{w}_i^c(z_{k|k-1}-{\hat{z}}_{k|k-1})(z_{k|k-1}-{\hat{z}}_{k|k-1}{)}^T$$
sigma点集在状态空间和测量空间的互相关函数 $T_{k|k-1}$为：

$$T_{k+1|k}=\sum _{i=0}^{2L}{w}_i^c(x_{k+1|k,i}-{\hat{x}}_{k+1|k,i})\sum _{i=0}^{2L}{w}_i^c(z_{k+1|k,i}-{\hat{z}}_{k+1|k,i})$$
请注意，由于额外噪声假设，假设的测量噪声协方差矩阵 R 被添加到输出协方差矩阵。 这些协方差矩阵用于计算卡尔曼增益矩阵K，使用

$${\mathbf{K}}_{k+1|k}=T_{k+1|k}{S}_{k+1|k}^{-1}$$

更新状态：
$$x_{k+1|k+1}=x_{k+1|k}+K_{k+1|k}(z_{k+1}-{\hat{z}}_{k+1|k})$$
协方差矩阵更新：
$$P_{k+1|k+1}=P_{k+1|k}-K_{k+1|k}{S}_{k+1|k}{K}_{k+1|k}^T$$