correct

拡張カルマン フィルター、アンセンテッド カルマン フィルター、または粒子フィルターと測定値を使用して、状態および状態推定誤差の共分散を修正します。

構文

[CorrectedState,CorrectedStateCovariance] = correct(obj,y)

[CorrectedState,CorrectedStateCovariance] = correct(obj,y,Um1,...,Umn)

説明

correct コマンドは、測定されたシステム出力を使用して、extendedKalmanFilter、unscentedKalmanFilter、または particleFilter オブジェクトの状態および状態推定誤差の共分散を更新します。拡張カルマン フィルター、アンセンテッド カルマン フィルター、または粒子フィルターを実装するには、correct コマンドと predict コマンドを一緒に使用します。現在の出力測定値が存在する場合、correct と predict を使用できます。測定値がない場合は、predict のみを使用できます。コマンドの使用順序の詳細については、predict コマンドと correct コマンドの使用を参照してください。

[CorrectedState,CorrectedStateCovariance] = correct(obj,y) は、測定された出力 y を使用して、拡張カルマン フィルター オブジェクト、アンセンテッド カルマン フィルター オブジェクト、または粒子フィルター オブジェクト obj の状態推定と状態推定誤差の共分散を修正します。

extendedKalmanFilter コマンド、unscentedKalmanFilter コマンド、または particleFilter コマンドを使用して obj を作成します。非線形システムの状態遷移関数と測定関数を obj に指定します。また、これらの関数においてプロセス ノイズ項と測定ノイズ項が加法性であるか非加法性であるかを指定します。オブジェクトの State プロパティには最新の推定状態値が格納されます。タイム ステップ k で、obj.State が $$\widehat{x}[k|k-1]$$ であると仮定します。この値は、時間 k の状態推定であり、時間 k-1 までに測定された出力を使用して推定されます。測定されたシステム出力 y[k] に correct コマンドを使用すると、ソフトウェアは修正された状態推定 $$\widehat{x}[k|k]$$ を CorrectedState 出力に返します。ここで、$$\widehat{x}[k|k]$$ は時間 k における状態推定であり、時間 k までに測定された出力を使用して推定されます。コマンドは $$\widehat{x}[k|k]$$ の状態推定誤差の共分散を CorrectedStateCovariance 出力に返します。ソフトウェアはまた、これらの修正された値をもつ obj の State プロパティと StateCovariance プロパティを更新します。

obj.MeasurementFcn で指定した測定関数 h に次のいずれかの形式が含まれている場合、この構文を使用します。

• y(k) = h(x(k)) — 加法性の測定ノイズ向け。

• y(k) = h(x(k),v(k)) — 非加法性の測定ノイズ向け。

ここで、y(k)、x(k)、および v(k) はタイム ステップ k におけるシステムの測定出力、状態、そして測定ノイズです。h への入力のみが状態および測定ノイズです。

システムの測定関数がこれらの入力を必要とする場合、[CorrectedState,CorrectedStateCovariance] = correct(obj,y,Um1,...,Umn) は追加の入力引数を指定します。複数の引数を指定できます。

測定関数 h に次のいずれかの形式がある場合は、この構文を使用します。

• y(k) = h(x(k),Um1,...,Umn) — 加法性の測定ノイズ向け。

• y(k) = h(x(k),v(k),Um1,...,Umn) — 非加法性の測定ノイズ向け。

correct コマンドはこれらの入力を測定関数に渡し、推定出力を計算します。

例

拡張カルマン フィルターを使用したオンラインでの状態の推定

拡張カルマン フィルター アルゴリズムと測定出力データを使用して、ファン デル ポール振動子の状態を推定します。振動子には 2 つの状態と 1 つの出力があります。

振動子の拡張カルマン フィルター オブジェクトを作成します。前に記述して保存した状態遷移関数 vdpStateFcn.m と測定関数 vdpMeasurementFcn.m を使用します。これらの関数は、1 と等しい非線形パラメーター $\mathrm{mu}$ を使用して、ファン デル ポール振動子への離散近似を記述します。これらの関数は、システム内の加法性プロセスと測定ノイズを仮定します。2 つの状態の初期状態の値を [1;0] と指定します。これは、時間 k-1 までのシステム出力に関する情報に基づいて、初期時間 k における状態を推定した値 $$\underset{}{\overset{ˆ}{x}}[k|k-1]$$ です。

obj = extendedKalmanFilter(@vdpStateFcn,@vdpMeasurementFcn,[1;0]);

振動子から測定出力データ y を読み込みます。この例では、説明のためにシミュレーションされた静的データを使用します。データは、vdp_data.mat ファイルに保存されています。

load vdp_data.mat y

振動子のプロセス ノイズ共分散と測定ノイズ共分散を指定します。

obj.ProcessNoise = 0.01;
obj.MeasurementNoise = 0.16;

配列を初期化して、推定の結果を取得します。

residBuf = [];
xcorBuf = [];
xpredBuf = [];

拡張カルマン フィルター アルゴリズムを実装し、correct コマンドと predict コマンドを使用して振動子の状態を推定します。最初に、時間 k における測定値を使用して $$\underset{}{\overset{ˆ}{x}}[k|k-1]$$ を修正し、$$\underset{}{\overset{ˆ}{x}}[k|k]$$ を取得します。次に、時間 k までの測定値を使用して推定されたタイム ステップ k における状態推定 $$\underset{}{\overset{ˆ}{x}}[k|k]$$ を使用して、次のタイム ステップにおける状態値 $$\underset{}{\overset{ˆ}{x}}[k+1|k]$$ を予測します。

リアルタイムのデータ測定をシミュレートするには、一度に 1 つのタイム ステップの測定データを使用します。予測値と実際の測定値の残差を計算して、フィルターの実行と収束がどの程度適切であるか評価します。残差の計算はオプションのステップです。residual を使用する場合は、correct コマンドの直前にこのコマンドを置きます。予測が測定値と一致した場合、残差はゼロです。

タイム ステップのリアルタイム コマンドを実行した後に、結果をバッファー処理して、実行が完了した後に結果をプロットできます。

for k = 1:length(y)
    [Residual,ResidualCovariance] = residual(obj,y(k));
    [CorrectedState,CorrectedStateCovariance] = correct(obj,y(k));
    [PredictedState,PredictedStateCovariance] = predict(obj);
    
     residBuf(k,:) = Residual;
     xcorBuf(k,:) = CorrectedState';
     xpredBuf(k,:) = PredictedState';
   
end

correct コマンドを使用すると、obj.State および obj.StateCovariance は、タイム ステップ k での修正済み状態値 CorrectedState と修正済み状態推定誤差の共分散値 CorrectedStateCovariance で更新されます。predict コマンドを使用すると、obj.State および obj.StateCovariance は、タイム ステップ k+1 での予測値 PredictedState と PredictedStateCovariance で更新されます。residual コマンドを使用する場合は、obj プロパティを変更しないでください。

この例では、初期状態値が $$\underset{}{\overset{ˆ}{x}}[k|k-1]$$ (つまり、時間 k-1 までのシステム出力に基づく初期時間 k での状態推定値) であったため、predict の前に correct を使用しました。初期状態の値が $$\underset{}{\overset{ˆ}{x}}[k-1|k-1]$$ (つまり、k-1 までの測定値に基づく前の時間 k-1 での値) である場合は、最初に predict コマンドを使用します。predict と correct の使用順序の詳細については、predict コマンドと correct コマンドの使用を参照してください。

修正後の値を使用して、推定状態をプロットします。

plot(xcorBuf(:,1), xcorBuf(:,2))
title('Estimated States')

実際の測定値、修正された推定測定値、および残差をプロットします。vdpMeasurementFcn の測定関数の場合、測定値が最初の状態になります。

M = [y,xcorBuf(:,1),residBuf];
plot(M)
grid on
title('Actual and Estimated Measurements, Residual')
legend('Measured','Estimated','Residual')

推定値は測定値に近接して追跡します。初期の過渡状態の後、残差は実行全体で比較的小さく維持されます。

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

ファン デル ポールの 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')

追加入力を使用した状態遷移関数と測定関数の指定

次の状態遷移方程式と測定方程式に従って状態 x と測定値 y が変化するような入力 u をもつ、非線形システムについて考えます。

$$x[k]=\sqrt{x[k-1]+u[k-1]}+w[k-1]$$

$$y[k]=x[k]+2*u[k]+v[k{]}^2$$

システムのプロセス ノイズ w は加法性で、測定ノイズ v は非加法性です。

システムの状態遷移関数と測定関数を作成します。追加入力 u を使用して関数を指定します。

f = @(x,u)(sqrt(x+u));
h = @(x,v,u)(x+2*u+v^2);

f と h は状態遷移関数と測定関数をそれぞれ保存する無名関数に対する関数ハンドルです。測定関数では、測定ノイズが非加法性であるため、v も入力として指定されます。追加入力 u の前に、v が入力として指定されることに注意してください。

指定した関数を使用して、非線形システムの状態を推定するために拡張カルマン フィルター オブジェクトを作成します。状態の初期値を 1、測定ノイズを非加法性として指定します。

obj = extendedKalmanFilter(f,h,1,'HasAdditiveMeasurementNoise',false);

測定ノイズ共分散を指定します。

obj.MeasurementNoise = 0.01;

これで、predict コマンドと correct コマンドを使用して、システムの状態を推定できます。u の値を predict と correct に渡すと、状態遷移関数と測定関数にそれぞれ渡されます。

タイム ステップ k で測定値 y[k]=0.8 と入力 u[k]=0.2 を使用して状態推定値を修正します。

correct(obj,0.8,0.2)

次のタイム ステップで u[k]=0.2 が与えられた場合の状態を予測します。

predict(obj,0.2)

予測値と測定値の誤差、つまり "残差" を取得します。

[Residual, ResidualCovariance] = residual(obj,0.8,0.2);

入力引数

obj — 拡張カルマン フィルター オブジェクト、アンセンテッド カルマン フィルター オブジェクト、または粒子フィルター オブジェクト

extendedKalmanFilter オブジェクト | unscentedKalmanFilter オブジェクト | particleFilter オブジェクト

オンライン状態推定のための拡張カルマン フィルター オブジェクト、アンセンテッド カルマン フィルター オブジェクト、または粒子フィルター オブジェクト。次のいずれかのコマンドを使用して作成されます。

• extendedKalmanFilter — 拡張カルマン フィルター アルゴリズムを使用。

• unscentedKalmanFilter — アンセンテッド カルマン フィルター アルゴリズムを使用。

• particleFilter — 粒子フィルター アルゴリズムを使用。

y — 測定されたシステム出力

ベクトル

現在のタイム ステップで測定されたシステム出力。N 要素のベクトルとして指定します。ここで、N は測定の回数です。

Um1,...,Umn — 測定関数への追加の入力引数

任意の型の入力引数

システムの測定関数への追加の入力引数。任意の型の入力引数として指定します。測定関数 h は obj の MeasurementFcn プロパティまたは MeasurementLikelihoodFcn プロパティで指定されます。この関数が状態や測定ノイズの値に加えて入力引数も必要とする場合、これらの入力を correct コマンド構文で指定します。correct コマンドは、これらの入力を測定関数または測定尤度関数に渡し、推定出力を計算します。複数の引数を指定できます。

たとえば、測定関数または測定尤度関数が状態 x に加えて、システム入力 u と現在の時間 k を使用して、推定システム出力 y を計算すると仮定します。

y(k) = h(x(k),u(k),k)

タイム ステップ k でオンライン状態推定を実行するとき、これらの追加の入力を correct コマンド構文で指定します。

[CorrectedState,CorrectedStateCovariance] = correct(obj,y,u(k),k);

出力引数

CorrectedState — 修正済み状態推定

ベクトル

サイズが M のベクトルとして返される、修正済みの状態推定。ここで、M はシステムの状態数です。obj の初期状態を列ベクトルとして指定する場合、M は列ベクトルとして返されます。そうでない場合、M は行ベクトルとして返されます。

オブジェクトの初期状態を指定する方法の詳細については、extendedKalmanFilter、unscentedKalmanFilter、および particleFilter のリファレンス ページを参照してください。

CorrectedStateCovariance — 修正済み状態推定誤差の共分散

行列

M 行 M 列の行列として返される、修正済みの状態推定誤差の共分散。ここで、M はシステムの状態の数です。

詳細

predict コマンドと correct コマンドの使用

拡張カルマン フィルター オブジェクト、アンセンテッド カルマン フィルター オブジェクト、または粒子フィルター オブジェクト obj を作成した後で、推定アルゴリズムを実装するには、correct コマンドと predict コマンドを一緒に使用します。

タイム ステップ k で、correct コマンドは、同じタイム ステップにおいて測定されたシステム出力 y[k] を使用して、状態と状態推定誤差の共分散の修正値を返します。測定関数に追加の入力引数 Um がある場合、これらの引数を correct コマンドへの入力として指定します。これらの値は、コマンドによって測定関数に渡されます。

[CorrectedState,CorrectedCovariance] = correct(obj,y,Um)

correct コマンドは、オブジェクトの State プロパティと StateCovariance プロパティを推定値 CorrectedState と CorrectedCovariance で更新します。

predict コマンドは、次のタイム ステップで状態と状態推定誤差の共分散の予測結果を返します。状態遷移関数に追加の入力引数 Us がある場合、これらの引数を predict コマンドへの入力として指定します。これらの値は、コマンドによって状態遷移関数に渡されます。

[PredictedState,PredictedCovariance] = predict(obj,Us)

predict コマンドは、オブジェクトの State プロパティと StateCovariance プロパティを予測値の PredictedState と PredictedCovariance で更新します。

現在の出力測定値が所定のタイム ステップに存在する場合は、correct と predict を使用できます。測定値がない場合は、predict のみを使用できます。これらのコマンドによるアルゴリズムの実装方法の詳細については、オンライン状態推定のための拡張カルマン フィルター アルゴリズムおよびアンセンテッド カルマン フィルター アルゴリズムを参照してください。

コマンドを実装する順序は、システムの測定データ y、Us および Um を使用できるかどうかによって変わります。

• correct の次に predict — タイム ステップ k において obj.State の値は $$\widehat{x}[k|k-1]$$ であると仮定します。この値は、時間 k-1 までに測定された出力を使用して推定された時間 k におけるシステムの状態です。また、同じタイム ステップで測定された出力 y[k] と入力 Us[k] および Um[k] もあります。

  この場合、測定されたシステム データ y[k] と追加入力 Um[k] を指定して correct コマンドを最初に実行します。このコマンドは obj.State の値を $$\widehat{x}[k|k]$$ に更新します。これは、時間 k までに測定された出力を使用して推定された時間 k の状態推定です。これで、入力 Us[k] を指定して predict コマンドを実行すると、obj.State に $$\widehat{x}[k+1|k]$$ が保存されます。アルゴリズムはこの状態値を次のタイム ステップにおける correct コマンドへの入力として使用します。

• predict の次に correct — タイム ステップ k において obj.State の値は $$\widehat{x}[k-1|k-1]$$ であると仮定します。同じタイム ステップで測定された出力 y[k] と入力 Um[k] もありますが、前回のタイム ステップからの Us[k-1] があります。

  その場合、入力 Us[k-1] を指定して predict コマンドを最初に実行します。このコマンドは obj.State の値を $$\widehat{x}[k|k-1]$$ に更新します。次に入力引数 y[k] および Um[k] を指定して correct コマンドを実行すると、obj.State は $$\widehat{x}[k|k]$$ で更新されます。アルゴリズムはこの状態値を次のタイム ステップにおける predict コマンドへの入力として使用します。

したがって、いずれの場合も、時間 k の状態推定 $$\widehat{x}[k|k]$$ は同じですが、時間 k において現在の状態遷移の関数入力 Us[k] にアクセスできず、代わりに Us[k-1] がある場合は、predict を最初に使用してから correct を使用します。

predict コマンドと correct コマンドを使用して状態を推定する例については、拡張カルマン フィルターを使用したオンラインでの状態の推定または粒子フィルターを使用したオンラインでの状態の推定を参照してください。

バージョン履歴

R2016b で導入

参考

predict | clone | extendedKalmanFilter | unscentedKalmanFilter | particleFilter | initialize | residual

トピック

• アンセンテッド カルマン フィルターおよび粒子フィルターを使用した非線形の状態推定
• MATLAB でのオンライン状態推定のコードの生成
• What Is Online Estimation?
• オンライン状態推定のための拡張カルマン フィルター アルゴリズムおよびアンセンテッド カルマン フィルター アルゴリズム