Main Content

このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

stateEstimatorPF

粒子フィルターの状態推定器の作成

説明

stateEstimatorPF オブジェクトは、推定状態の事後分布を離散粒子によって近似する再帰的ベイズ状態推定器です。

粒子フィルター アルゴリズムは状態推定を再帰的に計算します。予測と補正の 2 つのステップが含まれます。予測ステップでは、与えられたシステム モデルに基づき、前の状態を使用して現在の状態を予測します。補正ステップでは、現在のセンサー測定値を使用して状態推定を補正します。アルゴリズムは状態空間内の粒子を定期的に再分布 (リサンプリング) して、推定された状態の事後分布と一致するようにします。

推定状態は状態変数で構成されます。各粒子は、これらの状態変数の離散的な状態仮説を表します。すべての粒子のセットを使用して、最終状態推定が決定されます。

粒子フィルターは、任意の非線形システム モデルに適用できます。プロセス ノイズと測定ノイズは、任意の非ガウス分布に従うことがあります。

粒子フィルターのワークフローと特定のパラメーターの設定の詳細については、次を参照してください。

作成

説明

pf = stateEstimatorPF は、3 つの状態変数を使用して単純なシステムの状態推定を可能にするオブジェクトを作成します。initialize メソッドを使用して、既知の平均および共分散、または定義済みの範囲内に一様分布する粒子を使用して粒子を初期化します。粒子フィルターのシステムおよび測定モデルをカスタマイズするには、StateTransitionFcn および MeasurementLikelihoodFcn プロパティを変更します。

オブジェクトを作成したら、initialize を使用して NumStateVariables プロパティと NumParticles プロパティを初期化します。関数 initialize は、入力に基づいてこれら 2 つのプロパティを設定します。

プロパティ

すべて展開する

この プロパティ は読み取り専用です。

状態変数の数。スカラーとして指定します。このプロパティは、initialize メソッドへの入力に基づいて設定されます。状態の数は、初期の状態と共分散に指定された行列に基づいて暗黙的に決まります。

この プロパティ は読み取り専用です。

フィルターで使用される粒子数。スカラーとして指定します。このプロパティは、initialize メソッドの呼び出しによってのみ指定できます。

粒子フィルター ステップ間の状態遷移を決定するためのコールバック関数。関数ハンドルとして指定します。状態遷移関数は、各粒子のシステム状態を変化させます。関数シグネチャは次のとおりです。

function predictParticles = stateTransitionFcn(pf,prevParticles,varargin)

コールバック関数は少なくとも 2 つの入力引数を受け入れます。stateEstimatorPF オブジェクト pf と、前のタイム ステップにおける粒子 prevParticles です。指定したこれらの粒子は、前のオブジェクト呼び出しで返された predictParticles です。predictParticlesprevParticles は同じサイズで、NumParticlesNumStateVariables 列です。

また、varargin を使用して、関数 predict の可変数の引数を渡すこともできます。次を呼び出した場合、

predict(pf,arg1,arg2)

MATLAB® は実質的に stateTranstionFcn を次のように呼び出していることになります。

stateTransitionFcn(pf,prevParticles,arg1,arg2)

センサー測定値の尤度を計算するコールバック関数。関数ハンドルとして指定します。センサー測定値が利用可能になると、このコールバック関数によって、測定値が各粒子の状態仮説と整合性がとれているかどうかの尤度が計算されます。この関数は、使用する測定モデルに基づいて実装しなければなりません。関数シグネチャは次のとおりです。

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

このコールバック関数は少なくとも 3 つの入力引数を受け入れます。

  1. pf – 関連付けられた stateEstimatorPF オブジェクト

  2. predictParticles – 現在のタイム ステップにおいて予測されるシステム状態を NumParticlesNumStateVariables 列のサイズの配列として表す粒子

  3. measurement – 現在のタイム ステップにおける状態測定値

また、varargin を使用して可変数の引数を渡すこともできます。これらの引数は関数 correct によって渡されます。次を呼び出した場合、

correct(pf,measurement,arg1,arg2)

MATLAB は実質的に measurementLikelihoodFcn を次のように呼び出していることになります。

measurementLikelihoodFcn(pf,predictParticles,measurement,arg1,arg2)

コールバックは厳密に 1 つの出力 likelihood を返す必要があります。これは、各粒子の状態仮説に対する、指定した measurement の尤度です。

状態変数が循環分布をもつかどうかのインジケーター。logical 配列として指定します。循環 (または角) 分布は範囲が [-pi,pi] の確率密度関数を使用します。オブジェクトに複数の状態変数がある場合、IsStateVariableCircular は行ベクトルです。各ベクトル要素は、関連付けられた状態変数が循環であるかどうかを示します。オブジェクトに状態変数が 1 つしかない場合、IsStateVariableCircular はスカラーです。

リサンプリングをトリガーするタイミングを決定するポリシー設定。オブジェクトとして指定します。リサンプリングは一定の間隔でトリガーするか、有効粒子の数に基づいて動的にトリガーすることができます。詳細については、resamplingPolicyPF を参照してください。

粒子のリサンプリングに使用される手法。'multinomial''residual''stratified''systematic' として指定します。

状態推定に使用される手法。'mean' および 'maxweight' として指定します。

粒子の値の配列。NumParticlesNumStateVariables 列の行列として指定します。各行は、単一の粒子の状態仮説に対応します。

粒子の重み。NumParticles 行 1 列のベクトルとして指定します。それぞれの重みは、Particles プロパティの同じ行の粒子に関連付けられています。

この プロパティ は読み取り専用です。

最良の状態推定。長さ NumStateVariables のベクトルとして返されます。推定は、StateEstimationMethod プロパティに基づいて抽出されます。

この プロパティ は読み取り専用です。

システムの補正済み共分散。N 行 N 列の行列として返されます。ここで、N は NumStateVariables プロパティに等しい値です。補正済み状態は、StateEstimationMethod プロパティと MeasurementLikelihoodFcn に基づいて計算されます。共分散がサポートされない状態推定手法を指定すると、このプロパティは [] に設定されます。

オブジェクト関数

initializeInitialize the state of the particle filter
getStateEstimateExtract best state estimate and covariance from particles
predictPredict state of robot in next time step
correctAdjust state estimate based on sensor measurement

すべて折りたたむ

stateEstimatorPF オブジェクトを作成し、状態推定のための予測と補正のステップを実行します。粒子フィルターは、StateTransitionFcn の戻り値に基づいて、予測された状態推定を与えます。その後、与えられた測定値と MeasurementLikelihoodFcn の戻り値に基づいて状態を補正します。

3 つの既定の状態をもつ粒子フィルターを作成します。

pf = stateEstimatorPF
pf = 
  stateEstimatorPF with properties:

           NumStateVariables: 3
                NumParticles: 1000
          StateTransitionFcn: @nav.algs.gaussianMotion
    MeasurementLikelihoodFcn: @nav.algs.fullStateMeasurement
     IsStateVariableCircular: [0 0 0]
            ResamplingPolicy: [1x1 resamplingPolicyPF]
            ResamplingMethod: 'multinomial'
       StateEstimationMethod: 'mean'
            StateOrientation: 'row'
                   Particles: [1000x3 double]
                     Weights: [1000x1 double]
                       State: 'Use the getStateEstimate function to see the value.'
             StateCovariance: 'Use the getStateEstimate function to see the value.'

平均状態推定法と、体系的なリサンプリング法を指定します。

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] を仮定して、予測のステップを 1 回、補正のステップを 1 回実行します。

[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 オブジェクトを使用して、2 次元空間内を移動するロボットを追跡します。測定される位置にはランダム ノイズが追加されています。predictcorrect を使用して、測定値、および仮定された運動モデルに基づいてロボットを追跡します。

粒子フィルターを初期化し、既定の状態遷移関数、測定尤度関数、リサンプリング ポリシーを指定します。

pf = stateEstimatorPF;
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';

初期位置 [0 0] と単位共分散によって 1000 個の粒子をサンプリングします。

initialize(pf,1000,[0 0],eye(2));

推定の前に、点がたどる正弦波パスを定義します。予測位置と推定位置を格納する配列を作成します。ノイズの振幅を定義します。

t = 0:0.1:4*pi;
dot = [t; sin(t)]';
robotPred = zeros(length(t),2);
robotCorrected = zeros(length(t),2);
noise = 0.1;

測定値に基づいて推定位置を予測し補正するループを開始します。ResamplingPolicy プロパティに基づいて、粒子のリサンプリングが行われます。測定値にランダム ノイズが追加された正弦波関数に基づき、ロボットが動きます。

for i = 1:length(t)
    % Predict next position. Resample particles if necessary.
    [robotPred(i,:),robotCov] = predict(pf);
    % Generate dot measurement with random noise. This is
    % equivalent to the observation step.
    measurement(i,:) = dot(i,:) + noise*(rand([1 2])-noise/2);
    % Correct position based on the given measurement to get best estimation.
    % Actual dot position is not used. Store corrected position in data array.
    [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('Actual position','Estimated position')
grid on

Figure contains an axes object. The axes object contains 2 objects of type line. One or more of the lines displays its values using only markers These objects represent Actual position, Estimated position.

次の図は、推定状態がロボットの実際の位置にどの程度近いかを示します。粒子数を調整したり、異なる初期位置と共分散を指定したりして、時間の経過に伴って追跡が受ける影響を確認してみてください。

参照

[1] Arulampalam, M.S., S. Maskell, N. Gordon, and T. Clapp. "A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking." IEEE Transactions on Signal Processing. Vol. 50, No. 2, Feb 2002, pp. 174-188.

[2] Chen, Z. "Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond." Statistics. Vol. 182, No. 1, 2003, pp. 1-69.

拡張機能

C/C++ コード生成
MATLAB® Coder™ を使用して C および C++ コードを生成します。

バージョン履歴

R2016a で導入

すべて展開する