MATLAB - 估计滤波器（Estimation Filters）-编程知识

系列文章目录

前言

本篇文章翻译自官网，部分下标有问题，请自行分辨。

一、背景介绍

1.1 估算系统

对于许多自主系统（autonomous systems）来说，了解系统状态（system state）是设计任何应用的先决条件。但在现实中，系统状态往往无法直接获取（not directly obtainable）。系统状态通常是根据某些仪器（如传感器（sensors））测得的系统输出以及动力学或运动模型（a dynamic or motion model）控制的状态流推断或估计出来的。一些简单的技术，如最小二乘估计（least square estimation）或批量估计，足以解决静态或离线估计问题（static or offline estimation problems）。对于在线和实时（连续）估算（online and real time (sequential) estimation）问题，通常需要使用更复杂的估算滤波器。

估计系统由描述状态流的动态或运动模型和描述如何获得测量值的测量模型组成。在数学上，这两个模型可以用运动方程和测量方程来表示。例如，一般非线性离散估计系统的运动方程和测量方程可以写成：

$\begin{array}{c}{{x_{k+1}=f(x_{k})}}\\ {{y_{k}=h(x_{k})}}\end{array}$

其中，k 为时间步长，xk 为时间步长 k 时的系统状态，f(xk) 为与状态相关的运动方程，h(xk) 为与状态相关的测量方程，yk 为输出。

1.2 噪声分布

在大多数情况下，建立一个完美的模型来捕捉所有动态现象是不可能的。例如，在自动驾驶汽车的运动模型中包含所有摩擦是不可能的。为了弥补这些未建模的动态现象（unmodeled dynamics），通常会在动态模型中加入过程噪声（process noise） (w)。此外，在进行测量时，测量结果中不可避免地会包含校准误差（calibration errors）等多种误差源。为了考虑这些误差，必须在测量模型中加入适当的测量噪声。包含这些随机噪声和误差（random noises and errors）的估算系统称为随机估算系统（stochastic estimation system），可以用以下方式表示：

$\begin{array}{c}{{x_{k+1}=f(x_{k},w_{k})}}\\ {{y_{k}=h(x_{k},v_{k})}}\end{array}$

其中，wk 和 vk 分别代表过程噪声和测量噪声。

在大多数工程应用中，过程噪声和测量噪声被假定遵循零均值高斯分布（zero-mean Gaussian）或正态分布（normal distributions），或至少近似于高斯分布。此外，由于确切状态未知，状态估计值是一个随机变量（random variable），通常假定遵循高斯分布。假设这些变量的分布为高斯分布，可以大大简化估计滤波器的设计，并构成卡尔曼滤波器（Kalman filter）系列的基础。

随机变量 (x) 的高斯分布由均值 μ 和协方差矩阵（covariance matrix） P 参数化，可写成 x∼N(μ,P)。给定一个高斯分布，平均值也就是 x 的最可能值，由期望 (E) 定义为：

$\mu=E[x]$

平均值也称为 x 关于原点（the origin）的第一矩（first moment）。描述 x 不确定性的协方差用期望 (E) 来定义，即

$P=E[(x-\mu)(x-\mu)^{T}]$

协方差（The covariance）也称为 x 关于其均值（mean）的第二矩（second moment）。

如果 x 的维数为一，则 P 只是一个标量。在这种情况下，P 的值通常用 σ2 表示，称为方差（variance）。平方根 σ 称为 x 的标准差（standard deviation）。标准差具有重要的物理意义。例如，下图显示了均值等于 μ、标准差等于 σ 的一维高斯分布的概率密度函数（probability density function）（描述了 x 取某个值的可能性）。约 68% 的数据位于 x 的 1σ 边界内，95% 的数据位于 2σ 边界内，99.7% 的数据位于 3σ 边界内。

尽管高斯分布假设是工程应用中的主要假设，但也存在无法用高斯分布近似其状态的系统。在这种情况下，需要使用非卡尔曼滤波器（如粒子滤波器（particle filter））来准确估计系统状态。

二、滤波器设计

设计滤波器的目的是利用测量结果和系统动态来估计系统状态。由于测量通常是按离散的时间步长进行的，因此滤波过程通常分为两个步骤：

预测（Prediction）：使用动态模型在离散测量时间步（k = 1、2、3......，N）之间传播状态和协方差（Propagate state and covariance）。这一步也称为流程更新（flow update）。
校正（Correction）：利用测量结果修正离散时间步的状态估计值和协方差。这一步也称为测量更新（measurement update）。

在表示不同步骤的状态估计和协方差状态时，xk|k 和 Pk|k 表示时间步骤 k 修正后的状态估计和协方差，而 xk+1|k 和 Pk+1|k 表示从上一个时间步骤 k 到当前时间步骤 k+1 预测的状态估计和协方差。

2.1 预测

在预测步骤中，状态传播非常简单。滤波器只需将状态估计值代入动态模型，并按 xk+1|k = f(xk|k) 的方式向前传播。

协方差传播则更为复杂。如果估计系统是线性的，那么协方差就可以根据系统特性在标准方程中精确传播 (Pk|k→Pk+1|k)。对于非线性系统，精确的协方差传播具有挑战性。不同滤波器的主要区别在于它们如何传播系统协方差。例如

线性卡尔曼滤波器使用线性方程精确传播协方差。
扩展卡尔曼滤波器基于线性近似来传播协方差，当系统高度非线性时，会产生较大误差。
无迹卡尔曼滤波器使用无迹变换对协方差分布进行采样，并在时间上传播。

如何传播状态和协方差也会极大地影响滤波器的计算复杂度。举个例子：

线性卡尔曼滤波器使用线性方程精确传播协方差，通常计算效率较高。
扩展卡尔曼滤波器使用线性近似，需要计算雅各布矩阵，需要更多计算资源。
无特征卡尔曼滤波器需要对协方差分布进行采样，因此需要传播多个采样点，这对高维系统来说成本很高。

2.2 修正

在修正步骤中，滤波器通过测量反馈来修正状态估计值。基本上，真实测量值与预测测量值之间的差值在乘以反馈增益矩阵后，会被添加到状态估计值中。例如，在扩展卡尔曼滤波器中，状态估计值的修正公式为

$x_{k+1|k+1} =x_{k+1|k} +K _k(y _{k+1}-h(x_{k+1|k}))$

如前所述，xk+1|k 是（先验）修正前的状态估计值，xk+1|k+1 是（后验）修正后的状态估计值。Kk 是卡尔曼增益，受最优准则制约，yk 是真实测量值，h(xk+1|k) 是预测测量值。

在修正步骤中，滤波器还要修正估计误差协方差。其基本思想是利用 yk+1 的分布信息修正 x 的概率分布。在滤波器中，预测和修正步骤是递归处理的。流程图显示了卡尔曼滤波器的一般算法。

三、传感器融合与跟踪工具箱中的估计滤波器（Estimation Filters in Sensor Fusion and Tracking Toolbox）

传感器融合与跟踪工具箱™ 提供多种估计滤波器，可用于估计和跟踪动态系统的状态。

3.1 卡尔曼滤波器

经典卡尔曼滤波器（trackingKF）是具有高斯过程和测量噪声的线性系统的最佳滤波器。线性估计系统可表示为

$\begin{array}{c}{{x_{k+1}=A_{k}x_{k}+w_ k}}\\ {{y_{k}=H_{k}x_{k}+v_{k}}}\end{array}$

假设过程噪声和测量噪声都是高斯噪声，即

$\begin{array}{c}{{w _k\sim N(0,Q_{k})}}\\ {{v_{k}\sim N(0,R_{k})}}\end{array}$

因此，协方差矩阵可以通过线性代数方程在测量步骤之间直接传播，即

$P_{k+1|k}=A_{k}P_{k|k}A_{k}^{T}+Q_{k}$

测量更新的校正方程为

$\begin{array}{c}{{x_{k+1|k+1}=x_{k+1|k}+K_{k}(y_{k}-H_{k}x_{k+1|k})}}\\ {{P_{k+1|k+1}=(I-K_{k}H_{k})P_{k+1|k}}}\end{array}$

为了在每次更新中计算卡尔曼增益矩阵 (Kk)，滤波器需要计算一个矩阵的逆矩阵：

$K_{k}=P_{k|k-1}H_{k}^{T}\left[H_{k}P_{k|k-1}H_{k}^{T}+R_{k}\right]^{-1}$

由于转置矩阵的维数等于估计状态的维数，因此对于高维系统来说，这种计算需要一定的计算量。更多详情，请参阅线性卡尔曼滤波器。

3.2 Alpha-Beta 滤波器

Alpha-Beta 滤波器（trackingABF）是一种适用于线性系统的次优滤波器。该滤波器可视为简化的卡尔曼滤波器。在卡尔曼滤波器中，卡尔曼增益矩阵和协方差矩阵是动态计算的，每一步都会更新。然而，在 Alpha-Beta 滤波器中，这些矩阵是恒定的。这种处理方法牺牲了卡尔曼滤波器的最优性，但提高了计算效率。因此，在计算资源有限的情况下，Alpha-Beta 滤波器可能是首选。

3.3 扩展卡尔曼滤波器

最流行的扩展卡尔曼滤波器（trackingEKF）是从经典卡尔曼滤波器改进而来，以适应非线性模型。它的工作原理是将非线性系统与状态估计值线性化，并忽略二阶及更高阶的非线性项。除了卡尔曼滤波器中的 Ak 和 Hk 矩阵被 f(xk ) 和 h(xk) 的雅可比矩阵所取代外，它的公式与线性卡尔曼滤波器的公式基本相同：

$\begin{array}{c}{{A_{k}=\dfrac{\partial f(x_{k})}{\partial x_{k}}\bigg|_{x_{k|k-1}}}}\\ {{H_{k}=\dfrac{\partial h(x_{k})}{\partial x_{k}}\bigg|_{x_{k|k-1}}}}\end{array}$

如果估计系统的真实动态接近线性化动态，那么使用这种线性近似在短时间内不会产生明显误差。因此，对于更新间隔较短的轻度非线性估计系统，EKF 可以产生相对准确的状态估计。然而，由于 EKF 忽略了高阶项，因此对于高度非线性系统（例如四旋翼飞行器），特别是更新间隔较大的系统，EKF 可能会出现偏离。

与 KF 相比，EKF 需要推导雅各布矩阵，这就要求系统动力学是可微分的，还需要计算雅各布矩阵使系统线性化，这就需要更多的计算资产。

需要注意的是，对于状态以球面坐标表示的估计系统，可以使用 trackingMSCEKF。

3.4 无迹卡尔曼滤波器

无迹卡尔曼滤波器（trackingUKF）使用无迹变换（UT）来近似传播非线性模型的协方差分布。UT 方法对当前时间的协方差高斯分布进行采样，利用非线性模型传播采样点（称为 sigma 点），并通过评估这些传播的 sigma 点来近似假定为高斯分布的协方差分布。图中说明了实际传播、线性化传播和不确定性协方差的UT 传播之间的差异。

与 EKF 采用的线性化方法相比，UT 方法能更准确地传播协方差，并能进行更准确的状态估计，尤其是对于高度非线性系统。UKF 不需要推导和计算雅各矩阵。但是，UKF 需要通过非线性模型传播 2n+1 个 sigma 点，其中 n 是估计状态的维度。对于高维系统来说，计算成本可能会很高。

3.5 立方卡尔曼滤波器

立方卡尔曼滤波器（trackingCKF）采用与 UKF 稍有不同的方法生成 2n 个用于传播协方差分布的样本点，其中 n 是估计状态的维度。这种交替的样本点集通常能带来更好的统计稳定性，并避免 UKF 可能出现的发散现象，尤其是在单精度平台上运行时。请注意，当 UKF 参数设置为 α = 1、β = 0 和 κ = 0 时，CKF 本质上等同于 UKF。有关这些参数的定义，请参见 trackingUKF。

3.6 高斯求和滤波器

高斯和滤波器（TrackingGSF）使用多个高斯分布的加权和来近似估计状态的分布。估计状态由高斯状态的加权和给出：

$x_ k=\sum_{i=1}^{N}{{c}}^i_{k}{{{x}}}^i_{k}$

其中，N 是滤波器中保持的高斯状态的数量，cki 是相应高斯状态的权重，每次更新都会根据测量结果进行修改。多个高斯状态遵循相同的动态模型：

${x}_{k+1}^{{i}}=f(x_{k}^{{i}},w_{k}^{{i}}),\,\mathrm{for}\,i=1,2,\dots,N.$

该滤波器能有效估计不完全可观测估计系统的状态。例如，当只有测距数据时，滤波器可以使用多个角度参数化的扩展卡尔曼滤波器来估计系统状态。有关示例，请参阅 "仅测距跟踪"。

3.7 交互式多重模型滤波器

交互式多模型滤波器（trackingIMM）使用多个高斯滤波器来跟踪目标的位置。在高机动性系统中，系统动力学可以在多个模型（例如匀速、匀加速和匀转向）之间切换。仅使用一种运动模型来模拟目标的运动是很困难的。多模型估计系统可描述为：

$\begin{array}{l}\\ {{x^{i}_{k+1}=f_ i(x_{k}^{\ i},w_{k}^{\ i})}}\\ {{y_{k}^i=h _i(x_{k}^{i},v_{k}^i)}}\end{array}$

其中 i = 1，2，...，M，M 是动态模型的总数。IMM 过滤器通过对机动目标使用多个模型来解决目标运动的不确定性。滤波器同时处理所有模型，并将总体估计值表示为这些模型估计值的加权和，其中权重为每个模型的概率。有关示例，请参阅 "跟踪机动目标"。

3.8 粒子滤波器

粒子滤波器（trackingPF）不同于卡尔曼滤波器系列（例如 EKF 和 UKF），因为它不依赖于高斯分布假设，后者相当于使用均值和方差对不确定性进行参数化描述。取而代之的是，粒子滤波器通过时间对系统运行的加权样本（粒子）进行多次模拟，然后将这些粒子作为未知真实分布的代理进行分析。粒子滤波算法简介如图所示。

这种方法背后的动机是大数定律论证 —— 随着粒子数量的增加，粒子的经验分布会越来越接近真实分布。与各种卡尔曼滤波器相比，粒子滤波器的主要优势在于它可以应用于非高斯分布。此外，该滤波器对系统动力学没有限制，可用于高度非线性系统。另一个优点是，粒子滤波器本身就能代表关于当前状态的多种假设。由于每个粒子都代表一个具有一定相关可能性的状态假设，因此粒子滤波器在状态存在模糊性的情况下非常有用。

除了这些吸引人的特性外，粒子滤波器的计算复杂度也很高。例如，UKF 需要传播 13 个采样点来估计物体的三维位置和速度。然而，粒子滤波器可能需要数千个粒子才能获得合理的估计值。而且，实现良好估计所需的粒子数量会随着状态维度的增加而快速增长，在高维空间中会导致粒子剥夺问题。因此，粒子滤波器大多被应用于维数较低的系统（例如机器人）。

四、如何选择跟踪滤波器

下表列出了传感器融合与跟踪工具箱中所有可用的跟踪滤波器，以及在系统非线性、状态分布和计算复杂度等限制条件下如何选择这些滤波器。

滤波器名称	支持非线性模型	高斯状态	计算复杂度	评论
Alpha-Beta			Low	Suboptimal filter.次优滤波器。
Kalman		✓	Medium Low	Optimal for linear systems.线性系统的最佳方案
Extended Kalman	✓	✓	Medium	Uses linearized models to propagate uncertainty covariance.使用线性化模型传播不确定性协方差。
Unscented Kalman	✓	✓	Medium High	Samples the uncertainty covariance to propagate the sample points. May become numerically unstable in a single-precision platform.采样不确定性协方差来传播采样点。在单精度平台上可能会出现数值不稳定的情况。
Cubature Kalman	✓	✓	Medium High	Samples the uncertainty covariance to propagate the sample points. Numerically stable.采样不确定性协方差来传播采样点。数值稳定。
Gaussian-Sum	✓	✓ (Assumes a weighted sum of distributions假设分布的加权和)	High	Good for partially observable cases (angle-only tracking for example).适用于部分可观测的情况（例如只跟踪角度）。
Interacting Multiple Models (IMM)	✓ Multiple models	✓ (Assumes a weighted sum of distributions假设分布的加权和)	High	Maneuvering objects (which accelerate or turn, for example)操纵物体（例如加速或转弯的物体）
Particle	✓		Very High	Samples the uncertainty distribution using weighted particles.使用加权粒子对不确定性分布进行采样。

References

[1] Wang, E.A., and R. Van Der Merwe. "The Unscented Kalman Filter for Nonlinear Estimation." IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium. No. 00EX373, 2000, pp. 153–158.

[2] Fang, H., N. Tian, Y. Wang, M. Zhou, and M.A. Haile. "Nonlinear Bayesian Estimation: From Kalman Filtering to a Broader Horizon." IEEE/CAA Journal of Automatica Sinica. Vol. 5, Number 2, 2018, pp. 401–417.

[3] Arasaratnam, I., and S. Haykin. "Cubature Kalman Filters." IEEE Transactions on automatic control. Vol. 54, Number 6, 2009, pp. 1254–1269.

[4] Konatowski, S., P. Kaniewski, and J. Matuszewski. "Comparison of Estimation Accuracy of EKF, UKF and PF Filters." Annual of Navigation. Vol. 23, Number 1, 2016, pp. 69–87.

[5] Darko, J. "Object Tracking: Particle Filter with Ease." https://www.codeproject.com/Articles/865934/Object-Tracking-Particle-Filter-with-Ease.