主要内容

估计滤波器简介

背景

评估系统

对于许多自治系统，了解系统状态是设计任何应用程序的先决条件。然而，在现实中，这种状态往往不能直接获得。系统状态通常是根据某些仪器(如传感器)测量的系统输出和由动态或运动模型控制的状态流来推断或估计的。一些简单的技术，如最小二乘估计或批量估计，足以解决静态或离线估计问题。对于在线和实时(顺序)估计问题，通常应用更复杂的估计滤波器。

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

x k + 1 ＝ f （ x k ） y k ＝ h （ x k ）

在哪里k是时间步长，xk系统是时间步长的状态吗k，f（xk)为运动的状态依赖方程，h（xk)为状态依赖测量方程，且yk是输出。

噪声分布

在大多数情况下，建立一个完美的模型来捕捉所有的动态现象是不可能的。例如，在自动驾驶汽车的运动模型中包含所有的摩擦是不可能的。为了补偿这些未建模的动态，过程噪声(w)通常被添加到动态模型中。此外，在进行测量时，不可避免地会包含多种误差来源，如校准误差。为了解释这些误差，必须在测量模型中添加适当的测量噪声。包含这些随机噪声和误差的估计系统称为随机估计系统，可以表示为:

x k + 1 ＝ f （ x k ， w k ） y k ＝ h （ x k ， v k ）

在哪里wk而且vk分别表示过程噪声和测量噪声。

对于大多数工程应用，假定过程噪声和测量噪声服从零均值高斯分布或正态分布，或至少近似于高斯分布。此外，由于确切的状态是未知的，状态估计是一个随机变量，通常假设遵循高斯分布。假设这些变量为高斯分布，大大简化了估计滤波器的设计，并形成了卡尔曼滤波器族的基础。

随机变量(x)由平均值参数化μ和协方差矩阵P，写为x∼N（μ，P)．给出一个高斯分布的均值，也就是最可能的值x，由期望(E):

μ ＝ E ［ x ］

均值也被称为的第一矩x关于原点。的不确定度的协方差x由期望(E):

P ＝ E ［ （ x − μ ） （ x − μ ） T ］

协方差也叫做的二阶矩x关于它的平均值。

如果的维数x是1,P只是一个标量。在本例中，的值P通常用σ2叫做方差。平方根，σ的标准差x．标准差具有重要的物理意义。例如，下图显示了概率密度函数(它描述了…的可能性)x取某值)，对于一维高斯分布，其均值为μ标准差等于σ．大约68%的数据属于1σ边界的x， 95%的数据都在2之内σ边界，99.7%的数据属于3σ边界。

高斯分布

尽管高斯分布假设是工程应用中的主要假设，但仍有系统的状态不能用高斯分布近似。在这种情况下，需要非卡尔曼滤波器(如粒子滤波器)来准确估计系统状态。

滤波器的设计

设计滤波器的目标是利用测量和系统动力学来估计系统的状态。由于测量通常是在离散时间步长下进行的，滤波过程通常分为两个步骤:

  1. 预测:在离散测量时间步长之间传播状态和协方差(k= 1,2,3，…，N)使用动态模型。此步骤也称为流更新。

  2. 修正:使用测量在离散时间步上修正状态估计和协方差。此步骤也称为度量更新。

为表示不同步骤的状态估计和协方差状态，xk|k而且Pk|k表示在时间步改正后的状态估计和协方差k,而xk+ 1 |k而且Pk+ 1 |k表示先前时间步长的状态估计和协方差预测k到当前时间步长k+ 1。

预测

在预测步骤中，状态传播非常简单。该滤波器只需要将状态估计代入动态模型，并将其及时传播xk+ 1 |k＝f（xk|k)．

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

  • 线性卡尔曼滤波器使用线性方程来精确地传播协方差。

  • 扩展卡尔曼滤波器基于线性逼近传播协方差，当系统高度非线性时产生较大误差。

  • 无气味卡尔曼滤波器利用无气味变换对协方差分布进行采样并及时传播。

状态和协方差的传播方式也极大地影响了滤波器的计算复杂度。例如:

  • 线性卡尔曼滤波器使用线性方程来精确传播协方差，这通常是计算效率高的。

  • 扩展卡尔曼滤波器使用线性逼近，需要计算雅可比矩阵，需要更多的计算资源。

  • 无气味卡尔曼滤波器需要对协方差分布进行采样，因此需要多个样本点的传播，这对于高维系统是昂贵的。

修正

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

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)为预测测度。

在校正步骤中，滤波器还校正估计误差协方差。基本思想是修正的概率分布x的分布信息yk + 1．的后验概率密度x鉴于y．在滤波器中，预测和校正步骤是递归处理的。流程图展示了卡尔曼滤波器的一般算法。

卡尔曼滤波算法流程图

传感器融合与跟踪工具箱中的估计滤波器

传感器融合和跟踪工具箱™提供多种估计过滤器，您可以使用来估计和跟踪动态系统的状态。

卡尔曼滤波器

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

x k + 1 ＝ 一个 k x k + w k y k ＝ H k x k + v k

假设过程噪声和测量噪声均为高斯分布，即:

w k ～ N （ 0 ， 问 k ） v k ～ N （ 0 ， R k ）

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

P k + 1 | k ＝ 一个 k P k | k 一个 k T + 问 k

测量更新的修正方程为:

x k + 1 | k + 1 ＝ x k + 1 | k + K k （ y k − H k x k + 1 | k ） P k + 1 | k + 1 ＝ （ 我 − K k H k ） P k + 1 | k

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

K k ＝ P k | k − 1 H k T ［ H k P k | k − 1 H k T + R k ］ − 1

由于倒置矩阵的维数等于估计状态的维数，对于高维系统，这种计算需要一些计算工作。详情请参见线性卡尔曼滤波器．

α-β滤波器

alpha-beta过滤器(trackingABF)是应用于线性系统的次优滤波器。该滤波器可以看作是一个简化的卡尔曼滤波器。在卡尔曼滤波器中，卡尔曼增益和协方差矩阵是动态计算的，并在每一步中更新。然而，在alpha-beta过滤器中，这些矩阵是常数。这种处理牺牲了卡尔曼滤波器的最优性，但提高了计算效率。因此，当计算资源有限时，alpha-beta过滤器可能是首选。

扩展卡尔曼滤波器

最流行的扩展卡尔曼滤波器(trackingEKF)对经典卡尔曼滤波器进行了改进，以适应非线性模型。它的工作原理是对非线性系统的状态估计进行线性化，忽略二阶和更高阶非线性项。它的公式基本上与线性卡尔曼滤波器相同，除了一个k而且Hk的雅可比矩阵代替卡尔曼滤波器中的矩阵f（xk),h（xk)：

一个 k ＝ ∂ f （ x k ） ∂ x k | x k | k − 1 H k ＝ ∂ h （ x k ） ∂ x k | x k | k − 1

如果估计系统的真实动态接近线性化动态，则使用这种线性近似在短时间内不会产生显著误差。基于这个原因，EKF可以为具有较短更新间隔的轻度非线性估计系统产生相对准确的状态估计。然而，由于EKF忽略了高阶项，对于高度非线性系统(例如四旋翼系统)，特别是具有较大更新间隔的系统，它可能会发散。

与KF相比，EKF需要推导雅可比矩阵，这要求系统动力学是可微的，并且需要计算雅可比矩阵来线性化系统，这需要更多的计算资源。

注意，对于用球坐标表示状态的估计系统，您可以使用trackingMSCEKF．

无味卡尔曼滤波器

无味卡尔曼滤波器(trackingUKF)使用unscented变换(UT)来近似传播非线性模型的协方差分布。UT方法对当前时刻的协方差高斯分布进行采样，使用非线性模型传播样本点(称为西格玛点)，并通过评估这些传播的西格玛点来近似计算假设为高斯的协方差分布。该图说明了不确定性协方差的实际传播、线性化传播和UT传播之间的差异。

True、Linear和UT转换

与EKF采用的线性化方法相比，UT方法可以更准确地传播协方差，并导致更准确的状态估计，特别是对于高度非线性的系统。UKF不需要推导和计算雅可比矩阵。而UKF要求2的传播n+1 σ点通过非线性模型，其中n是估计状态的维数。对于高维系统，这可能需要大量的计算。

容积卡尔曼滤波

容积卡尔曼滤波(trackingCKF)采用与UKF略有不同的方法生成2n样本点用于传播协方差分布，其中n是估计状态的维数。这种交替的样本点集通常会产生更好的统计稳定性，并避免UKF中可能发生的发散，特别是在单精度平台上运行时。注意，当UKF参数设置为时，CKF本质上等同于UKFα= 1,β= 0，和κ= 0。看到trackingUKF对于这些参数的定义。

高斯和滤波

高斯和滤波器(trackingGSF)使用多个高斯分布的加权和来近似估计状态的分布。估计状态由高斯状态的加权和给出:

x k ＝ ∑ 我 ＝ 1 N c k 我 x k 我

在哪里N在滤波器中保持高斯状态的数量，和ck我为对应高斯状态的权值，在每次更新中都会根据测量值进行修改。多个高斯态遵循相同的动态模型:

x k + 1 我 ＝ f （ x k 我 ， w k 我 ） ， 为 我 ＝ 1 ， 2 ， .．. ， N ．

该滤波器可以有效地估计一个不完全可观测估计系统的状态。例如，当只有距离测量值可用时，滤波器可以使用多个角度参数化扩展卡尔曼滤波器来估计系统状态。看到仅距离测量跟踪举个例子。

交互式多模型滤波器

交互式多模型筛选器(trackingIMM)使用多个高斯滤波器来跟踪目标的位置。在高机动系统中，系统动力学可以在多个模型之间切换(例如恒定速度、恒定加速度和恒定转弯)。仅使用一种运动模型来模拟目标的运动是困难的。多模型估计系统可以描述为:

x k + 1 我 ＝ f 我 （ x k 我 ， w k 我 ） y k 我 ＝ h 我 （ x k 我 ， v k 我 ）

在哪里我= 1, 2，…米,米是动态模型的总数。IMM滤波器采用多个模型来解决机动目标的运动不确定性问题。过滤器同时处理所有模型，并将总体估计表示为来自这些模型的估计的加权和，其中权重是每个模型的概率。看到机动目标跟踪举个例子。

粒子滤波

粒子滤镜(trackingPF)不同于卡尔曼滤波器家族(例如EKF和UKF)，因为它不依赖于高斯分布假设，这对应于使用均值和方差对不确定性的参数描述。相反，粒子过滤器创建多个模拟系统运行的加权样本(粒子)，然后分析这些粒子作为未知真实分布的代理。简单介绍粒子滤波算法如图所示。

粒子滤波算法

这种方法背后的动机是一个大数定律的论点——当粒子的数量变大时，它们的经验分布就会接近真实分布。粒子滤波器相对于各种卡尔曼滤波器的主要优点是它可以应用于非高斯分布。此外，该滤波器对系统动力学没有限制，可用于高度非线性的系统。另一个好处是过滤器固有的能力来表示当前状态的多个假设。由于每个粒子都代表一个具有一定关联似然的状态假设，粒子滤波器在状态存在模糊性的情况下是有用的。

伴随着这些吸引人的特性的是粒子滤波器的高计算复杂度。例如，UKF需要传播13个样本点来估计物体的三维位置和速度。然而，一个粒子过滤器可能需要数千个粒子来获得一个合理的估计。此外，实现良好估计所需的粒子数量随着状态维的增长而迅速增长，并可能导致高维空间中的粒子剥夺问题。因此，粒子滤波器主要应用于维数相对较低的系统(例如机器人)。

如何选择跟踪过滤器

下表列出了传感器融合和跟踪工具箱中所有可用的跟踪滤波器，以及如何在系统非线性、状态分布和计算复杂性的约束下选择它们。

过滤器的名字 金宝app支持非线性模型 高斯状态 计算复杂度 评论
α-β 低 非最优滤波器。
卡尔曼 ✓ 介质低 最适合线性系统。
扩展卡尔曼 ✓ ✓ 媒介 使用线性化模型来传播不确定性协方差。
无味卡尔曼 ✓ ✓ 中等高 对不确定度协方差进行抽样，以传播样本点。在单精度平台中可能变得数值不稳定。
容积卡尔曼 ✓ ✓ 中等高 对不确定度协方差进行抽样，以传播样本点。数值稳定。
高斯和 ✓

✓

(假设分布的加权和)

 高 适用于部分可观测的情况(例如仅角度跟踪)。
交互多模型(IMM)

✓

多个模型

✓

(假设分布的加权和)

 高 机动物体(例如加速或转弯)
粒子 ✓ 非常高的 用加权粒子对不确定度分布进行抽样。

参考文献

[1]王，e。a。和r。范德默。非线性估计的无气味卡尔曼滤波器IEEE 2000信号处理、通信和控制自适应系统研讨会。No. 00EX373, 2000, pp. 153-158。

[2]方浩，田楠楠，王颖，周敏和海乐M.A.。非线性贝叶斯估计:从卡尔曼滤波到更广阔的视野自动化学报。Vol. 5, no . 2, 2018, pp. 401-417。

[3] Arasaratnam，我，和S.海金。“容积卡尔曼滤波器。”IEEE自动控制汇刊。Vol. 54, no . 6, 2009, pp. 1254-1269。

[4]科纳托斯基，S.卡尼耶夫斯基，J.马图舍夫斯基。EKF, UKF和PF滤波器估计精度的比较。航海年。Vol. 23, no . 1, 2016, pp. 69-87。

[5]达科，J。《对象跟踪:轻松的粒子滤波》https://www.codeproject.com/Articles/865934/Object-Tracking-Particle-Filter-with-Ease。