粒子过滤器工作流程

Particle Filter Workflow

粒子滤波器是一种递归贝叶斯状态估计器，它使用离散粒子来近似估计状态的后验分布。

粒子滤波算法以递归方式计算状态估计值，包括两个步骤：

预测 - 算法根据给定的系统模型，利用先前的状态来预测当前的状态。

校正 - 算法使用当前传感器测量值来校正状态估计值。

该算法还会定期重新分配或重新采样状态空间中的粒子，以匹配估计状态的后验分布。

估计状态由所有状态变量组成。每个粒子代表一个离散的状态假设。所有粒子的集合用来帮助确定最终的状态估计值。您可以将粒子滤波器应用于任意非线性系统模型。过程和测量噪声可以遵循任意的非高斯分布。

要正确使用粒子滤波器，必须指定粒子数量、初始粒子位置和状态估计方法等参数。此外，如果您有特定的运动和传感器模型，还需要分别在状态转换函数和测量似然函数中指定这些参数。更多信息，请参阅粒子过滤器参数。

按照此基本工作流程创建并使用粒子滤波器。本页详细介绍了估算工作流程，并举例说明了如何循环运行粒子滤波器以连续估算状态。

估计工作流程

使用粒子滤波器时，需要一系列步骤来创建粒子滤波器和估计状态。预测和修正步骤是连续估计状态的主要迭代步骤。

 创建粒子过滤器

创建一个 stateEstimatorPF 对象。

设置非线性系统参数

 修改这些 stateEstimatorPF 参数，以适合您的特定系统或应用：

1. 状态转换（StateTransitionFcn）
2. 测量可能性Fcn（MeasurementLikelihoodFcn）
3. 重采样策略(ResamplingPolicy)
4. 重采样方法(ResamplingMethod)
5. 状态估计方法(StateEstimationMethod)

这些参数的默认值用于基本操作。

状态转换函数（StateTransitionFcn）和测量可能性函数（MeasurementLikelihoodFcn）定义了系统行为和测量整合。它们对粒子滤波器的精确跟踪至关重要。更多信息，请参阅粒子滤波参数。

初始化粒子

使用initialize函数设置粒子数量和初始状态。

从分布图中提取粒子样本

您可以通过两种方式对初始粒子位置进行采样：

初始姿态和协方差 - 如果您对初始状态有所了解，建议您指定初始姿态和协方差。指定初始姿态和协方差有助于将粒子聚集到更接近估计值的位置，从而从一开始就能更有效地进行跟踪。

状态界限 - 如果不知道初始状态，可以指定每个状态变量的可能界限。粒子会均匀分布在每个变量的状态边界上。分布较广的粒子跟踪效果不佳，因为靠近实际状态的粒子较少。使用状态界限通常需要更多粒子、计算时间和迭代次数才能收敛到实际状态估计值。

预测

根据指定的状态转换函数，粒子会不断演化以估计下一个状态。使用预测来执行 StateTransitionFcn 属性中指定的状态转换函数。

获取测量结果

下一步将利用从传感器收集到的测量结果来修正当前的预测状态。

校正

然后利用测量结果调整预测状态并修正估计值。correct 使用 MeasurementLikelihoodFcn 来计算每个粒子的传感器测量值的可能性。随着后续迭代中状态的变化，需要对粒子进行重新采样以更新估计值。此步骤会根据重采样方法（ResamplingMethod）和重采样策略（ResamplingPolicy）属性触发重采样。

提取最佳状态估计值

调用 correct 函数后，将根据每个粒子的权重和对象中指定的状态估计方法属性自动提取最佳状态估计值。最佳状态估计值和协方差将由 correct 函数输出。

重新采样粒子

这一步不会单独调用，而是在调用 correct 时执行。一旦状态发生了足够大的变化，就会根据最新的估计值对粒子重新采样。correct 方法会检查 ResamplingPolicy（重采样策略），以根据当前的粒子分布及其权重触发粒子重采样。如果没有触发重采样，下一次估算将使用相同的粒子。如果状态变化不大或时间步长较小，可以调用预测和校正方法而不进行重采样。

持续预测和纠正

根据估计状态的需要，重复前面的预测和修正步骤。校正步骤决定是否需要对粒子重新取样。在下列情况下，可能需要多次调用预测或修正步骤

没有测量数据，但控制输入和时间更新频率很高。使用预测方法对粒子进行演化，以更频繁地获得更新的预测状态。

可提供多个测量读数。使用校正功能可整合来自同一或多个传感器的多个读数。该功能根据收集到的每组信息修正状态。

Particle Filter Parameters

要使用 stateEstimatorPF 粒子滤波器，必须指定粒子数量、初始粒子位置和状态估计方法等参数。此外，如果有特定的运动和传感器模型，还需分别在状态转换函数和测量似然函数中指定这些参数。本页将详细介绍这些参数。有关粒子滤波工作流程的更多信息，请参阅粒子滤波工作流程。

粒子数（Number of Particles）

要指定粒子的数量，请使用初始化方法。每个粒子都是对当前状态的假设。粒子根据指定的均值和协方差或指定的状态边界分布在状态空间中。根据状态估计方法（StateEstimationMethod）属性的不同，会选择权重最大的粒子或所有粒子的平均值来确定最佳状态估计值。

默认粒子数为 1000 个。除非性能有问题，否则请勿使用少于 1000 个粒子。粒子数越多，估算结果越好，但会牺牲性能速度，因为算法需要处理更多的粒子。调整粒子数是影响粒子过滤器性能的最佳方法。

这些结果基于 stateEstimatorPF 示例，显示了使用 100 个粒子和 5000 个粒子时跟踪精度的差异。

 

初始粒子位置（Initial Particle Location）

在初始化粒子过滤器时，您可以使用以下命令指定粒子的初始位置：

均值和协方差

状态界限

您的初始状态被定义为相对于系统的均值和协方差。这个均值和协方差与系统的初始位置和不确定性相关。stateEstimatorPF 对象会根据你的协方差在给定的均值周围分布粒子。算法会使用这种粒子分布来获得对状态的最佳估计，因此精确的粒子初始化有助于快速收敛到最佳状态估计。

如果初始状态未知，可以在给定的状态边界内均匀分布粒子。状态边界是状态的界限。例如，在估算机器人的位置时，状态边界仅限于机器人可以实际居住的环境。一般来说，均匀分布粒子是一种效率较低的粒子初始化方式，可以提高收敛速度。

该图显示了均值和协方差规范如何比指定完整的状态边界更有效地在空间中对粒子进行聚类。

 

状态转换函数（State Transition Function）

粒子滤波器的状态转换函数 StateTransitionFcn 有助于将粒子演化到下一个状态。它在粒子滤波工作流的预测步骤中使用。在 stateEstimatorPF 对象中，状态转换函数被指定为一个回调函数，它接收前一个粒子和任何其他必要的参数，并输出预测的位置。函数头语法如下

function predictParticles = stateTransitionFcn(pf,prevParticles,varargin)

默认情况下，状态转换函数假定采用速度恒定的高斯运动模型。该函数使用高斯分布来确定粒子在下一个时间步中的位置。

对于您的应用而言，准确描述系统行为的状态转换函数非常重要。为了准确地演化所有粒子，您必须为系统开发并实现一个运动模型。如果粒子没有围绕下一状态分布，stateEstimatorPF 对象就无法找到准确的估计值。因此，了解系统的行为方式非常重要，这样才能准确地跟踪系统。

您还必须在 StateTransitionFcn 中指定系统噪声。如果在预测系统中不应用随机噪音，粒子过滤器就无法发挥预期功能。

虽然您可以根据许多系统的上一状态对其进行预测，但有时系统可能包含额外的信息。在函数中使用 varargin 可以输入任何与预测下一状态相关的额外参数。在调用预测时，您可以使用

predict(pf,param1,param2)

由于这些参数与您定义的状态转换函数相匹配，因此调用预测功能实质上就是调用该函数：

predictParticles = stateTransitionFcn(pf,prevParticles,param1,param2)

然后，输出粒子（predictParticles）会被测量似然函数用于修正粒子，如果不需要修正，则会用于下一个预测步骤。

测量似然函数（Measurement Likelihood Function）

预测下一个状态后，可以使用传感器的测量结果来修正预测的状态。通过在 stateEstimatorPF 对象中指定 MeasurementLikelihoodFcn，就可以使用正确的函数修正预测粒子。顾名思义，这个测量似然函数是根据给定的测量值为状态假设（粒子）给出一个权重。从本质上讲，它给出的是观察到的测量结果与每个粒子观察到的实际结果相匹配的可能性。这种可能性被用作预测粒子的权重，以帮助修正粒子并获得最佳估计。虽然预测步骤可以证明少量中间步骤是准确的，但要获得准确的跟踪，需要经常使用传感器观测结果来修正粒子。

测量可能性函数（MeasurementLikelihoodFcn）的说明与状态转换函数（StateTransitionFcn）相似。它在 stateEstimatorPF 对象的属性中被指定为函数句柄。函数头语法如下

function likelihood = measurementLikelihoodFcn(pf,predictParticles,measurement,varargin)

输出结果是根据给出的测量值预测出的每个粒子的可能性。不过，您也可以在 varargin 中指定更多参数。在函数中使用 varargin 可以输入任何与修正预测状态相关的额外参数。在调用 correct 时，您可以使用

correct(pf,measurement,param1,param2)

这些参数与您定义的测量似然函数相匹配：

likelihood = measurementLikelihoodFcn(pf,predictParticles,measurement,param1,param2)

正确的函数使用似然输出进行粒子重采样，并给出最终的状态估计值。

重新取样政策（Resampling Policy）

对粒子重新采样是连续跟踪物体的重要步骤。它能让你根据当前状态选择粒子，而不是使用初始化时给出的粒子分布。通过围绕当前估计值不断对粒子进行重新采样，可以获得更精确的跟踪并提高长期性能。

当你调用 correct 时，用于状态估计的粒子可以根据在 stateEstimatorPF 对象中指定的 ResamplingPolicy 属性重新采样。该属性被指定为 aresamplingPolicyPF（导航工具箱）resamplingPolicyPF 对象。该对象上的触发方法（TriggerMethod）属性会告诉粒子过滤器使用哪种方法进行重采样。

您可以在固定间隔或达到最小有效粒子比时触发重新采样。固定间隔法在设定的迭代次数下重新采样，迭代次数在 SamplingInterval 属性中指定。最小有效粒子比是衡量当前粒子集对后验分布的近似程度。有效粒子数的计算方法是

 在这个公式中，N 是粒子的数量，w 是每个粒子的归一化重量。有效粒子比率就是 Neff / NumParticles。因此，有效粒子比率是所有粒子权重的函数。当粒子的权重达到足够低的值时，它们对状态估计就没有贡献了。这个低值会触发重新采样，因此粒子会更接近当前的状态估计值，并具有更高的权重。

状态估计法（State Estimation Method）

粒子滤波工作流程的最后一步是选择单一状态估计值。在整个分布中采样的粒子及其权重用于给出实际状态的最佳估计值。不过，你可以通过多种方式使用粒子信息来获得单一状态估计值。使用 stateEstimatorPF 对象，可以根据权重最大的粒子选择最佳估计值，也可以取所有粒子的平均值。在 StateEstimationMethod 属性中指定估计方法为 "平均值"（默认）或 "最大权重"。

由于可以通过多种方法从所有粒子中估计状态，因此也可以使用粒子属性从 stateEstimatorPF 中提取每个粒子及其权重。