particleFilter

オンライン状態推定のための粒子フィルター オブジェクト

説明

粒子フィルターは、離散粒子を使用して推定状態の事後分布を近似する再帰的なベイズ状態推定器です。測定値および、モデルの状態を測定値に関係付けるシステム モデルが利用可能な場合のオンライン状態推定に役立ちます。粒子フィルター アルゴリズムは、状態推定値を再帰的に計算するものであり、初期化、予測、補正のステップを含みます。

particleFilter は、離散時間粒子フィルター アルゴリズムを使用する離散時間非線形システムのオンライン状態推定用オブジェクトを作成します。

状態 x、入力 u、出力 m、プロセス ノイズ w および測定値 y をもつプラントについて考えます。プラントを非線形システムとして表現できると仮定します。

アルゴリズムは、指定した状態遷移関数と測定尤度関数を使用して非線形システムの状態推定 $$\widehat{x}$$ を計算します。

ソフトウェアは、任意のプロセスおよび測定ノイズ分布をもつ、任意の非線形状態遷移および測定モデルをサポートします。

オンライン状態推定を実行するには、非線形の状態遷移関数と測定尤度関数を作成します。その後、これらの非線形関数を使用して particleFilter オブジェクトを作成します。オブジェクトを作成したら、以下を行います。

1. initialize コマンドを使用して粒子を初期化します。

2. predict コマンドを使用して次のステップでの状態推定値を予測します。

3. correct コマンドを使用して状態推定値を修正します。

予測ステップでは、最新の状態を使用し、指定した状態遷移モデルに基づいて次の状態を予測します。修正ステップでは、現在のセンサー測定値を使用して状態推定値を修正します。アルゴリズムは必要に応じて状態空間内の粒子を推定状態の事後分布に一致するよう再分布、つまりリサンプリングします。各粒子は、これらの状態変数の離散状態仮説を表します。状態推定値の判定にはすべての粒子のセットが使用されます。

作成

構文

pf = particleFilter(StateTransitionFcn,MeasurementLikelihoodFcn)

オブジェクトの説明

pf = particleFilter(StateTransitionFcn,MeasurementLikelihoodFcn) は、離散時間非線形システムのオンライン状態推定のための粒子フィルター オブジェクトを作成します。StateTransitionFcn は、与えられたタイム ステップの状態ベクトルで、次のタイム ステップでの粒子 (状態仮説) を計算する関数です。MeasurementLikelihoodFcn は、センサー測定値に基づいて各粒子の尤度を計算する関数です。

オブジェクトの作成後、initialize コマンドを使用して、既知の平均と共分散または定義された範囲内で一様分布する粒子により、粒子を初期化します。その後、correct コマンドと predict コマンドを使用して、センサー測定値で粒子 (状態推定値) を更新します。

入力引数

StateTransitionFcn — 状態遷移関数

関数ハンドル

状態遷移関数。関数ハンドルとして指定します。これはタイム ステップ間の粒子 (状態仮説) の遷移を決定します。particleFilter オブジェクトのプロパティでもあります。詳細については、プロパティを参照してください。

MeasurementLikelihoodFcn — 測定尤度関数

関数ハンドル

測定尤度関数。関数ハンドルとして指定します。これはセンサー測定値から粒子 (状態仮説) の尤度を計算するために使用されます。particleFilter オブジェクトのプロパティでもあります。詳細については、プロパティを参照してください。

プロパティ

NumStateVariables — 状態変数の数

[ ] (既定値) | スカラー

状態変数の数。スカラーとして指定します。このプロパティは読み取り専用で、initialize を使って設定します。状態の数は、粒子 (つまり状態範囲) の初期平均に指定された行列に基づいて暗黙的に決まります。

NumParticles — フィルターで使用される粒子の数

[ ] (既定値) | スカラー

フィルターで使用される粒子の数。スカラーとして指定します。各粒子が 1 つの状態仮説を表します。このプロパティの指定には initialize を使用する必要があります。

StateTransitionFcn — 状態遷移関数

関数ハンドル

状態遷移関数。関数ハンドルとして指定します。これはタイム ステップ間の粒子 (状態仮説) の遷移を決定します。この関数は、あるタイム ステップ粒子を所与として、プロセス ノイズを含む次のタイム ステップでの粒子を計算します。

これとは対照的に、extendedKalmanFilter および unscentedKalmanFilter の状態遷移関数は、与えられたタイム ステップにおける状態推定値を 1 つ生成します。

非線形システムの状態遷移関数を記述して保存し、particleFilter オブジェクトの作成時に関数ハンドルとして指定します。たとえば、vdpParticleFilterStateFcn.m が状態遷移関数の場合、StateTransitionFcn を @vdpParticleFilterStateFcn として指定します。StateTransitionFcn を無名関数への関数ハンドルとして指定することもできます。

関数シグネチャは次のようになります。

function predictedParticles = myStateTransitionFcn(previousParticles,varargin)

関数 StateTransitionFcn は少なくとも 1 つの入力引数を受け入れます。最初の引数は粒子のセット previousParticles で、前のタイム ステップでの状態仮説を表します。オプションとして関数で varargin を使用すると、以下のように predict を使って次の状態の予測に関連する追加のパラメーターを入力できます。

predict(pf,arg1,arg2)

StateOrientation が 'column' の場合、previousParticles は NumStateVariables 行 NumParticles 列の配列となります。StateOrientation が 'row' の場合、previousParticles は NumParticles 行 NumStateVariables 列の配列となります。

StateTransitionFcn は厳密に 1 つの出力 predictedParticles を返さなければなりません。これは、現在のタイム ステップでの予測された粒子位置のセット (previousParticles と同じ次元の配列) です。

StateTransitionFcn は、predictedParticles に (アプリケーションに適した任意の分布からの) ランダムなプロセス ノイズを含めなければなりません。

StateOrientation プロパティが 'column' に設定された状態遷移関数の例を確認するには、コマンド ラインで edit vdpParticleFilterStateFcn を入力します。

MeasurementLikelihoodFcn — 測定尤度関数

関数ハンドル

測定尤度関数。関数ハンドルとして指定し、センサー測定値で粒子 (状態仮説) の尤度を計算するために使用されます。各状態仮説 (粒子) に対して、関数はまず N 要素の測定仮説ベクトルを計算します。次に、センサー測定値および測定ノイズの確率分布に基づいて各測定仮説の尤度が計算されます。

これとは対照的に、extendedKalmanFilter と unscentedKalmanFilter の測定関数は、1 つの状態仮説を受け取り、1 つの測定推定値を返します。

測定モデルに基づいて測定尤度関数を記述して保存し、それを使用してオブジェクトを作成します。たとえば、vdpMeasurementLikelihoodFcn.m が測定尤度関数の場合、MeasurementLikelihoodFcn を @vdpMeasurementLikelihoodFcn として指定します。MeasurementLikelihoodFcn を無名関数への関数ハンドルとして指定することもできます。

関数シグネチャは次のようになります。

function likelihood = myMeasurementLikelihoodFcn(predictedParticles,measurement,varargin)

関数 MeasurementLikelihoodFcn は少なくとも 2 つの入力引数を受け入れます。最初の引数は、予測された状態仮説を表す粒子のセット predictedParticles です。StateOrientation が 'column' の場合、predictedParticles は NumStateVariables 行 NumParticles 列の配列となります。StateOrientation が 'row' の場合、predictedParticles は NumParticles 行 NumStateVariables 列の配列となります。2 番目の引数 measurement は、現在のタイム ステップでの N 要素のセンサー測定値です。varargin を使用して追加の入力引数を指定できます。

MeasurementLikelihoodFcn は厳密に 1 つの出力 likelihood を返さなければなりません。これは長さが NumParticles のベクトルで、各粒子 (状態仮説) について与えられた measurement の尤度です。

測定尤度関数の例を確認するには、コマンド ラインで「edit vdpMeasurementLikelihoodFcn」を入力します。

IsStateVariableCircular — 状態変数が循環分布をもつかどうか

[ ] (既定値) | logical 配列

状態変数が循環分布をもつかどうか。logical 配列として指定します。

これは読み取り専用のプロパティで、initialize を使用して設定します。

循環 (または角) 分布は範囲が [-pi,pi] の確率密度関数を使用します。IsStateVariableCircular は NumStateVariables の要素をもつ行ベクトルです。各ベクトル要素は、関連する状態変数が循環であるかどうかを示します。

ResamplingPolicy — リサンプリングをトリガーするタイミングを決定するポリシー設定

particleResamplingPolicy オブジェクト

リサンプリングをトリガーするタイミングを決定するポリシー設定。particleResamplingPolicy オブジェクトとして指定します。

粒子のリサンプリングは、粒子フィルターを使用した状態推定には欠かせないステップです。これにより、初期化時に与えられた粒子分布を使用する代わりに、現在の状態に基づいて粒子を選択できるようになります。現在の推定値の周りで粒子のリサンプリングを継続的に行うことにより、追跡の精度と長期的な性能が改善されます。

リサンプリングは一定の間隔でトリガーするか、有効粒子の数に基づいて動的にトリガーすることができます。最小有効粒子比率は、現在の粒子セットが事後分布をどれだけよく近似しているかを示す尺度です。有効粒子の数は次のように計算されます。

$$N_{eff}=\frac{1}{{\displaystyle \sum _{i=1}^N{\left(w^i\right)}^2}}$$

この方程式で N は粒子の数、w は各粒子の正規化された重みです。よって有効粒子比率は Neff / NumParticles となります。したがって、有効粒子比率はすべての粒子の重みの関数です。重みが十分に低い値まで下がった粒子は、状態推定に寄与しません。この低値によってリサンプリングがトリガーされるため、粒子が現在の状態推定値により近く、重みが大きくなります。

particleResamplingPolicy オブジェクトの以下のプロパティを変更して、リサンプリングが発生するタイミングを制御できます。

TriggerMethod — リサンプリングが発生するタイミングを決定する手法

'ratio' (既定値) | 'interval'

選択した値に基づいてリサンプリングが発生するタイミングを決定する手法です。'interval' の値を指定すると、粒子フィルター動作の一定のタイム ステップ間隔でリサンプリングをトリガーします。'ratio' の値を指定すると、合計有効粒子の比率に基づいてリサンプリングをトリガーします。

SamplingInterval — リサンプリング間の固定間隔

1 (既定値) | 正のスカラー

リサンプリング間の固定間隔。スカラーとして指定します。この間隔により、どの修正ステップでリサンプリングが実行されるかが決まります。たとえば、2 の値を指定すると、修正ステップ 1 つおきにリサンプリングが実行されます。inf の値を指定すると、リサンプリングが実行されることはありません。

このプロパティは、TriggerMethod が 'interval' に設定されている場合にのみ適用されます。

MinEffectiveParticleRatio — 合計粒子数に対する有効粒子数の最小比率

0.5 (既定値) | 正のスカラー

合計粒子数 NumParticles に対する有効粒子数の最小目標比率です。有効粒子数は、現在の粒子セットが事後分布をどれだけよく近似しているかを示す尺度です。有効粒子比率が低い場合、推定に寄与している粒子の数が少なく、リサンプリングが必要であることを意味します。

合計粒子数 NumParticles に対する有効粒子数の比率が MinEffectiveParticleRatio より低い場合、リサンプリング ステップがトリガーされます。

ResamplingMethod — 粒子のリサンプリングに使用される手法

'multinomial' (既定値) | 'residual' | 'stratified' | 'systematic'

粒子のリサンプリングに使用される手法。次のいずれかとして指定します。

• 'multinomial' — 多項リサンプリング (簡易無作為抽出とも呼ばれる) は、開区間 (0,1) で一様分布とは別に N 個の乱数を生成し、それらを使用して重みに比例した粒子を選択します。

• 'residual' — 残差リサンプリングは 2 つの段階で構成されています。1 つ目の段階は、重みが 1/N よりも大きい各粒子の確定的な複製です。2 つ目の段階は、重みの残余 (残余としてラベル付けされた) を使用する無作為抽出で構成されます。

• 'stratified' — 層化リサンプリングは、粒子の母集団全体を階層と呼ばれる部分集合に分割します。(0,1) 区間を、サイズが 1/N の N 個の重ならない部分区間に事前分割します。これらの各部分区間と階層で選択されたサンプルのインデックスとは別に乱数が取得されます。

• 'systematic' — 体系的リサンプリングも階層を使用するという点で、層化リサンプリングと似ています。1 つの違いは、開区間 (0,1/N) から 1 つの乱数のみを取得し、残りのサンプル点は固定 1/N ステップ サイズで確定的に計算される点です。

StateEstimationMethod — 粒子から状態推定値を抽出するのに使用される手法

'mean' (既定値) | 'maxweight'

粒子から状態推定値を抽出するのに使用される手法。次のいずれかとして指定します。

• 'mean' - オブジェクトは Weights プロパティと Particles プロパティに基づいて、粒子の加重平均を状態推定値として出力します。

• 'maxweight' - オブジェクトは重みが最も大きい粒子を状態推定値として出力します。

Particles — 粒子値の配列

[ ] (既定値) | 配列

粒子値の配列。次のように StateOrientation プロパティに基づく配列として指定します。

• StateOrientation が 'row' の場合、Particles は NumParticles 行 NumStateVariables 列の配列となります。

• StateOrientation が 'column' の場合、Particles は NumStateVariables 行 NumParticles 列の配列となります。

各行または各列が 1 つの状態仮説 (1 つの粒子) に対応します。

Weights — 粒子の重み

[ ] (既定値) | ベクトル

粒子の重み。次のように StateOrientation プロパティの値に基づくベクトルとして定義します。

• StateOrientation が 'row' の場合、Weights は NumParticles 行 1 列のベクトルとなり、各重みが Particles プロパティの同じ行にある粒子に関連付けられます。

• StateOrientation が 'column' の場合、Weights は 1 行 NumParticles 列のベクトルとなり、各重みが Particles プロパティの同じ列にある粒子に関連付けられます。

State — 現在の状態推定値

[ ] (既定値) | ベクトル

現在の状態推定値。次のように StateOrientation プロパティの値に基づくベクトルとして定義します。

• StateOrientation が 'row' の場合、State は 1 行 NumStateVariables 列のベクトルです。

• StateOrientation が 'column' の場合、State は NumStateVariables 行 1 列のベクトルです。

State は読み取り専用のプロパティで、StateEstimationMethod プロパティに基づき Particles から導出されます。State の値の決定方法の詳細については、StateEstimationMethodを参照してください。

State と StateCovariance は、getStateEstimate を使用して求めることもできます。

StateCovariance — 状態推定誤差の共分散の現在の推定値

NumStateVariables 行 NumStateVariables 列の配列 (既定値) | [ ] | 配列

状態推定誤差の共分散の現在の推定値。NumStateVariables 行 NumStateVariables 列の配列として定義します。StateCovariance は読み取り専用のプロパティで、StateEstimationMethod に基づいて計算されます。共分散がサポートされない状態推定法を指定した場合、関数は StateCovariance を [ ] として返します。

StateCovariance と State は、getStateEstimate を使用して一緒に求めることができます。

オブジェクト関数

initialize	粒子フィルターの状態を初期化
predict	拡張カルマン フィルター、アンセンテッド カルマン フィルター、または粒子フィルターを使用した次のタイム ステップにおける状態および状態推定誤差の共分散の予測
correct	拡張カルマン フィルター、アンセンテッド カルマン フィルター、または粒子フィルターと測定値を使用して、状態および状態推定誤差の共分散を修正します。
getStateEstimate	粒子から最良の状態推定値と共分散を抽出
clone	オンライン状態推定オブジェクトをコピー

例

オンライン状態推定のための粒子フィルター オブジェクトの作成

システムの状態を推定するための粒子フィルター オブジェクトを作成するには、システムに適切な状態遷移関数と測定尤度関数を作成します。

この例では、関数 vdpParticleFilterStateFcn が、1 と等しい非線形性パラメーター mu を使用して、ファン デル ポール振動子に対する離散時間近似を記述します。また、ガウス プロセス ノイズのモデル化も行います。関数 vdpMeasurementLikelihood は、ガウス分布の測定ノイズを仮定して、最初の状態のノイズを含む測定値から粒子の尤度を計算します。

粒子フィルター オブジェクトを作成します。関数ハンドルを使用して、オブジェクトに状態遷移関数と測定尤度関数を提供します。

myPF = particleFilter(@vdpParticleFilterStateFcn,@vdpMeasurementLikelihoodFcn);

作成したオブジェクトから状態と状態推定誤差の共分散を初期化して推定するには、initialize、predict、および correct の各コマンドを使用します。

Copyright 2012 The MathWorks, Inc..

粒子フィルターを使用したオンラインでの状態の推定

ファン デル ポールの ODE データを読み込み、サンプル時間を指定します。

vdpODEdata.mat には、非線形パラメーター mu=1 で、ode45 を使用し、初期条件 [2;0] をもつファン デル ポール ODE のシミュレーションが含まれています。実際の状態はサンプル時間 dt = 0.05 で抽出されたものです。

load ('vdpODEdata.mat','xTrue','dt')
tSpan = 0:dt:5;

測定値を取得します。この例では、センサーが、標準偏差 0.04 のガウス ノイズをもつ最初の状態を測定します。

sqrtR = 0.04;
yMeas = xTrue(:,1) + sqrtR*randn(numel(tSpan),1);

粒子フィルターを作成して、状態遷移関数と測定尤度関数を設定します。

myPF = particleFilter(@vdpParticleFilterStateFcn,@vdpMeasurementLikelihoodFcn);

粒子フィルターを状態 [2; 0] で単位共分散を使って初期化し、1000 個の粒子を使用します。

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

mean 状態推定法と systematic リサンプリング法を選択します。

myPF.StateEstimationMethod = 'mean';
myPF.ResamplingMethod = 'systematic';

correct コマンドと predict コマンドを使用して状態を推定し、推定された状態を保存します。

xEst = zeros(size(xTrue));
for k=1:size(xTrue,1)
    xEst(k,:) = correct(myPF,yMeas(k));
    predict(myPF);
end

結果をプロットして、推定された状態と実際の状態を比較します。

figure(1)
plot(xTrue(:,1),xTrue(:,2),'x',xEst(:,1),xEst(:,2),'ro')
legend('True','Estimated')

参照

[1] T. Li, M. Bolic, P.M. Djuric, "Resampling Methods for Particle Filtering: Classification, implementation, and strategies," IEEE Signal Processing Magazine, vol. 32, no. 3, pp. 70-86, May 2015.

拡張機能

C/C++ コード生成

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

使用に関するメモと制限:

詳細については、MATLAB でのオンライン状態推定のコードの生成を参照してください。

MATLAB Function ブロックのサポート: なし

バージョン履歴

R2017b で導入

参考

関数

• initialize | predict | correct | clone | unscentedKalmanFilter | extendedKalmanFilter

トピック

• アンセンテッド カルマン フィルターおよび粒子フィルターを使用した非線形の状態推定
• MATLAB でのオンライン状態推定のコードの生成
• オンライン状態推定のコマンド ラインでの検証
• オンライン状態推定のトラブルシューティング

外部の Web サイト

• Understanding Kalman Filters: Nonlinear State Estimators — MATLAB ビデオ シリーズ