stateEstimatorPF
创建粒子滤波状态估计量
描述
的stateEstimatorPF对象是一个递归的贝叶斯状态估计量,利用离散粒子近似估计状态的后验分布。
粒子滤波算法计算状态估计递归和涉及两个步骤:预测和修正。以前预测步骤使用状态来预测当前状态基于给定的系统模型。校正步骤使用电流传感器测量正确的状态估计。算法周期性地重新分配,或重新取样,状态空间中的粒子匹配估计状态的后验分布。
估计状态由状态变量。每个粒子代表一个离散状态假设这些状态变量。所有粒子的集合是用来帮助确定最终的状态估计。
可以将粒子滤波应用到任意的非线性系统模型。过程和测量噪声可以按照任意非高斯分布。
在粒子滤波的更多信息工作流和设置特定参数,见:
创建
属性
对象的功能
初始化 |
初始化粒子滤波的状态 |
getStateEstimate |
从粒子提取最佳状态估计和协方差 |
预测 |
预测下一个时间步的机器人 |
正确的 |
调整状态估计基于传感器测量 |
例子
粒子滤波预测和修正
创建一个stateEstimatorPF对象和执行状态估计的预测和校正步骤。粒子滤波给出了预测状态估计基于的返回值StateTransitionFcn。它纠正了国家基于给定的测量和的返回值MeasurementLikelihoodFcn。
创建一个默认的粒子滤波三种状态。
pf = stateEstimatorPF
pf = stateEstimatorPF属性:NumStateVariables: 3 NumParticles: 1000 StateTransitionFcn: @nav.algs。gaussianMotion MeasurementLikelihoodFcn: @nav.algs。fullStateMeasurement IsStateVariableCircular: [0 0 0] ResamplingPolicy: [1 x1 resamplingPolicyPF] ResamplingMethod:“多项式”StateEstimationMethod:“的意思是”StateOrientation:“行”粒子:[1000 x3双)重量:[1000 x1双]状态:“使用getStateEstimate函数值。“StateCovariance:“使用getStateEstimate函数的值。
指定的意思是状态估计方法和系统重采样方法。
pf.StateEstimationMethod =“的意思是”;pf.ResamplingMethod =“系统”;
初始化粒子滤波在状态协方差(4 1 9)与单位((3))。使用5000个粒子。
初始化(pf, 5000,(4 1 9),眼(3));
假设测量[4.2 - 0.9 9],运行一个预测和一个正确的一步。
[statePredicted, stateCov] =预测(pf);[stateCorrected, stateCov] =正确(pf, 9 [4.2 - 0.9]);
得到最好的状态估计的基础上StateEstimationMethod算法。
状态= getStateEstimate (pf)
状态=1×34.1562 0.9185 9.0202
使用粒子滤波估计机器人的位置在一个循环中
使用stateEstimatorPF对象跟踪机器人移动时在一个二维空间。测量位置添加随机噪声。使用预测和正确的、跟踪机器人基于测量和在一个假定的运动模型。
初始化粒子滤波和指定默认状态转换功能,测量似然函数和重采样策略。
pf = stateEstimatorPF;pf.StateEstimationMethod =“的意思是”;pf.ResamplingMethod =“系统”;
样品1000个粒子的初始位置[0 0]和单元协方差。
初始化(pf, 1000年,[0 0],眼(2));
估计之前,定义一个正弦波路径点。创建一个数组来存储预测和估计位置。定义噪声的振幅。
t = 0:0.1:4 *π;点= [t;罪(t)];robotPred = 0(长度(t), 2);robotCorrected = 0(长度(t), 2);噪音= 0.1;
开始循环预测基于测量和校正估计位置。重采样的粒子发生根据ResamplingPolicy财产。机器人移动基于正弦波函数与随机噪声测量。
为i = 1:长度(t)%预测下一个位置。必要时重新取样粒子。[robotPred(我,:),robotCov] =预测(pf);%生成与随机噪声点测量。这是%相当于观察步骤。测量(我:)=点(我:)+噪声*(兰德([1 - 2])噪声/ 2);%根据给定的正确位置测量得到最好的估计。%不使用实际点位置。纠正位置存储在数据数组。[robotCorrected(我,:),robotCov] =正确(pf、测量(我,:));结束
实际的路径与估计位置的阴谋。实际结果可能不同,由于粒子分布的随机性。
情节(点(:1),点(:,2),robotCorrected (: 1), robotCorrected (:, 2),”或“)xlim ([0 t(结束)])ylim([1])传说(的实际位置,“估计位置”网格)在
图显示了如何关闭状态估计与机器人的实际位置相匹配。尝试调整粒子的数量或指定一个不同的初始位置和协方差随着时间的推移,看看它如何影响跟踪。
引用
[1]Arulampalam,硕士,S. Maskell, N. Gordon, and T. Clapp. "A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking."IEEE信号处理。50卷,2号,2002年2月,页174 - 188。
[2],Z。“贝叶斯过滤:从卡尔曼粒子过滤器,过滤器,和超越。”统计数据。1号卷。182年,2003年,页1 - 69。
