선형 칼만필터(의사코드 포함)
우선 어떤 선형 시불변 시스템이 있다고 생각해봅시다. 여기서 우리는 $x_{k}$는 직접 측정할 수 없고, $y_{k}$만 직접 측정할 수 있습니다.
$$ x_{k+1} = Ax_k + Bu_{k+1} + w_{k}$$
$$ y_k = Cx_k + v_{k}$$
여기서 $v_{k}$, $w_{k}$는 각각 시스템노이즈, 측정노이즈이며, 이들에 대해서 각각 공분산행렬 $COV(w_{k}) = Q_{k}$, $COV(v_{k}) = R_{k}$를 정의해줍니다.
공분산 행렬에 대해서 간략히 설명하자면, 상태변수들을 쌍을 지어봤을때 이들이 만드는 공분산들을 행렬에 표현한 것입니다. 만약 쌍을 맺는 두 상태변수들의 노이즈가 서로 아무런 상관관계 없이 독립이라면 공분산행렬은 대각행렬에 가까운 행렬이 되겠지요?(대각성분을 제외한 나머지 성분들이 0에 매우 가까운 행렬)
여기서, 상태변수 $x$를 추정할 것인데, 우리는 $x_k$에 대해서 모델에 의해서 예측된 $\bar{x}_k$ 그리고 필터링 된 추정치인 $\hat{x}_k$를 가정할 것입니다. 거꾸로 말하자면, 칼만필터는 시스템의 모델을 이용해서 예측을 하고, 이것에 다시 실제 측정치를 이용해서 보정을 수행합니다. 맨 윗줄에서 언급했듯이 $x_{k}$는 직접 측정할 수 없다는 것을 기억해두세요.
1. 시스템 모델에 의한 예측단계
이제, $\bar{x}_k$을 다시 한번 적어보겠습니다.
$$ \bar{x}_k = A\hat{x}_{k-1}+ Bu_{k} $$
$$\bar{P}_{k} = AP_{k-1}A^{T}+Q_{k-1}$$
여기서 뜬금없이 $P_k$와 $\bar{P}_k$가 등장했습니다. 얘네들은 각각 상태변수$x$의 추정오류($x_k - \hat{x}_k$), 예측오류($x_k - \bar{x}_k$)의 공분산행렬($COV(x_k - \hat{x}_k)$, $COV(x_k - \bar{x}_k)$)들입니다. $x$의 값은 직접 알아낼 수 없기에, $x_k - \bar{x}_k$와 같은 값들또한 알아낼 수 없어서, 우리는 초기값인 $P_0$만 적절히 가정해주고 그 이후로는 연쇄적으로 계산하게 방치할 것입니다.
또한 $\bar{P}_{k}$의 식은 유도가 조금 복잡하니까 접은 글로 해두겠습니다.
공분산 행렬의 정의(편차 행렬의 제곱행렬의 평균)으로부터:
$$\bar{P}_k = E[(x_k-\bar{x}_k)(x_k-\bar{x}_k)^{T}]$$
여기서 $x_k$, $\bar{x}_k$는 시스템의 정의로부터 도출할수 있으니까 다시
$$\bar{P}_k = E[(A(x_{k-1}-\bar{x}_{k-1} +w_{k-1})(A(x_{k-1}-\bar{x}_{k-1} +w_{k-1})^{T}]$$
$$=E[A(x_{k-1}-\bar{x}_{k-1})A^{T} + A(x_{k-1}-\bar{x}_{k-1})w_{k-1}^{T} + w_{k-1}(x_{k-1}-\bar{x}_{k-1})A^{T}+w_kw_{k-1}^{T}]$$
로 전개할 수 있습니다. 그런데, $w_{k}$와 예측오류 $x_{k-1}-\bar{x}_{k-1}$가 서로 상관관계가 없다고 하면 저 둘의 곱은 0이 되어 소거되므로
$$\bar{P}_k = E[A(x_{k-1}-\bar{x}_{k-1})A^{T} + w_{k-1}w_{k-1}^{T}]$$
만 남게 되고, $w_kw_{k}^{T}$는 공분산 행렬인 $Q_k$의 정의로 인해
$$\bar{P}_{k} = AP_{k-1}A^{T}+Q_{k-1}$$
만 남게됩니다!
그 다음, 측정치인 $y_k$를 입력받았다면, 예측치인 $\bar{x}_k$에 적절한 비율로 혼합을 해줄 것입니다.
이 적절한 비율을 "칼만 게인"$K_k$이라고 합니다.
2. 측정치와 측정잡음을 반영한 추정(보정)단계
이걸 지수이동평균필터 비슷하게 표현하면, 아래의 식처럼 되고,
$$\hat{x}_{k} = (I - K_kC)\bar{x}_k + K_ky_k$$
State observer같은 표현식으로 적으면
$$\hat{x}_{k} = \bar{x}_k + K_k(y_k - C\bar{x}_k)$$
같은 식으로 적어줄 수 있습니다. 필터나 상태관측기를 전공하신 분이라면 알겠지만, $K_k$의 고윳값은 모두 0~1사이어야 이 필터가 실제 상태변수 $x_k$를 추종할 수 있다는 걸 느끼실 수 있겠지요!
여기서, 칼만게인$K_k$는 앞서 언급한 추정오류 $P_k$를 최소화 하기 위해서 계산하게 만들 것입니다. 이걸 유도하는 방법은 제가 알기론 두가지 있습니다.
1. 베이지안 필터를 응용해서 두 가우시안 PDF의 곱으로 얻는 두 PDF의 평균값/분산값을 이용하는 방법
2. $P_k$의 식을 전개 한 후, 칼만게인$K_k$로 미분하여 0을 만드는 값을 찾는 방법
이 있습니다. 1번의 경우 상태변수가 단일하게 스칼라라면 참 쉽게 되겠지만, 다변량인 시스템이라면 굉장히 식이 지저분하게 되겠지요. 그래서 2번의 방법을 사용하겠습니다.
이것을 만족하는 칼만게인 $K_k$는
$K_k = \bar{P}_kC^{T}S_{k}^{-1}$이 되고, $S_k$는 예측한 측정치와 실제 측정된 치의 잔차인
$C\bar{P}_kC^{T}+R_{k}$ 혹은 $COV(y_k-C\bar{x}_k)$
이 되고 다음단계에서 $\bar{P}_k$를 구하기 위해
$P_k=(I-K_kC_k)\bar{P}_{k}$로 업데이트를 해줘야 합니다.
이 두 수식은 전개가 난해하니까 접은글로 해두도록 하겠습니다.
우선 추정오류 $P_k$를 정의해봅시다. 또한 추정오류는 서로 독립이라고 가정해두도록 하죠
$$P_k = COV(x_k - \hat{x}_k)$$
이렇게 하면 이 행렬은 추정오류가 없을경우 0행렬이 될 것입니다.
여기서 $\hat{x}_k$를 전개($\bar{x}_k + K_k(y_k - C\bar{x}_k)$)하면
$P_k = COV(x_k - (\bar{x}_k + K_k(y_k - C\bar{x}_k)))$
또다시 $y_k = Cx_k + v_{k}$를 전개
$P_k = COV(x_k - (\bar{x}_k + K_k(Cx_k + v_{k} - C\bar{x}_k)))$
여기서 $(I-K_kC)$로 묶어주면
$P_k = COV((I-K_kC)x_k - (I-K_kC)\bar{x}_k + K_kv_k)$
여기서 측정오차인 $v_k$가 상태변수인 $x$와 완전히 독립이라면 따로 빼줄 수 있고, $x_k$와 $\bar{x}_k$도 다시 한번 정리해줍니다.
$P_k = COV((I-K_kC)(x_k - \bar{x}_k)) + COV(K_kv_k)$
또, 벡터공분산의 속성에 따라
$P_k = (I-K_kC)COV(x_k - \bar{x}_k)(I-K_kC)^{T} + K_kCOV(v_k)K_{k}^{T}$
가 되고 $\bar{P}_k$, $S_k$의 정의를 따르면
$P_k = (I-K_kC)\bar{P}_k(I-K_kC)^{T} + K_kR_kK_{k}^{T}$
가 됩니다.
$P_k$를 구했으니 앞서 말했듯이 $K_k$로 미분해줍시다.
우선 $P_k$를 전개해줍니다.
$P_k = \bar{P}_k - K_kC\bar{P}_k - \bar{P}_kC^{T}K_{k}^{T} + K_kC\bar{P}_kC^{T}K_{k}^{T} + K_kR_kK_{k}^{T}$
여기서 $C\bar{P}_kC^{T}$는 $COV(C(x_k - \bar{x}_k))$로 다시 묶어주고 $R_k$도 다시 $COV(v_k)$로 풀어주면 윗 식의 4,5번째 항을 다시한번 묶어줄 수 있습니다.($y_k = Cx_k + v_k$에서)
$P_k = \bar{P}_k - K_kC\bar{P}_k - \bar{P}_kC^{T}K_{k}^{T} + K_kCOV(y_k - C\bar{x}_k)K_{k}^{T}$
측정치인 $y_k$와 예측치로부터 얻은 예측치인 $C\bar{x}_k$를 빼준 잔차의 공분산 $COV(y_k - C\bar{x}_k)$를 $S_k$라고 따로 빼주면
$P_k = \bar{P}_k - K_kC\bar{P}_k - \bar{P}_kC^{T}K_{k}^{T} + K_kS_kK_{k}^{T}$
가 되고 이번엔 진짜로 $K_k$로 미분해줍니다.
$\frac{\partial{tr(P_k)}}{K_k} = -2(C\bar{P}_k)^{T} + 2K_kS_k = 0$
$K_k = \bar{P}_kC^{T}S^{-1}$
여기서, 역행렬인 $S^{-1}$가 나와서 수치연산을 느리게 만들 가능성이 있으나, 시스템에 복잡한 역행렬 연산을 수행할만한 여력이 안된다면, 공분산행렬을 그냥 대각행렬이라고 가정하고, 대각선분을 제외한 나머지를 전부 0으로 만든 후, 대각성분을 역수를 취해줘도 무방할 듯 합니다.
또 다음번에 $\bar{P}_k$를 구할수 있도록 $\bar{P}_k$를 기록해두어야 합니다만, 이것의 식에 앞서 구한 $K_k = \bar{P}_kC^{T}S^{-1}$를 대입한다면, $P_k$또한 심플하게 $P_k=(I-K_kC_k)\bar{P}_{k}$로 정리될 수 있습니다.
이를 매트랩코드로 의사코드로 정리하면 이렇습니다.
여기서 저는 시스템 노이즈의 공분산행렬 Q와 센서 노이즈의 공분산행렬 R을 상수로 가정했습니다. 또 $\bar{P}$를 따로 구분하지 않고 한 주기에 두번(P = A*P*A'+Q;, P = (eye(length(A))-K*C)*P;) 계산되게 했습니다. 중간 예측치인 $\bar{x}$ 또한 따로 계산하지 않고 출력치인 xhat($\hat{x}$)의 계산과정의 중간에 끼워넣었습니다.
센서 노이즈인 R을 상수로 두었으니 칼만게인을 구하는 식에서 등장하는 $S_k$또한 $C\bar{P}_kC^{T}+R_{k}$를 사용하면 되겠지요.
function xhat = KFilter(z,u,A,B,C)
%x(k+1)=Ax(k)+Bu(k)
%y(k)=Cx(k)
%A: nxn matrix
%B: nx1 matrix
%C: mxn matrix
%u: constant
%z: vector which length is m
persistent prevAvg
persistent firstRun
persistent P
Q = 0.01*eye(length(A));
R = 1*eye(size(C,1));
if isempty(firstRun) %이게 처음 실행된 거냐?
P = eye(length(A)); %P_0(초기값)지정
prevAvg = zeros(length(A),1); %xhat_0(초기값)지정
firstRun = 1; %이제 이건 한번 돌렸던 코드임.
arr = []; %특별히 의미없음
end
%예측공분산 계산
P = A*P*A'+Q;
%칼만게인 산출
K = P*C'*inv(C*P*C'+R);
%예측치 계산
xhat = A*prevAvg+B*u;
%칼만게인을 이용해서 측정치 반영
xhat = xhat + K*(z-C*xhat);
%다음회차 연산을 위해서 P를 업데이트
prevAvg = xhat;
P = (eye(length(A))-K*C)*P;이것을 호출하기 위한 코드는 이렇습니다.
칼만필터뿐 아니라 이동평균필터나 평균필터도 별도구현해야하기때문에, 직접 돌려보고 싶으시다면 해당 부분을 삭제하거나 이들 필터도 구현해야 합니다.
%위치밖에 측정할 수 없는 등가속도 운동
clear all
clc
dt = .02;
t = 0:dt:5;
vs = 3; %전체 상태변수의 갯수 - 정방행렬 A의 크기
vm = 1; %측정 가능한 상태변수 - C의 행벡터의 갯수
Nsamples = zeros(length(t),vm);
Nfiltered = zeros(length(t),vs);
NMfiltered = zeros(length(t),vm);
Nreal = zeros(length(t),vs);
u = 0.3*ones(length(t),1);
A = [1 0 0;dt 1 0;0 dt 1];
B = [1 0 0]';
C = [0 0 1];
realx = zeros(vs,1); %임시로만 사용하는 변수
realx(1,1) = 2; %초기 가속도
for k = 1:length(Nsamples)
%실제 상태변수의 변화
realx = A*realx+B*u(k);
Nreal(k,:) = realx';
%잡음 투입
for l = 1:vm;
w(l,1) = randn();
end
%시간에 따른 잡음 크기 변화
if t(k) > 4
w = w*10;
elseif t(k) > 2.5
w = w*50;
elseif t(k) > 1.5
w = w*5;
end
Nsamples(k,:) = (C*realx + w)';
Nfiltered(k,:) = (KFilter((Nsamples(k,1))',u(k),A,B,C))';
NMfiltered(k,:) = (ExmoAvgFilter(Nsamples(k,:)',0.6))';
end
clf
figure(1)
hold on
paf = plot(t,Nfiltered(:,1),'b:');
pvf = plot(t,Nfiltered(:,2),'b:');
pdf = plot(t,Nfiltered(:,3),'b:');
pds = plot(t,Nsamples,'r');
%par = plot(t,Nreal(:,1),'g--');
%pvr = plot(t,Nreal(:,2),'g--');
pdr = plot(t,Nreal(:,3),'g--');
pdf2 = plot(t,NMfiltered(:,1),'y:');
axis([t(1) t(length(t)) -1 300]);
hold off
legend('추정 가속도', '추정 속도', '추정 위치', '측정 위치')
hold off
사실 이 코드는 의사코드이기도 하지만, 매트랩에서 입력만 적절하면 실제로 동작하는 코드입니다. 학생시절때 세미나 발표용으로 만들었었던 코드였거든요. 이 코드는 인터프리터 언어인 매트랩을 사용하고, 역행렬도 구하기때문에 굉장히 느립니다만, C++등의 언어를 사용하고, S도 대각행렬이라고 가정하고 역행렬을 단순화시켜버리면 실제 제어에서도 동작할 것입니다.
사실 접은글로 해둔 칼만필터의 유도 공식은 한국어 위키백과에 나와있는 것을 그대로 옮겨 적은 것이고, 거기에 제가 이해했던 방식으로 살을 덧대고, 표기방식도 칼만필터 이외에도 다른 상태변수관측기나, 필터이론 등에서도 범용적으로 쓰이는 표기법 등으로 바꾼 것에 불과합니다.