粒子フィルターのパラメーター

stateEstimatorPF (Robotics System Toolbox) 粒子フィルターを使用するには、粒子数、粒子の初期位置、状態推定法などのパラメーターを指定しなければなりません。また、特定の運動およびセンサーモデルがある場合、これらのパラメーターを状態遷移関数と測定尤度関数でそれぞれ指定します。これらのパラメーターの詳細については、このページで説明します。粒子フィルターワークフローの詳細については、粒子フィルターワークフローを参照してください。

粒子数

粒子数を指定するには、initialize メソッドを使用します。それぞれの粒子は現在の状態の仮説です。粒子は、指定された平均と共分散、または指定された状態の範囲のいずれかに基づいて、状態空間全体に分散します。StateEstimationMethod プロパティによって、最も重みの大きい粒子か、またはすべての粒子の平均のいずれかが取られて、最良の状態推定が決定されます。

既定の粒子数は 1000 です。パフォーマンスが問題になる場合を除き、1000 個未満の粒子を使用してはなりません。粒子数を増やすと推定が改善されますが、アルゴリズムで処理しなければならない粒子が増えるため、パフォーマンス速度が低下します。粒子フィルターのパフォーマンスに影響を与える最適な方法は、粒子数を調整することです。

次の結果は stateEstimatorPF の例に基づき、100 個の粒子と 5000 個の粒子を使用した場合の追跡精度の差異を示します。

粒子の初期位置

粒子フィルターを初期化するときに、次を使用して粒子の初期位置を指定できます。

平均と共分散
状態の範囲

初期状態は、システムを基準にした平均と共分散として定義されます。この平均と共分散は、システムの初期位置および不確かさと相関があります。stateEstimatorPF オブジェクトは、共分散に基づいて、指定した平均の周辺に粒子を分布します。このアルゴリズムは粒子の分布を使用して最適な状態の推定を取得するため、粒子を正確に初期化すると、最適な状態推定へ迅速に収束させることができます。

初期状態が未知の場合、指定した状態の範囲全体にわたって、粒子を均等に分布させることができます。状態の範囲は、状態の限界です。たとえば、ロボットの位置を推定する際に、状態の範囲はロボットが実際に存在できる環境に制限されます。一般に、粒子の均等分布は、収束速度を改善するために粒子を初期化する方法としてあまり効率的ではありません。

以下のプロットは、平均と共分散を指定すると、状態の範囲全体を指定するよりも、はるかに効率的に空間内で粒子をクラスタリングできることを示します。

状態遷移関数

粒子フィルターの状態遷移関数 StateTransitionFcn は、粒子を次の状態に変化させるのに役立ちます。これは、粒子フィルターワークフローの予測ステップで使用されます。stateEstimatorPF オブジェクト内の状態遷移関数は、前の粒子と必要なその他のパラメーターを取り、予測した位置を出力するコールバック関数として指定されます。関数ヘッダーの構文は次のとおりです。

function predictParticles = stateTransitionFcn(pf,prevParticles,varargin)

既定では、状態遷移関数は一定速度のガウス運動モデルを前提としています。この関数はガウス分布を使用して、次のタイムステップにおける粒子の位置を決定します。

アプリケーションでは、システムの想定動作を正確に記述する状態遷移関数があることが重要です。すべての粒子を正確に変化させるには、システムの運動モデルを開発して実装しなければなりません。次の状態の周辺に粒子が分布していなければ、stateEstimatorPF オブジェクトで高精度の推定が求まりません。したがって、システムを正確に追跡できるように、システムがどのように動作する可能性があるかを理解することが重要です。

また、StateTransitionFcn にはシステムノイズも指定しなければなりません。予測されたシステムにランダムなノイズが適用されていなければ、粒子フィルターは想定どおりに動作しません。

多くのシステムはその前の状態に基づいて予測できますが、システムに追加の情報が含まれている場合もあります。関数で varargin を使用すると、次の状態の予測に関連する任意の追加パラメーターを入力することができます。predict を呼び出すときに、以下を使用してこれらのパラメーターを含めることができます。

predict(pf,param1,param2)

これらのパラメーターは定義済みの状態遷移関数と一致しているため、predict を呼び出すと、実質的に関数を次のように呼び出すことになります。

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

その後、出力粒子 predictParticles が測定尤度関数で使用されて粒子が補正されるか、補正が不要な場合は次の予測ステップで使用されます。

測定尤度関数

次の状態が予測された後、センサーの測定値を使用して、予測された状態を補正できます。stateEstimatorPF オブジェクトに MeasurementLikelihoodFcn を指定することで、予測された粒子を関数 correct を使用して補正できます。定義上、測定尤度関数は、指定された測定値に基づいて状態仮説 (粒子) に重みを付けます。基本的に、この関数は観測された測定値が各粒子によって実際に観測される値と一致する尤度を与えます。この尤度は予測された粒子の重みとして、粒子を補正して最適な推定を得るために使用されます。予測ステップは少数の中間ステップについては正しいことが証明される場合もありますが、正確な追跡を得るには、センサーの観測値を使用して頻繁に粒子を補正してください。

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)

関数 correct は、粒子のリサンプリング、および最終的な状態の推定に、likelihood の出力を使用します。

リサンプリングポリシー

粒子のリサンプリングは、オブジェクトの継続的な追跡には欠かせない手順です。リサンプリングにより、初期化の際に与えられた粒子分布を使用する代わりに、現在の状態に基づいて粒子を選択することができます。現在の推定の周辺で粒子を継続的にリサンプリングすることで、追跡がより正確になり、長期的なパフォーマンスが向上します。

correct を呼び出すと、stateEstimatorPF オブジェクトに指定されている ResamplingPolicy プロパティに基づいて、状態推定に使用されている粒子をリサンプリングできます。このプロパティは、resamplingPolicyPF オブジェクトとして指定されています。このオブジェクトの TriggerMethod プロパティは、粒子フィルターに対して、リサンプリングに使用する方法を指定します。

固定間隔で、または最小有効粒子比率に達したときに、リサンプリングをトリガーできます。固定間隔法では、SamplingInterval プロパティで指定した反復回数でリサンプリングを行います。最小有効粒子比率は、現在の粒子のセットが事後分布をどの程度良好に近似しているかの測定値です。有効粒子の数は次のように計算されます。

この方程式で、N は粒子数、w は各粒子の正規化された重みです。つまり、有効粒子比率は N_eff / NumParticles です。したがって、有効粒子比率は、全粒子の重みの関数となります。粒子の重みが十分に小さい値に達した後は、状態推定に寄与しなくなります。この小さい値によってリサンプリングがトリガーされ、粒子が現在の状態推定に近づいてより大きな重みをもつようになります。

状態推定法

粒子フィルターワークフローの最後の手順は、単一の状態推定の選択です。粒子と、分布全体にわたってサンプリングされた粒子の重みを使用して、実際の状態の最良の推定が得られます。ただし、粒子の情報を使用して、複数の方法によって単一の状態推定を得ることができます。stateEstimatorPF オブジェクトを使用すると、重みが最も大きい粒子に基づいて最適な推定を選択するか、すべての粒子の平均を取ることができます。StateEstimationMethod プロパティで、推定法を 'mean' (既定) または 'maxweight' に指定します。

さまざまな方法ですべての粒子から状態を推定できるため、Particles プロパティを使用して、各粒子とその重みを stateEstimatorPF から抽出することもできます。