Particle Filter ブロックを使用した Simulink でのパラメーターおよび状態の推定

この例では次を使用します。

この例は、Control System Toolbox™ の Particle Filter ブロックの使用方法を示します。離散時間伝達関数のパラメーター推定問題を再定式化し、状態の推定問題として再帰的に解きます。

はじめに

Control System Toolbox には非線形の状態推定を行うための Simulink ブロックが 3 つあります。

Particle Filter: 離散時間の粒子フィルターアルゴリズムを実装します。
Extended Kalman Filter: 1 次離散時間の拡張カルマンフィルターアルゴリズムを実装します。
Unscented Kalman Filter: 離散時間のアンセンテッドカルマンフィルターアルゴリズムを実装します。

これらのブロックは、さまざまなサンプルレートで動作する複数のセンサーを使用した状態推定をサポートします。これらのブロックの使用に関する一般的なワークフローは次のとおりです。

MATLAB または Simulink 関数を使用してプラントとセンサーの動作をモデル化します。
ブロックのパラメーターを構成します。
フィルターをシミュレートして結果を解析し、フィルターの性能の信頼度を得ます。
ハードウェアにフィルターを展開します。Simulink Coder™ ソフトウェアを使用して、これらのフィルターのコードを生成できます。

この例では Particle Filter ブロックを使用し、このワークフローの最初の 2 つの手順を示します。最後の 2 つの手順については「次のステップ」の節で簡単に説明します。この例の目標は、離散時間伝達関数 (出力誤差モデル) のパラメーターを再帰的に推定することです。ここでは新しい情報が着信すると各タイムステップでモデルのパラメーターが更新されます。

拡張カルマンフィルターに関心がある場合、「複数のマルチレートセンサーをもつ非線形システムの状態の推定」の例を参照してください。アンセンテッドカルマンフィルターの使用手順は拡張カルマンフィルターの場合と似ています。

プラントのモデル化

大半の状態推定アルゴリズムは、タイムステップ間におけるプラントの状態の変化を記述するプラントモデル (状態遷移関数) に依存します。この関数は通常、で表されます。ここで x は状態、w はプロセスノイズ、u はたとえばシステム入力やパラメーターといったオプションの追加入力です。粒子フィルターブロックには、この関数を少し異なる構文で $X[k+1]=f_{pf}(X[k],u[k])$ のように指定しなければなりません。違いは以下のとおりです。

粒子フィルターは多くの状態仮説 (粒子) の軌跡を追うことによって機能し、ブロックはすべての状態仮説を一度に関数に渡します。具体的には、状態ベクトルに個の要素があり粒子の使用を選択した場合、は次元 $[N_s \; N_p]$ をもち、その各列が状態仮説になります。
関数 $f_{pf}(...)$ 内の状態仮説に対するプロセスノイズの影響を計算します。ブロックでは、プロセスノイズの確率分布についての仮定が行われず、入力としてを必要としません。

関数 $f_{pf}(...)$ は MATLAB Coder™ の制限に準拠する MATLAB 関数か、Simulink Function ブロックにすることができます。 $f_{pf}(...)$ を作成した後、Particle Filter ブロック内に関数名を指定します。

この例では、離散時間伝達関数のパラメーター推定問題を、状態の推定問題として定式化し直します。この伝達関数は、離散時間プロセスのダイナミクスを表現するか、ゼロ次ホールドなど何らかの連続時間ダイナミクスに信号リコンストラクターを連動させたものを表現することができます。次の 1 次離散時間の伝達関数のパラメーター推定に関心があると仮定します。

$y[k] = \frac{20q^{-1}}{1-0.7q^{-1}} u[k] + e[k]$

ここではプラント出力、はプラント入力、は測定ノイズ、 $q^{-1}$ は $q^{-1}u[k]=u[t-1]$ が成り立つむだ時間演算子です。伝達関数を $\frac{n\;q^{-1}}{1+d\;q^{-1}}$ としてパラメーター化します。ここでとは推定するパラメーターです。伝達関数とパラメーターは、状態ベクトルの選択によって、必要な状態空間形式で複数の方法により表現することができます。1 つの選択肢として $x[k]=[\; y[k]; d[k]; n[k] \;]$ があり、ここで 2 番目と 3 番目の状態がパラメーターの推定値を表します。したがって、伝達関数を同じく次のように記述することができます。

$x[k+1] = \left[
 \begin{array}{c}
 -x_2[k] x_1[k] + x_3[k] u[k] \\
 x_2[k] \\
 x_3[k] \\
 \end{array}
 \right]$

測定ノイズ項はセンサーモデリングで処理されます。この例では上記の式を、計算の効率性のためにベクトル化された形式を用いて MATLAB 関数で実装します。

type pfBlockStateTransitionFcnExample

function xNext = pfBlockStateTransitionFcnExample(x,u)
% pfBlockStateTransitionFcnExample State transition function for particle
%                                  filter, for estimating parameters of a 
%                                  SISO, first order, discrete-time transfer 
%                                  function model
%
% Inputs:
%    x - Particles, a NumberOfStates-by-NumberOfParticles matrix
%    u - System input, a scalar 
%
% Outputs:
%    xNext - Predicted particles, with the same dimensions as input x

% Implement the state-transition function
%    xNext = [x(1)*x(2) + x(3)*u; 
%             x(2); 
%             x(3)];
% in vectorized form (for all particles).
xNext = x;
xNext(1,:) = bsxfun(@times,x(1,:),-x(2,:)) + x(3,:)*u;

% Add a small process noise (relative to expected size of each state), to
% increase particle diversity
xNext = xNext + bsxfun(@times,[1; 1e-2; 1e-1],randn(size(xNext)));
end

センサーモデリング

Particle Filter ブロックには、各状態仮説の尤度 (確率) を計算する測定尤度関数を指定しなければなりません。この関数は $L[k] = h_{pf}(X[k],y[k],u[k])$ の形式をとります。は要素のベクトルで、は選択する粒子の数です。内の $m^{th}$ 要素は、内の $m^{th}$ 粒子 (列) の尤度です。はセンサー測定値です。はオプションの入力引数で、これは状態遷移関数の入力と異なるものを指定できます。

センサーはこの例の最初の状態を測定します。この例では、実際の測定値と予測された測定値の誤差がガウス分布に従うと仮定していますが、尤度の計算には任意の確率分布またはその他の方法を使用できます。 $h_{pf}(...)$ を作成し、Particle Filter ブロック内に関数名を指定します。

type pfBlockMeasurementLikelihoodFcnExample

function likelihood = pfBlockMeasurementLikelihoodFcnExample(particles, measurement)
% pfBlockMeasurementLikelihoodFcnExample Measurement likelihood function for particle filter
%
% The measurement is the first state
%
% Inputs:
%    particles   - A NumberOfStates-by-NumberOfParticles matrix
%    measurement - System output, a scalar
%
% Outputs:
%    likelihood - A vector with NumberOfParticles elements whose n-th
%                 element is the likelihood of the n-th particle

%#codegen

% Predicted measurement
yHat = particles(1,:);

% Calculate likelihood of each particle based on the error between actual
% and predicted measurement
%
% Assume error is distributed per multivariate normal distribution with
% mean value of zero, variance 1. Evaluate the corresponding probability
% density function
e = bsxfun(@minus, yHat, measurement(:)'); % Error
numberOfMeasurements = 1;
mu = 0; % Mean
Sigma = eye(numberOfMeasurements); % Variance
measurementErrorProd = dot((e-mu), Sigma \ (e-mu), 1);
c = 1/sqrt((2*pi)^numberOfMeasurements * det(Sigma));
likelihood = c * exp(-0.5 * measurementErrorProd);
end

フィルター構築

Particle Filter ブロックを推定用に構成します。状態遷移関数と測定尤度関数の名前、粒子の数およびその初期分布を指定します。

ブロックダイアログの [システムモデル] タブで、次のパラメーターを指定します。

状態遷移

[関数] に状態遷移関数 pfBlockStateTransitionFcnExample を指定します。関数名を入力して [適用] をクリックすると、ブロックは関数に追加の入力があることを検出して入力端子 "StateTransitionFcnInputs" を作成します。この端子にシステム入力を接続します。

初期化

[粒子数] に 10000 を指定します。通常は粒子の数が多いほど優れた推定が得られますが、計算コストは増加します。
[分布] に Gaussian を指定して、多変量ガウス分布から粒子の初期セットを取得します。その後、[平均] に [0; 0; 0] を指定します。これは、3 つの状態があり、これが最適な推定であるためです。[共分散] に diag([10 5 100]) を指定して、3 番目の状態の推定の分散 (不確かさ) が大きく、最初の 2 つは分散が小さいことを指定します。この粒子の初期セットは、可能な真の状態をカバーするのに十分な幅に広がっている (分散が大きい) ことが重要です。

測定 1

[関数] に、測定尤度関数の名前 pfBlockMeasurementLikelihoodFcnExample を指定します。

サンプル時間

ブロックダイアログの下部にある [サンプル時間] に 1 を入力します。状態遷移関数と測定尤度関数の間にサンプル時間の相違がみられる場合や、サンプル時間の異なる複数のセンサーを使用する場合、これらを [ブロック出力、マルチレート] タブで構成できます。

粒子フィルターは、尤度の低い粒子を削除し、尤度の高いものを使って新しい粒子をシードします。これは [リサンプリング] グループの下のオプションによって制御されます。この例では既定の設定を使用します。

既定では、ブロックは状態仮説の平均値をその尤度で加重したものだけを出力します。すべての粒子や重みを表示したり、状態推定の別の抽出方法を選択するには、[ブロック出力、マルチレート] タブのオプションを使用します。

シミュレーションと結果

簡単なテストとして、ホワイトノイズ入力をもつ真のプラントモデルをシミュレートします。プラントからの入力およびノイズを含む測定値が Particle Filter ブロックに渡されます。次の Simulink モデルはこのセットアップを表します。

open_system('pfBlockExample')

システムをシミュレートして、推定されたパラメーターと実際のパラメーターを比較します。

sim('pfBlockExample')
open_system('pfBlockExample/Parameter Estimates')

プロットには分子と分母の実際のパラメーターと、その粒子フィルター推定値が表示されています。推定値は 10 タイムステップ後に真値にほぼ収束します。初期状態推定が真値から遠い場合でも収束が得られます。

トラブルシューティング

ここではアプリケーションで粒子フィルターが予想どおりに機能しない場合に備えて、潜在的な実装の問題とトラブルシューティングのアイデアをいくつか示します。

通常、粒子フィルターのトラブルシューティングを行う場合、粒子とその重みのセットを調査します。これらを取得するには、ブロックダイアログの [ブロック出力、マルチレート] タブにある [すべての粒子を出力] と [Output all weights] を選択します。

最初の正常性チェックとして、状態遷移関数と測定尤度関数がシステムの動作を適切に捉えていることを確認します。システムのシミュレーションモデルがある (よってシミュレーション内の実際の状態にアクセスできる) 場合、実際の状態でフィルターを初期化してみることができます。その後、状態遷移関数によって実際の状態の時間伝播が正確に計算され、また測定尤度関数によってこれらの粒子で高い尤度が計算されるかどうかを検証できます。

粒子の初期セットは重要です。シミュレーションの開始時に少なくとも一部の粒子が高い尤度をもつことを確認してください。実際の状態が状態仮説の初期の広がりの外にある場合、推定値が不正確になったり発散したりする可能性があります。

状態推定の精度が最初は良好であるが、時間とともに劣化する場合、粒子が縮退したり粒子の精度が悪化したりする問題が起きている可能性があります [2]。粒子の縮退は粒子の分布が広すぎる場合に発生し、粒子の精度悪化はリサンプリングの後に粒子が塊を形成するために発生します。粒子の縮退は、直接リサンプリングの結果、粒子の精度悪化につながります。この例で使用されている状態遷移関数内での人工的なプロセスノイズの追加は、実用的なアプローチの 1 つです。このような問題の解決方法については多くの文献が公開されており、アプリケーションによってはさらに体系的な方法を利用できる可能性もあります。[1]、[2] は有用な参考資料です。

次のステップ

状態推定の検証: シミュレーションでフィルターから予想どおりの性能が得られるようになったら、通常は幅広いモンテカルロ法シミュレーションを使用してこの性能をさらに検証します。詳細については、Simulink でのオンライン状態推定の検証を参照してください。Particle Filter ブロックダイアログの [ランダム性] グループのオプションを使用してこれらのシミュレーションを実行できます。
コードの生成: Particle Filter ブロックは、Simulink Coder™ ソフトウェアを使用した C および C++ コードの生成をサポートします。このブロックに提供する関数は、MATLAB Coder™ ソフトウェア (MATLAB 関数を使用してシステムをモデル化している場合) および Simulink Coder ソフトウェア (Simulink Function ブロックを使用してシステムをモデル化している場合) の制限に準拠しなければなりません。

まとめ

この例では、Control System Toolbox の Particle Filter ブロックを使用する方法を示しました。ここでは離散時間の伝達関数のパラメーターを再帰的に推定し、新しい情報の着信に合わせて各タイムステップでパラメーターを更新しました。

close_system('pfBlockExample', 0)

参考文献

[1] Simon, Dan. Optimal state estimation: Kalman, H infinity, and nonlinear approaches. John Wiley & Sons, 2006.

[2] Doucet, Arnaud, and Adam M. Johansen. "A tutorial on particle filtering and smoothing: Fifteen years later." Handbook of nonlinear filtering 12.656-704 (2009): 3.

参考

Extended Kalman Filter | Unscented Kalman Filter | Particle Filter