最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

カルマン フィルター処理

このケース スタディでは、カルマン フィルター設計およびシミュレーションについて説明します。ここでは、定常状態カルマン フィルターおよび時変カルマン フィルターの両方について考えます。

プラント ダイナミクス

入力 u[n] に加法性のガウス ノイズ w[n] をもつ離散プラントを考えます。

x[n+1]=Ax[n]+B(u[n]+w[n])y[n]=Cx[n].

さらに、yv[n] を出力 y[n] のノイズを含む測定値とし、v[n] は測定ノイズを示すものとします。

yv[n]=y[n]+v[n].

以下の行列は、プラントのダイナミクスを表します。

A = [1.1269   -0.4940    0.1129;
     1.0000         0         0;
          0    1.0000         0];

B = [-0.3832;
      0.5919;
      0.5191];

C = [1 0 0];

離散カルマン フィルター処理

この問題の定常状態のカルマン フィルターの方程式は、次のとおりです。

  • 測定値の更新:

xˆ[n|n]=xˆ[n|n-1]+M(yv[n]-Cxˆ[n|n-1])

  • 時刻の更新:

xˆ[n+1|n]=Axˆ[n|n]+Bu[n]

この方程式では、次のようになります。

  • xˆ[n|n-1] は、yv[n-1] までの過去の測定値からの x[n] の推定値。

  • xˆ[n|n] は、最後の測定値 yv[n] に基づき更新された推定値。

現在の推定値を xˆ[n|n] とすると、時刻の更新によって、次のサンプル n + 1 (1 ステップ先の予測子) での状態値が予測されます。測定値の更新により、新しい測定値 yv[n+1] を基にこの予測が調整されます。修正項は、イノベーションの関数、つまり y[n+1] の測定値と予測値間の誤差の関数です。この誤差は次のように与えられます。

yv[n+1]-Cxˆ[n+1|n]

イノベーション ゲイン M は、ノイズ共分散に対する推定誤差の定常状態共分散を最小にするように選択します。

E(w[n]w[n]T)=QE(v[n]v[n]T)=RN=E(w[n]v[n]T)=0

時刻および測定値更新方程式を組み合わせて、1 つの状態空間モデル (カルマン フィルター) を生成できます。

xˆ[n+1|n]=A(I-MC)xˆ[n|n-1]+[BAM][u[n]yv[n]]yˆ[n|n]=C(I-MC)xˆ[n|n-1]+CMyv[n].

このフィルターにより、yn の最適推定値 yˆ[n|n] が生成されます。フィルターの状態は xˆ[n|n-1] であることに注意してください。

定常状態の設計

関数 kalman を使用して、上記の定常状態カルマン フィルターを設計できます。まず、次のプロセス ノイズを含むプラント モデルを設定します。

x[n+1]=Ax[n]+Bu[n]+Bw[n]y[n]=Cx[n]

ここで、最初の式は状態方程式、2 番目の式は測定方程式です。

次のコマンドにより、このプラント モデルを指定します。サンプル時間を指定せずにモデルを離散とマークするために、サンプル時間を -1 に設定します。

Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},'outputname','y');

Q = R = 1 と仮定して、離散カルマン フィルターを設計します。

Q = 1; 
R = 1;
[kalmf,L,P,M] = kalman(Plant,Q,R);

このコマンドは、フィルターの状態空間モデル kalmf とイノベーション ゲイン M を返します。

M
M = 3×1

    0.3798
    0.0817
   -0.2570

kalmf の入力は uyv です。出力はプラント出力と状態推定値 ye=yˆ[n|n] および xˆ[n|n] です。

ここでは出力推定値 ye に注目しているため、kalmf の最初の出力を選択して残りは破棄します。

kalmf = kalmf(1,:);

フィルターがどのように機能するかを確認するには、何らかの入力データとランダム ノイズを生成し、フィルターを適用した応答 ye を真の応答 y と比較します。それぞれの応答は、別々に生成することも、同時に生成することもできます。それぞれの応答を別々にシミュレートするには、関数 lsim をまずプラントだけで使用し、次にプラントとフィルターを一緒にして使用します。両者を同時にシミュレーションするもう 1 つの方法については、次に説明します。

次のブロック線図は、真の出力とフィルターを適用した出力を生成する方法を示します。

このブロック線図の状態空間モデルは、関数 parallel と関数 feedback を使って構成できます。まず、uwv を入力とし、yyv (測定値) を出力とする完全なプラント モデルを作成します。

a = A;
b = [B B 0*B];
c = [C;C];
d = [0 0 0;0 0 1];
P = ss(a,b,c,d,-1,'inputname',{'u' 'w' 'v'},'outputname',{'y' 'yv'});

次に parallel を使用して、次の図の並列接続を形成します。

sys = parallel(P,kalmf,1,1,[],[]);

最後に、正のフィードバックによりプラント出力 yv をフィルター入力 yv に接続してセンサー ループを閉じます。

SimModel = feedback(sys,1,4,2,1);   % Close loop around input #4 and output #2
SimModel = SimModel([1 3],[1 2 3]); % Delete yv from I/O list

結果として得られるシミュレーション モデルは、入力として wvu をもち、出力として yye をもちます。InputName プロパティおよび OutputName プロパティを表示して確認します。

SimModel.InputName
ans = 3x1 cell array
    {'w'}
    {'v'}
    {'u'}

SimModel.OutputName
ans = 2x1 cell array
    {'y'  }
    {'y_e'}

これで、フィルターの挙動をシミュレーションする準備ができました。正弦波入力 u、プロセス ノイズ ベクトル w、測定ノイズ ベクトル v を生成します。

t = [0:100]';
u = sin(t/5);

n = length(t);
rng default
w = sqrt(Q)*randn(n,1);
v = sqrt(R)*randn(n,1);

応答のシミュレーションを実行します。

[out,x] = lsim(SimModel,[w,v,u]);

y = out(:,1);   % true response
ye = out(:,2);  % filtered response
yv = y + v;     % measured response

真の応答とフィルターを適用した応答をグラフ上で比較します。

subplot(211), plot(t,y,'--',t,ye,'-'), 
xlabel('No. of samples'), ylabel('Output')
title('Kalman filter response')
subplot(212), plot(t,y-yv,'-.',t,y-ye,'-'),
xlabel('No. of samples'), ylabel('Error')

最初のプロットは、真の応答 y (破線) とフィルターを適用した出力 ye (実線) を示しています。第 2 のプロットは、測定誤差 (鎖線と点) と推定誤差 (実線) を比較しています。このプロットは、ノイズ レベルが著しく減少したことを示しています。これは、共分散誤差を計算して確認します。フィルター処理を行う前の誤差共分散 (測定誤差) は、次のとおりです。

MeasErr = y-yv;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)
MeasErrCov = 0.9992

一方、フィルター処理を行った後の誤差共分散 (推定誤差) は、次のように減少します。

EstErr = y-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov = 0.4944

時変カルマン フィルター

時変カルマン フィルターは、時変システム、または非定常ノイズ共分散を含む LTI システム用に定常状態フィルターを一般化したものです。

次のプラントの状態および測定方程式について考えます。

x[n+1]=Ax[n]+Bu[n]+Gw[n]yv[n]=Cx[n]+v[n].

時変カルマン フィルターは、次の再帰計算で与えられます。

  • 測定値の更新:

xˆ[n|n]=xˆ[n|n-1]+M[n](yv[n]-Cxˆ[n|n-1])M[n]=P[n|n-1]CT(R[n]+CP[n|n-1]CT)-1P[n|n]=(I-M[n]C)P[n|n-1].

  • 時刻の更新:

xˆ[n+1|n]=Axˆ[n|n]+Bu[n]P[n+1|n]=AP[n|n]AT+GQ[n]GT.

ここで、xˆ[n|n-1] および xˆ[n|n] は、前述のとおりです。さらに、以下が成立します。

Q[n]=E(w[n]w[n]T)R[n]=E(v[n]v[n]T)P[n|n]=E({x[n]-x[n|n]}{x[n]-x[n|n]}T)P[n|n-1]=E({x[n]-x[n|n-1]}{x[n]-x[n|n-1]}T).

簡略化するために、状態空間行列の時間依存性を示す添字を省略しました。

初期条件 x[1|0] および P[1|0] が与えられると、これらの方程式を反復してフィルター処理を実行できます。時間サンプルごとに状態推定値 x[n|.] と誤差共分散行列 P[n|.] の両方を更新しなければなりません。

時変設計

これらのフィルターの再帰処理を実装するには、まず、ノイズの加わった出力観測量を生成します。前に生成したプロセス ノイズ w と測定ノイズ v を使用します。

sys = ss(A,B,C,0,-1);
y = lsim(sys,u+w);      
yv = y + v;

次の初期条件を仮定します。

x[1|0]=0,P[1|0]=BQBT

for ループをもつ時変フィルターを実装します。

P = B*Q*B';         % Initial error covariance
x = zeros(3,1);     % Initial condition on the state
ye = zeros(length(t),1);
ycov = zeros(length(t),1); 

for i = 1:length(t)
  % Measurement update
  Mn = P*C'/(C*P*C'+R);
  x = x + Mn*(yv(i)-C*x);   % x[n|n]
  P = (eye(3)-Mn*C)*P;      % P[n|n]

  ye(i) = C*x;
  errcov(i) = C*P*C';

  % Time update
  x = A*x + B*u(i);        % x[n+1|n]
  P = A*P*A' + B*Q*B';     % P[n+1|n]
end

真の出力と推定出力をグラフ上で比較します。

subplot(211), plot(t,y,'--',t,ye,'-')
title('Time-varying Kalman filter response')
xlabel('No. of samples'), ylabel('Output')
subplot(212), plot(t,y-yv,'-.',t,y-ye,'-')
xlabel('No. of samples'), ylabel('Output')

最初のプロットは、真の応答 y (破線) とフィルターを適用した応答 ye (実線) を示しています。第 2 のプロットは、測定誤差 (鎖線と点) と推定誤差 (実線) を比較しています。

時変フィルターは、サンプルごとに推定誤差 y-ye の共分散 errcov も推定します。それをプロットして、フィルターが (定常入力ノイズで期待されるとおりに) 定常状態に達したかどうかを確認します。

subplot(211)
plot(t,errcov), ylabel('Error covar')

この共分散プロットから、出力共分散がおよそ 5 つのサンプルで実際に定常状態に達したことが確認できます。それ以降、時変フィルターは定常状態バージョンと同じ性能を維持します。

実験データから得られた推定誤差の共分散と比較します。

EstErr = y - ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov = 0.4934

この値は、理論値 errcov より小さく、定常状態の設計で得られる値に近くなります。

最後に、最終値 M[n] とイノベーション ゲイン行列の定常値 M が一致することに注意してください。

Mn
Mn = 3×1

    0.3798
    0.0817
   -0.2570

M
M = 3×1

    0.3798
    0.0817
   -0.2570

参考文献

[1] Grimble, M.J., Robust Industrial Control: Optimal Design Approach for Polynomial Systems, Prentice Hall, 1994, p. 261 and pp. 443-456.

関連するトピック