Main Content

칼만 필터링

이 예제에서는 칼만 필터를 적용하는 방법을 보여줍니다. 먼저 kalman 명령을 사용하여 정상 상태 필터를 설계합니다. 그런 다음, 시스템을 시뮬레이션하여 측정 잡음으로 인한 오차를 줄이는 방법을 보여줍니다. 또한 비정상 잡음원이 있는 시스템에 유용할 수 있는 시변 필터를 구현하는 방법도 보여줍니다.

정상 상태 칼만 필터

입력에 가우스 잡음 w가 있고 출력에 측정 잡음 v가 있는 다음 이산 플랜트가 있다고 가정하겠습니다.

x[n+1]=Ax[n]+Bu[n]+Gw[n]y[n]=Cx[n]+Du[n]+Hw[n]+v[n]

목표는 잡음이 있는 측정값 y[n]을 기반으로 실제 플랜트 출력 yt[n]=y[n]-v[n]을 추정하는 칼만 필터를 설계하는 것입니다. 이 정상 상태 칼만 필터는 이러한 추정을 위해 다음 방정식을 사용합니다.

시간 업데이트:

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

측정값 업데이트:

xˆ[n|n]=xˆ[n|n-1]+Mx(y[n]-Cxˆ[n|n-1]-Du[n])yˆ[n|n]=Cxˆ[n|n-1]+Du[n]+My(y[n]-Cxˆ[n|n-1]-Du[n])

여기서 각각은 다음을 나타냅니다.

  • xˆ[n|n-1]y[n-1]까지의 과거 측정값이 주어질 경우 x[n]의 추정입니다.

  • xˆ[n|n]yˆ[n|n]은 추정된 상태 값 및 측정값이며 마지막 측정 y[n]을 기반으로 업데이트됩니다.

  • MxMy는 잡음 공분산 E(w[n]w[n]T)=Q , E(v[n]v[n]T)=RN=E(w[n]v[n]T)=0이 주어질 경우 추정 오차의 정상 상태 공분산을 최소화하기 위해 선택된 최적의 혁신 이득입니다. (이러한 이득을 선택하는 방법에 대한 자세한 내용은 kalman 항목을 참조하십시오.)

(이 업데이트 방정식은 current 유형 추정기를 설명합니다. current 추정기와 delayed 추정기의 차이점에 대한 정보는 kalman 항목을 참조하십시오.)

필터 설계하기

kalman 함수를 사용하여 이 정상 상태 칼만 필터를 설계할 수 있습니다. 이 함수는 사용자가 제공하는 공정 잡음 공분산 Q와 센서 잡음 공분산 R을 기반으로 특정 플랜트에 대한 최적의 정상 상태 필터 이득 M을 결정합니다. 이 예제에서는 플랜트의 상태공간 행렬에 다음 값을 사용합니다.

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;

이 예제에서는 G=B를 설정하며, 이는 공정 잡음 w가 가산성 입력 잡음임을 의미합니다. 또한 H=0을 설정하며, 이는 입력 잡음 w가 출력 y에 직접적인 영향을 미치지 않음을 의미합니다. 이러한 가정은 더 간단한 플랜트 모델을 생성합니다.

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

H = 0일 때, My=CMx(kalman 참조)임을 보일 수 있습니다. 이러한 가정은 또한 칼만 필터에 대한 업데이트 방정식을 단순화합니다.

시간 업데이트:

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

측정값 업데이트:

xˆ[n|n]=xˆ[n|n-1]+Mx(y[n]-Cxˆ[n|n-1])yˆ[n|n]=Cxˆ[n|n]

이 필터를 설계하려면 먼저 w에 대한 입력을 사용하여 플랜트 모델을 만들어야 합니다. 샘플 시간을 -1로 설정하여 플랜트를 이산으로 표시하십시오(특정 샘플 시간 없음).

Ts = -1;
sys = ss(A,[B B],C,D,Ts,'InputName',{'u' 'w'},'OutputName','y');  % Plant dynamics and additive input noise w

공정 잡음 공분산 Q와 센서 잡음 공분산 R은 0보다 큰 값으로, 일반적으로 해당 시스템에 대한 연구 결과나 측정값에서 얻습니다. 이 예제에서는 다음 값을 지정하십시오.

Q = 2.3; 
R = 1; 

kalman 명령을 사용하여 필터를 설계합니다.

[kalmf,L,~,Mx,Z] = kalman(sys,Q,R);

이 명령은 시간 업데이트 방정식과 측정값 업데이트 방정식을 구현하는 상태공간 모델인 칼만 필터 kalmf를 설계합니다. 필터 입력은 플랜트 입력 u와 잡음이 있는 플랜트 출력 y입니다. kalmf의 첫 번째 출력은 실제 플랜트 출력의 추정값 yˆ이고 나머지 출력은 상태 추정값 xˆ입니다.

kalmdemo2.png

이 예제에서는 상태 추정값을 버리고 첫 번째 출력값인 yˆ만 유지합니다.

kalmf = kalmf(1,:);

필터 사용하기

이 필터의 작동 방법을 살펴보려면 일부 데이터를 생성하고 필터링된 응답을 실제 플랜트 응답과 비교해보면 됩니다. 전체 시스템은 다음 다이어그램에 나와 있습니다.

kalmdemo1.png

이 시스템을 시뮬레이션하기 위해 sumblk를 사용하여 측정값 잡음 v에 대한 입력을 생성합니다. 그런 다음 connect를 사용해 sys와 칼만 필터를 함께 결합하여, u가 공유 입력이고 잡음이 있는 플랜트 출력 y가 다른 필터 입력에 공급되도록 합니다. 결과는 입력 w, v, u와 출력 yt(실제 응답), ye(필터링되거나 추정된 응답 yˆ)가 있는 시뮬레이션 모델입니다. ytye 신호는 각각 플랜트와 필터의 출력입니다.

sys.InputName = {'u','w'};
sys.OutputName = {'yt'};
vIn = sumblk('y=yt+v');

kalmf.InputName = {'u','y'};
kalmf.OutputName = 'ye';

SimModel = connect(sys,vIn,kalmf,{'u','w','v'},{'yt','ye'});

필터 동작을 시뮬레이션하기 위해 알려진 정현파 입력 벡터를 생성합니다.

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

필터 설계에 사용한 것과 동일한 잡음 공분산 값 QR을 사용하여 공정 잡음 벡터와 센서 잡음 벡터를 생성합니다.

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

마지막으로 lsim을 사용하여 응답을 시뮬레이션합니다.

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

lsimyt, w, v에 적용된 입력에 대해 출력 u와 ye에서 응답을 생성합니다. yt와 ye 채널을 추출하고 측정된 응답을 계산하십시오.

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

실제 응답을 필터링된 응답과 비교합니다.

clf
subplot(211), plot(t,yt,'b',t,ye,'r--'), 
xlabel('Number of Samples'), ylabel('Output')
title('Kalman Filter Response')
legend('True','Filtered')
subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'),
xlabel('Number of Samples'), ylabel('Error')
legend('True - measured','True - filtered')

Figure contains 2 axes objects. Axes object 1 with title Kalman Filter Response contains 2 objects of type line. These objects represent True, Filtered. Axes object 2 contains 2 objects of type line. These objects represent True - measured, True - filtered.

두 번째 플롯에서 볼 수 있듯이 칼만 필터는 측정 잡음으로 인한 오차 yt - y를 줄입니다. 이러한 오차 감소를 확인하기 위해 필터링 전(측정 오류 공분산)과 필터링 후(추정 오류 공분산) 오차의 공분산을 계산합니다.

MeasErr = yt-yt;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)
MeasErrCov = 0
EstErr = yt-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov = 0.3479

시변 칼만 필터 설계

이전 설계에서는 잡음 공분산이 시간이 지남에 따라 변하지 않는다고 가정했습니다. 시변 칼만 필터는 잡음 공분산이 비정상인 경우에도 잘 수행될 수 있습니다.

시변 칼만 필터는 다음과 같은 업데이트 방정식을 갖습니다. 시변 필터에서 오차 공분산 P[n]과 혁신 이득 Mx[n]은 모두 시간에 따라 변할 수 있습니다. 다음과 같이 시변을 고려하여 시간 및 측정값 업데이트 방정식을 수정할 수 있습니다. (이러한 수식에 대한 자세한 내용은 kalman 항목을 참조하십시오.)

시간 업데이트:

xˆ[n+1|n]=Axˆ[n|n]+Bu[n]+Bw[n]P[n+1|n]=AP[n|n]AT+BQBT

측정값 업데이트:

xˆ[n|n]=xˆ[n|n-1]+Mx[n](y[n]-Cxˆ[n|n-1])Mx[n]=P[n|n-1]CT(CP[n|n-1]CT+R[n])-1P[n|n]=(I-Mx[n]C)P[n|n-1]yˆ[n|n]=Cxˆ[n|n]

Kalman Filter 블록을 사용하여 Simulink®에서 시변 칼만 필터를 구현할 수 있습니다. 해당 블록의 사용을 보여주는 예제는 시변 칼만 필터를 사용한 상태 추정 항목을 참조하십시오. 이 예제에서는 MATLAB®에서 시변 필터를 구현합니다.

시변 칼만 필터를 생성하기 위해 먼저 잡음이 있는 플랜트 응답을 생성합니다. 그러고 나서 이전에 정의한 입력 신호 u와 공정 잡음 w에 대한 플랜트 응답을 시뮬레이션합니다. 그런 다음, 측정 잡음 v를 시뮬레이션된 실제 응답 yt에 추가하여 잡음 응답 y를 구합니다. 이 예제에서 잡음 벡터 wv의 공분산은 시간에 따라 변하지 않습니다. 그러나 비정상 잡음에 대해 동일한 절차를 사용할 수 있습니다.

yt = lsim(sys,[u w]);   
y = yt + v;         

다음으로, 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
  Mxn = P*C'/(C*P*C'+R);
  x = x + Mxn*(y(i)-C*x);   % x[n|n]
  P = (eye(3)-Mxn*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,yt,'b',t,ye,'r--')
xlabel('Number of Samples'), ylabel('Output')
title('Response with Time-Varying Kalman Filter')
legend('True','Filtered')
subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'),
xlabel('Number of Samples'), ylabel('Error')
legend('True - measured','True - filtered')

Figure contains 2 axes objects. Axes object 1 with title Response with Time-Varying Kalman Filter contains 2 objects of type line. These objects represent True, Filtered. Axes object 2 contains 2 objects of type line. These objects represent True - measured, True - filtered.

시변 필터는 추정 과정에서 출력 공분산도 추정합니다. 이 예제에서는 정상 입력 잡음을 사용하기 때문에 출력 공분산은 정상 상태 값이 되는 경향이 있습니다. 출력 공분산을 플로팅하여 필터가 정상 상태에 도달했는지 확인하십시오.

figure
plot(t,errcov) 
xlabel('Number of Samples'), ylabel('Error Covariance'),

Figure contains an axes object. The axes object contains an object of type line.

공분산 플롯으로부터 출력 공분산이 약 5개 샘플에서 정상 상태에 도달함을 알 수 있습니다. 그때부터 시변 필터는 정상 상태 버전과 동일한 성능을 갖게 됩니다.

정상 상태의 경우와 같이 필터가 측정 잡음으로 인한 오차를 줄여줍니다. 이러한 오차 감소를 확인하기 위해 필터링 전(측정 오류 공분산)과 필터링 후(추정 오류 공분산) 오차의 공분산을 계산합니다.

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

마지막으로, 시변 필터가 정상 상태에 도달하면 이득 행렬 Mxn의 값이 정상 상태 필터에 대해 kalman이 계산한 값과 일치합니다.

Mx,Mxn
Mx = 3×1

    0.5345
    0.0101
   -0.4776

Mxn = 3×1

    0.5345
    0.0101
   -0.4776

참고 항목

관련 항목

외부 웹 사이트