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

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

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

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

loadvdp_data.maty

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

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コマンドの直前にこのコマンドを置きます。予測が測定値と一致した場合、残差はゼロです。

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

fork = 1:size(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で更新されます。

この例では、初期状態値が $$\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) gridontitle('Actual and Estimated Measurements, Residual') legend(“测量”,'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));fork=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をもつ、非線形システムについて考えます。

システムのプロセス ノイズ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);