ドキュメンテーション

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

カルマン フィルター設計

この例では、カルマン フィルター処理を実行する方法を示します。以下では定常状態フィルターと時変フィルターの両方を設計し、シミュレートします。

問題の定義

次の離散プラントがあるとします。

ここで

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];

D = 0;

カルマン フィルターを設計し、ノイズの測定量 yv[n] = C x[n] + v[n] に基づいて出力 y を推定します。

定常状態カルマン フィルターの設計

関数 kalman を使用すると、定常状態カルマン フィルターを設計できます。この関数は、プロセス ノイズの共分散 Q とセンサー ノイズの共分散 R に基づいて、最適な定常状態フィルターのゲイン M を判定します。

まず、プラント + ノイズ モデルを指定します。注意: サンプル時間を -1 に設定してプラントを離散系としてマークします。

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

プロセス ノイズの共分散 (Q) を指定します。

Q = 2.3; % A number greater than zero

センサー ノイズの共分散 (R) を指定します。

R = 1; % A number greater than zero

次に、以下の方程式を使用して、定常状態カルマン フィルターを設計します。

   Time update:         x[n+1|n] = Ax[n|n-1] + Bu[n]
   Measurement update:  x[n|n] = x[n|n-1] + M (yv[n] - Cx[n|n-1])
                        where M = optimal innovation gain
using the KALMAN command:
[kalmf,L,~,M,Z] = kalman(Plant,Q,R);

カルマン フィルター KALMF の最初の出力はプラントの出力推定値 y_e = Cx[n|n] で、残りの出力は状態の推定値です。最初の出力 y_e のみを保持します。

kalmf = kalmf(1,:);

M,   % innovation gain
M =

    0.5345
    0.0101
   -0.4776

このフィルターの機能を確認するために、データを生成し、フィルターを適用した応答を真のプラント応答と比較します。

上記のシステムをシミュレートするには、各部分の応答を個別に生成するか、両方を一緒に生成できます。各部分を個別にシミュレートするには、まずプラントに LSIM を使用してからフィルターを使用します。次の例では、両方を一緒にシミュレートします。

% First, build a complete plant model with u,w,v as inputs and
% y and yv as outputs:
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'});

次に、u を共有入力として指定して、プラント モデルとカルマン フィルターを並列接続します。

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

最後に、プラント出力 yv をフィルター入力 yv に接続します。メモ: yv は SYS の 4 番目の入力で、その 2 番目の出力でもあります。

SimModel = feedback(sys,1,4,2,1);
SimModel = SimModel([1 3],[1 2 3]);     % Delete yv form I/O

結果として得られるシミュレーション モデルは w、v、u を入力とし、y, y_e を出力とします。

SimModel.inputname
ans =

  3×1 cell array

    'w'
    'v'
    'u'

SimModel.outputname
ans =

  2×1 cell array

    'y'
    'y_e'

これで、フィルターの動作をシミュレートする準備ができました。正弦波入力ベクトル (既知) を生成します。

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

プロセス ノイズ ベクトルとセンサー ノイズ ベクトルを生成します。

rng(10,'twister');
w = sqrt(Q)*randn(length(t),1);
v = sqrt(R)*randn(length(t),1);

次に、LSIM を使用して応答をシミュレートします。

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

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

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

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

2 番目のプロットに示すように、カルマン フィルターは測定ノイズによる誤差 y-yv を低減します。これを確認するために、誤差の共分散を比較します。

MeasErr = y-yv;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr);
EstErr = y-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr);

フィルター適用前の誤差の共分散 (測定誤差):

MeasErrCov
MeasErrCov =

    0.9871

フィルター適用後の誤差の共分散 (推定誤差):

EstErrCov
EstErrCov =

    0.3479

時変カルマン フィルターの設計

次に、時変カルマン フィルターを設計し、同じタスクを実行します。時変カルマン フィルターは、ノイズの共分散が定常的でないときでも良好に動作します。ただし、この例では定常共分散を使用します。

時変カルマン フィルターには次の更新方程式があります。

   Time update:        x[n+1|n] = Ax[n|n] + Bu[n]
                       P[n+1|n] = AP[n|n]A' + B*Q*B'
   Measurement update:
                       x[n|n] = x[n|n-1] + M[n](yv[n] - Cx[n|n-1])
                                                         -1
                       M[n] = P[n|n-1] C' (CP[n|n-1]C'+R)
                       P[n|n] = (I-M[n]C) P[n|n-1]

まず、ノイズのあるプラント応答を生成します。

sys = ss(A,B,C,D,-1);
y = lsim(sys,u+w);   % w = process noise
yv = y + v;          % v = meas. noise

次に、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);
errcov = 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,'b',t,ye,'r--'),
xlabel('No. of samples'), ylabel('Output')
title('Response with time-varying Kalman filter')
subplot(212), plot(t,y-yv,'g',t,y-ye,'r--'),
xlabel('No. of samples'), ylabel('Error')

時変フィルターは、推定中に出力共分散も推定します。フィルターが定常状態に達している (定常入力ノイズで期待されるとおり) かどうかを確認するために、出力共分散をプロットします。

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

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

共分散誤差の比較:

MeasErr = y-yv;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr);
EstErr = y-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr);

フィルター処理前の誤差の共分散 (測定誤差):

MeasErrCov
MeasErrCov =

    0.9871

フィルター処理後の誤差の共分散 (推定誤差):

EstErrCov
EstErrCov =

    0.3479

カルマン ゲイン行列の定常状態値と最終値が一致することを確認します。

M,Mn
M =

    0.5345
    0.0101
   -0.4776


Mn =

    0.5345
    0.0101
   -0.4776