stateEstimatorPF
创建粒子滤波状态估计器
stateEstimatorPF 对象是一个递归贝叶斯状态估计器，它使用离散粒子来近似估计状态的后验分布。
粒子滤波算法以递归方式计算状态估计值，包括两个步骤：预测和修正。预测步骤使用先前的状态，根据给定的系统模型预测当前的状态。修正步骤使用当前的传感器测量值来修正状态估计值。该算法会定期重新分配或重新采样状态空间中的粒子，以匹配估计状态的后验分布。
估计状态由状态变量组成。每个粒子代表这些状态变量的离散状态假设。所有粒子的集合用于帮助确定最终的状态估计值。
您可以将粒子滤波器应用于任意非线性系统模型。过程和测量噪声可以遵循任意的非高斯分布。
有关粒子滤波工作流程和特定参数设置的更多信息，请参阅：
粒子滤波工作流程
粒子滤波参数
语法
描述
pf=stateEstimatorPF创建一个对象，用于为具有三个状态变量的简单系统进行状态估计。使用initialize方法初始化具有已知平均值和协方差的粒子或定义边界内均匀分布的粒子。要自定义粒子过滤器的系统和测量模型，请修改StateTransitionFcn和MeasurementLikelihoodFcn属性。
创建对象后，请使用initialize初始化NumStateVariables和NumParticles属性。initialize函数根据您的输入设置这两个属性。
属性
NumStateVariables - 状态变量数
3（默认） | 标量
此属性为只读。
状态变量的数量，以标量形式指定。该属性根据初始化方法的输入设置。状态数根据指定的初始状态矩阵和协方差矩阵隐含计算。
NumParticles - 过滤器中使用的粒子数
1000（默认值） | 标量
此属性为只读。
过滤器中使用的粒子数，以标量形式指定。只能通过调用初始化方法来指定该属性。
StateTransitionFcn - 用于确定粒子滤波步骤之间状态转换的回调函数
函数句柄
用于确定粒子滤波步骤之间状态转换的回调函数，以函数句柄的形式指定。状态转换函数为每个粒子演化系统状态。函数签名为
function predictParticles = stateTransitionFcn(pf,prevParticles,varargin)
回调函数至少接受两个输入参数：stateEstimatorPF 对象 pf 和上一时间步的粒子 prevParticles。这些指定的粒子是上一次调用该对象时返回的 predictParticles，predictParticles 和 prevParticles 的大小相同： NumParticles 乘以 NumStateVariables。
您也可以使用 varargin 从 predict 函数中传入数量可变的参数。当你调用
predict(pf,arg1,arg2)
MATLAB® 基本上是这样调用 stateTranstionFcn 的：
stateTransitionFcn(pf,prevParticles,arg1,arg2)
MeasurementLikelihoodFcn - 计算传感器测量值可能性的回调函数
函数句柄
计算传感器测量值可能性的回调函数，以函数句柄形式指定。一旦有了传感器测量值，该回调函数就会计算测量值与每个粒子的状态假设一致的可能性。您必须根据自己的测量模型来实现该函数。函数签名为
function likelihood = measurementLikelihoodFcn(PF,predictParticles,measurement,varargin)
回调函数至少接受三个输入参数：
1.pf - 相关的 stateEstimatorPF 对象
2.predictParticles - 表示当前时间步长下预测系统状态的粒子，大小为 NumParticles-by-NumStateVariables 的数组
3.measurement - 当前时间步的状态测量值
您还可以使用 varargin 来传递可变数量的参数。这些参数将由正确的函数传递。当你调用
correct(pf,measurement,arg1,arg2)
MATLAB 基本上是这样调用 measurementLikelihoodFcn 的：
measurementLikelihoodFcn(pf,predictParticles,measurement,arg1,arg2)
回调需要准确返回一个输出，即 likelihood，这是每个粒子状态假设的给定测量值的可能性。
IsStateVariableCircular - 状态变量是否呈环形分布的指示符
逻辑数组
状态变量是否呈环形分布的指标，以逻辑数组形式指定。圆形（或角度）分布使用范围为 [-pi,pi] 的概率密度函数。如果对象有多个状态变量，则 IsStateVariableCircular 是一个行向量。每个向量元素都表示相关的状态变量是否为圆形。如果对象只有一个状态变量，则 IsStateVariableCircular 是一个标量。
ResamplingPolicy - 决定何时触发重采样的策略设置
对象
决定何时触发重采样的策略设置，以对象的形式指定。您可以按固定的时间间隔触发重采样，也可以根据有效粒子的数量动态触发重采样。请参阅 resamplingPolicyPF 获取更多信息。
ResamplingMethod - 用于粒子重采样的方法
多项式(multinomial)"（默认） | "残差(residual)" | "分层(stratified)" | "系统性(systematic)
用于粒子重采样的方法，可指定为 "multinomial"、"residual"、"stratified "和 "systematic"。
StateEstimationMethod - 用于状态估计的方法
平均值"（默认） | "最大权重
用于状态估计的方法，指定为 "平均值 "和 "最大权重"。
Particles - 粒子值数组
粒子数乘状态变量矩阵
粒子值数组，指定为 NumParticles-by-NumStateVariables 矩阵。每行对应一个粒子的状态假设。
weights- 粒子权重
逐-1 个粒子数向量
粒子权重，指定为 NumParticles-by-1 向量。每个权重都与粒子属性中同一行的粒子相关联。
State - 最佳状态估计值
矢量
此属性为只读。
最佳状态估计值，以长度为 NumStateVariables 的向量形式返回。估计值根据状态估计方法属性提取。
State Covariance - 修正系统协方差
N 乘 N 矩阵 | []
此属性为只读。
修正后的系统方差，以 N-by-N 矩阵形式返回，其中 N 等于 NumStateVariables 属性。校正后的状态是根据状态估计方法（StateEstimationMethod）属性和测量可能性（MeasurementLikelihoodFcn）计算得出的。如果指定的状态估计方法不支持协方差，则该属性将设置为[]。
例子
粒子滤波预测与校正
% 创建 stateEstimatorPF 对象，并执行状态估计的预测和修正步骤。
% 粒子滤波器根据 StateTransitionFcn 的返回值给出预测的状态估计值。
% 然后根据给定的测量值和 MeasurementLikelihoodFcn 的返回值修正状态。
%
% 使用默认的三个状态创建粒子滤波器。
pf = stateEstimatorPF
% 指定平均状态估计方法和系统重采样方法
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';
% 在状态 [4 1 9] 处以单位协方差（eye(3)）初始化粒子滤波器。
% 使用 5000 个粒子。
initialize(pf,5000,[4 1 9],eye(3));
% 假设测量结果为 [4.2 0.9 9]，运行一个预测步骤和一个正确步骤。
[statePredicted,stateCov] = predict(pf);
[stateCorrected,stateCov] = correct(pf,[4.2 0.9 9]);
% 根据 StateEstimationMethod 算法获取最佳状态估计值。
stateEst = getStateEstimate(pf)
stateEst = 1×3
4.1562 0.9185 9.0202
使用粒子过滤器在循环中估算机器人位置
% 使用 stateEstimatorPF 对象跟踪机器人在二维空间中的移动。
% 测量到的位置带有随机噪声。
% 使用预测和修正功能，根据测量结果和假定的运动模型跟踪机器人。
% 初始化粒子过滤器，并指定默认状态转换函数、测量似然函数和重采样策略。
pf = stateEstimatorPF;
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';
% 取样 1000 个粒子，初始位置为 [0 0]，单位协方差。
initialize(pf,100,[0 0],eye(2));
% 在估算之前，先定义一条正弦波路径，让小点跟随。
% 创建一个数组来存储预测位置和估计位置。定义噪声的振幅。
t = 0:0.1:10*pi;
dot = [t; sin(t)]';
robotPred = zeros(length(t),2);
robotCorrected = zeros(length(t),2);
noise = 0.1;%定义噪声的振幅。
% 开始循环，根据测量结果预测和修正估计位置。
% 根据 ResamplingPolicy 属性对粒子进行重新采样。
% 机器人根据正弦波函数移动，并在测量中加入随机噪声。
for i = 1:length(t)
% 预测下一个位置。必要时对粒子重新采样。
[robotPred(i,:),robotCov] = predict(pf);
% 用随机噪声生成点测量。这相当于观测步骤。
measurement(i,:) = dot(i,:) + noise*(rand([1 2])-noise/2);
% 根据给定的测量值修正位置，以获得最佳估计值。
%不使用实际点位置。将修正后的位置存储到数据数组中。
[robotCorrected(i,:),robotCov] = correct(pf,measurement(i,:));
end
% 绘制实际路径与估计位置的对比图。
% 由于粒子分布的随机性，实际结果可能会有所不同。
plot(dot(:,1),dot(:,2),robotCorrected(:,1),robotCorrected(:,2),'or')
xlim([0 t(end)])
ylim([-1 1])
legend('实际位置','估计位置')
grid on
图中显示了估计状态与机器人实际位置的匹配程度。试着调整粒子数量或指定不同的初始位置和协方差，看看对随时间推移的跟踪有何影响。
initialize
语法
initialize(pf,numParticles,mean,covariance)
initialize(pf,numParticles,stateBounds)
描述
initialize(pf,numParticles,mean,covariance)（初始化粒子滤波器对象 pf）。粒子在状态空间中的初始状态是通过从具有指定均值和协方差的多元正态分布中采样确定的。
initialize(pf,numParticles,stateBounds)通过在指定的状态边界内从多元均匀分布中采样来确定粒子的初始位置。
initialize(___,Name,Value) 用一个或多个 Name,Value 对参数指定的附加选项初始化粒子。
例子
输入参数
pf - StateEstimatorPF 对象
句柄
以句柄形式指定的 stateEstimatorPF 对象。更多信息请参阅 stateEstimatorPF。
numParticles - 过滤器中使用的粒子数
标量
过滤器中使用的粒子数，以标量形式指定。
mean - 粒子分布的平均值
矢量
粒子分布的平均值，以向量形式指定。pf 的 NumStateVariables 属性会根据该向量的长度进行设置。
covariance - 粒子分布的协方差
N 乘 N 矩阵
粒子分布的协方差，指定为 N 乘 N 矩阵，其中 N 是 pf.NumStateVariables 属性的值。
stateBounds - 状态变量的边界
n-by-2 矩阵
以 n×2 矩阵形式指定的状态变量边界。pf 的 NumStateVariables 属性根据 n 的值进行设置。每一行对应相应状态变量的下限和上限。
CircularVariables - 循环变量
逻辑向量
以逻辑向量形式指定的循环变量。该向量的长度等于 pf 的 NumStateVariables 属性。
predict
预测机器人下一时间步的状态
语法
[statePred,stateCov] = predict(pf)
[statePred,stateCov] = predict(pf,varargin)
描述
[statePred，stateCov]=predict（pf）计算预测的系统状态及其相关的不确定性协方差。predict使用stateEstimatorPF对象pf的StateTransitionFcn属性来演化所有粒子的状态。然后，它根据StateEstimationMethod属性中的设置提取最佳状态估计和协方差。
[statePred,stateCov]=predict(pf,varargin)会将 varargin 中指定的所有附加参数传递给 pf 的底层状态转换因子属性。StateTransitionFcn 的第一个输入是上一个时间步的粒子集，然后是 varargin 中的所有参数。
例子
输入参数
pf - StateEstimatorPF 对象
句柄
以句柄形式指定的 stateEstimatorPF 对象。更多信息请参阅 stateEstimatorPF。
varargin - 长度可变的输入参数列表
逗号分隔列表
长度可变的输入参数列表，以逗号分隔。该输入将直接传入 pf 的 StateTransitionFcn 属性，以生成每个粒子的系统状态。当你调用
predict(pf,arg1,arg2)
MATLAB® 基本上是这样调用 stateTranstionFcn 的：
stateTransitionFcn(pf,prevParticles,arg1,arg2)
输出参数
statePred - 预测的系统状态
向量
预测的系统状态，以长度为 NumStateVariables 的向量形式返回。预测状态根据 StateEstimationMethod 算法计算得出。
stateCov - 修正的系统协方差
N 乘 N 矩阵
修正后的系统方差，以 N-by-N 矩阵形式返回，其中 N 是 pf 中 NumStateVariables 属性的值。修正后的状态是根据状态估计方法（StateEstimationMethod）算法和测量似然 Fcn 计算得出的。如果指定的状态估计方法不支持协方差，则函数会将 stateCov 返回为[]。
correct
根据传感器测量结果调整状态估计值
语法
[stateCorr,stateCov] = correct(pf,measurement)
[stateCorr,stateCov] = correct(pf,measurement,varargin)
描述
[stateCorr,stateCov] = correct(pf,measurement) 根据当前时间步的传感器测量值计算修正后的系统状态及其相关的不确定性协方差。MeasurementLikelihoodFcn 函数的两个输入为
1.pf - stateEstimatorPF 对象，其中包含当前迭代的粒子
2.measurement - 用于修正状态估计的传感器测量值
然后，MeasurementLikelihoodFcn 函数会根据状态估计方法属性中的设置提取最佳状态估计值和协方差。
[stateCorr,stateCov] = correct(pf,measurement,varargin)在前三个必要输入之后，将 varargin 中的所有附加参数传递给底层的 MeasurementLikelihoodFcn。
例子
输入参数
pf - StateEstimatorPF 对象
句柄
以句柄形式指定的 stateEstimatorPF 对象。更多信息请参阅 stateEstimatorPF。
measurement - 传感器测量
数组
以数组形式指定的传感器测量值。此输入将直接传入 pf 的 MeasurementLikelihoodFcn 属性。它用于计算每个粒子的传感器测量值的可能性。
varargin - 长度可变的输入参数列表
逗号分隔列表
长度可变的输入参数列表，以逗号分隔。此输入将直接传入 pf 的 MeasurementLikelihoodFcn 属性。它用于计算每个粒子的传感器测量概率。当您调用
correct(pf,measurement,arg1,arg2)
MATLAB® 基本上会调用 measurementLikelihoodFcn 如下：
measurementLikelihoodFcn(pf,measurement,arg1,arg2)
输出参数
stateCorr - 经修正的系统状态
长度为 NumStateVariables 的向量
修正后的系统状态，以行向量形式返回，长度为 NumStateVariables。校正后的状态是根据状态估计方法（StateEstimationMethod）算法和测量可能性（MeasurementLikelihoodFcn）计算得出的。
stateCov - 修正的系统协方差
N 乘 N 矩阵
修正后的系统方差，以 N-by-N 矩阵形式返回，其中 N 是 pf 中 NumStateVariables 属性的值。修正后的状态是根据状态估计方法（StateEstimationMethod）算法和测量似然 Fcn 计算得出的。如果指定的状态估计方法不支持协方差，则函数会将 stateCov 返回为[]。
getStateEstimate
从粒子中提取最佳状态估计值和协方差
语法
stateEst = getStateEstimate(pf)
[stateEst,stateCov] = getStateEstimate(pf)
描述
stateEst = getStateEstimate(pf) 返回基于当前粒子集的最佳状态估计值。估计值是根据 stateEstimatorPF 对象 pf 的 StateEstimationMethod 属性提取的。
例子
输入参数
pf - StateEstimatorPF 对象
句柄
以句柄形式指定的 stateEstimatorPF 对象。更多信息请参阅 stateEstimatorPF。
[stateEst,stateCov] = getStateEstimate(pf) 也会返回状态估计值的协方差。协方差是状态估计值不确定性的度量。并非所有状态估计方法都支持协方差输出。在这种情况下，getStateEstimate 会以 [] 的形式返回 stateCov。
输出参数
stateEst - 最佳状态估计值
矢量
最佳状态估计值，以行向量形式返回，长度为 NumStateVariables。该估计值是根据 pf.NumStateVariables 中指定的 StateEstimationMethod 算法提取的。
stateCov - 修正的系统协方差
N 乘 N 矩阵
修正后的系统方差，以 N-by-N 矩阵形式返回，其中 N 是 pf 中 NumStateVariables 属性的值。修正后的状态是根据状态估计方法（StateEstimationMethod）算法和测量似然 Fcn 计算得出的。如果指定的状态估计方法不支持协方差，则函数会将 stateCov 返回为[]。
扫一扫
5万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
