칼만 필터
루돌프 칼만이 개발한 알고리즘.
1. 개요
오차(외란)를 가지는 관측치로 부터 시스템의 상태를 추정하거나 제어하기 위한 알고리즘, 상태 관측기의 최적버전. 예를 들어 로봇 팔의 각도와 각속도가 알고 싶은데, 관측가능한 점은 로봇 팔의 끝이라고 하자. 그러면 각도와 각속도를 상태로 가지는 상태방정식및 출력방정식을 세워서 칼만 필터를 돌리게 되면, 매우 근사하게 각도와 각속도를 시시각각 추정할 수 있다. 물론 상태방정식과 출력방정식이 가지는 물리적 패러미터가 정확하다라는 가정하의 얘기고, 이 시스템의 경우 비선형이기 때문에 선형 칼만 필터가 아닌 Unscented Kalman Filter같은 비선형 칼만 필터가 필요하다.
보다 구체적이고 간단한 예를 들어보자면, 지표면을 움직이는 자동차를 생각해볼 수 있다. 우리가 궁금한건 특정 시점 (T) 의 자동차의 "정확한 위치(X)" 이다. 우리가 관찰하는 차량엔 INS가 장착되어있고, 또한 적당한 방법 (GPS를 이용한다 치자)을 통해 자동차의 위치를 잴 수 있다고 하자.
위 상황에서 T 시점에서 자동차의 위치 X를 구하는 방법은 두 가지가 있다. 하나는 시점 T-1의 자동차 위치와 INS의 값을 이용해 추측하는것, 그리고 시점 T에서 GPS의 값을 읽는 것. 두 관측값 모두 오차를 가지고 있다 (모든 관측값은 오차를 가지고 있으며, 이는 기술의 발전을 통해 줄일 수는 있지만 완전히 없애는건 불가능하다). 한가지 다행인 것은, 우리는 두 가지 전혀 다른 방법을 통해 자동차의 위치를 구했으며, 오차가 어느 정도인지 알고 있다는 것이다. 따라서 이 오차의 역수에 비례하는 가중치를 줘서 두 값을 더하면 더한 값의 오차는 최소가 된다.
이렇게 구한 T시점의 위치 X를 다시 T+1시점의 위치를 계산하는데 사용하면, INS를 통해 계산되는 X의 오차가 줄어들게 된다 (초기값인 T의 X의 오차가 줄어듦으로). 따라서 계산된 위치값의 오차는 이 전 단계보다 더 작아지며, 이를 반복하면 오차는 점점 수렴하게 된다.
이를 보다 formal 한 언어로 일반화 하자면, 칼만 필터는 현 시점의 계의 상태의 측정치와 이전 시점의 계의 상태로부터 예측된 현 시점의 시스템의 상태를 재귀적으로 이용해 현재 시점의 계의 상태를 최적 추정 (optimal estimation)을 하는 기법이다. 기본 개념 자체는 그다지 어렵지 않으나 응용분야에 따라 "예측"에 사용하는 모델이 복잡하고 비선형인 경우가 많기 때문에 필터를 잘 설계하려면 모델에 대한 이해가 전제 되어야 한다.
특히 이 알고리즘이 유용한 이유는 수식적으로 앞서 입력된 값들을 모두 기억했다가 평균을 내는 것이 아니라, 매 순간 앞서 입력된 값들에 의한 어떠한 경향을 나타내는 값 몇가지만 메모리에 기억해두었다가 바로 다음 타이밍에 입력된 값을 가지고 이 경향을 나타내는 값을 다시 보정하는 방식이라는 점이다. 이는 메모리 용량이 끔찍하게 부족했던 1960년대에 대단히 유용한 기법이었으며, 지금도 실시간으로 빠르게 작동해야 하는 시스템에게 중요하다.
이 알고리즘은 루돌프 칼만이 미국의 NASA에 방문했을 때, 자신이 떠올린 아이디어가 아폴로 계획에 쓰일 수 있다고 생각하고 아폴로 프로젝트의 항법컴퓨터에 이 기법을 적용하고자 연구하였다.
현재도 항법, 제어 같은데 쓰인다. 또한 펄스형 레이더처럼 입력신호가 연속적이지 않고 끊겨져 들어올때, 다음 신호가 들어올 방향과 타이밍을 예측하는데 쓰인다.
더 상세한 내용과 정확한 수식들에 대하여 많은 엔지니어 여러분들의 적극적인 추가작성 바랍니다.
2. 선형 칼만필터 알고리즘
먼저 이해를 돕기위해 스칼라 칼만필터를 유도하겠다.
2.1. 스칼라 칼만필터 유도
배경
현재 상태와 다음상태의 관계는 다음과 같이 모델되어진다.
$$x_{j} = ax_{j-1} +bu_{j} $$
이때, 프로세스상의 에러를 process error term 을 두어 나타낸다.$$x_{j} = ax_{j-1} +bu_{j} + w_j $$
이때 observation은 다음과 같이 주어진다.$$z_j =hx_j $$
에러텀 추가$$z_j =hx_j + v_j $$
설명 추가할것..$$\hat{x}_{j}^- = a\hat{x}_{j-1} +bu_{j} $$
$$Residual = z_j - \hat{z}_j = z_j - h\hat{x}_j^- $$
$$\hat{x}_j = \hat{x}_j^- + k(Residual) = \hat{x}_j^- + k\left(z_j - h\hat{x}_j^-\right) $$
$$e_j^- = x_j - \hat{x}_j^- $$
$$ e_j = x_j - \hat{x}_j$$
$$p_j^{-}= E\left\{ \left( e_j^- \right)^2 \right\} $$
$$p_j= E\left\{ \left( e_j \right)^2 \right\} $$
$$p_j= E\left\{ \left( x_j - \hat{x}_j \right)^2 \right\} $$
$$ p_j= E\left\{ \left( x_j - \hat{x}_j^- - k\left(z_j - h\hat{x}_j^-\right) \right)^2 \right\} $$
$$\frac {\partial p_j}{\partial k} = 0 = \frac {\partial E\left\{ \left( x_j - \hat{x}_j^- - k\left(z_j - h\hat{x}_j^-\right) \right)^2 \right\} }{\partial k} $$
$$=2E\left\{ \left( x_j - \hat{x}_j^- - k\left(z_j - h\hat{x}_j^-\right) \right)\left(z_j - h\hat{x}_j^- \right) \right\} $$
$$=2E\left\{ x_j z_j - \hat{x}_j^- z_j - kz_j^2 + kh\hat{x}_j^-z_j - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 + khz_j\hat{x}_j^- - kh^2\left(\hat{x}_j^-\right)^2 \right\}$$
$$ =2E\left\{ x_j z_j - \hat{x}_j^- z_j - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 \right\} -2kE\left\{z_j^2 - 2h\hat{x}_j^-z_j + h^2\left(\hat{x}_j^-\right)^2 \right\}$$
$$ k= \frac{E\left\{ x_j z_j - \hat{x}_j^- z_j - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 \right\}}{E\left\{z_j^2 - 2h\hat{x}_j^-z_j + h^2\left(\hat{x}_j^-\right)^2 \right\}}$$
$$ numerator= E\left\{ x_j z_j - \hat{x}_j^- z_j - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 \right\}$$
$$ =E\left\{ x_j \left(hx_j + v_j\right) - \hat{x}_j^- \left(hx_j + v_j\right) - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 \right\} $$
$$Residual = z_j - \hat{z}_j = z_j - h\hat{x}_j^- $$
$$\hat{x}_j = \hat{x}_j^- + k(Residual) = \hat{x}_j^- + k\left(z_j - h\hat{x}_j^-\right) $$
$$e_j^- = x_j - \hat{x}_j^- $$
$$ e_j = x_j - \hat{x}_j$$
$$p_j^{-}= E\left\{ \left( e_j^- \right)^2 \right\} $$
$$p_j= E\left\{ \left( e_j \right)^2 \right\} $$
$$p_j= E\left\{ \left( x_j - \hat{x}_j \right)^2 \right\} $$
$$ p_j= E\left\{ \left( x_j - \hat{x}_j^- - k\left(z_j - h\hat{x}_j^-\right) \right)^2 \right\} $$
$$\frac {\partial p_j}{\partial k} = 0 = \frac {\partial E\left\{ \left( x_j - \hat{x}_j^- - k\left(z_j - h\hat{x}_j^-\right) \right)^2 \right\} }{\partial k} $$
$$=2E\left\{ \left( x_j - \hat{x}_j^- - k\left(z_j - h\hat{x}_j^-\right) \right)\left(z_j - h\hat{x}_j^- \right) \right\} $$
$$=2E\left\{ x_j z_j - \hat{x}_j^- z_j - kz_j^2 + kh\hat{x}_j^-z_j - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 + khz_j\hat{x}_j^- - kh^2\left(\hat{x}_j^-\right)^2 \right\}$$
$$ =2E\left\{ x_j z_j - \hat{x}_j^- z_j - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 \right\} -2kE\left\{z_j^2 - 2h\hat{x}_j^-z_j + h^2\left(\hat{x}_j^-\right)^2 \right\}$$
$$ k= \frac{E\left\{ x_j z_j - \hat{x}_j^- z_j - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 \right\}}{E\left\{z_j^2 - 2h\hat{x}_j^-z_j + h^2\left(\hat{x}_j^-\right)^2 \right\}}$$
$$ numerator= E\left\{ x_j z_j - \hat{x}_j^- z_j - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 \right\}$$
$$ =E\left\{ x_j \left(hx_j + v_j\right) - \hat{x}_j^- \left(hx_j + v_j\right) - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 \right\} $$
$$=E\left\{ hx_j^2 + x_jv_j - h\hat{x}_j^-x_j - \hat{x}_j^-v_j - hx_j\hat{x}_j^- + h\left(\hat{x}_j^-\right)^2 \right\} $$
$$=E\left\{ hx_j^2 - 2h\hat{x}_j^-x_j +h\left(\hat{x}_j^-\right)^2 + \left(x_j - \hat{x}_j^-\right)v_j \right\}$$
$$=E\left\{ hx_j^2 - 2h\hat{x}_j^-x_j +h\left(\hat{x}_j^-\right)^2 +e_j^-v_j \right\}$$
$$=E\left\{ hx_j^2 - 2h\hat{x}_j^-x_j +h\left(\hat{x}_j^-\right)^2 \right\}$$
$$=hE\left\{\left(x_j - \hat{x}_j^- \right)^2\right\} = hE\left\{\left(e_j^-\right)^2\right\} $$
$$=hp_j^-$$
$$denominator= E\left\{z_j^2 - 2h\hat{x}_j^-z_j + h^2\left(\hat{x}_j^-\right)^2 \right\} $$$$=E\left\{ hx_j^2 - 2h\hat{x}_j^-x_j +h\left(\hat{x}_j^-\right)^2 + \left(x_j - \hat{x}_j^-\right)v_j \right\}$$
$$=E\left\{ hx_j^2 - 2h\hat{x}_j^-x_j +h\left(\hat{x}_j^-\right)^2 +e_j^-v_j \right\}$$
$$=E\left\{ hx_j^2 - 2h\hat{x}_j^-x_j +h\left(\hat{x}_j^-\right)^2 \right\}$$
$$=hE\left\{\left(x_j - \hat{x}_j^- \right)^2\right\} = hE\left\{\left(e_j^-\right)^2\right\} $$
$$=hp_j^-$$
$$=E\left\{\left(hx_j + v_j\right)^2 - 2h\hat{x}_j^-\left(hx_j + v_j\right) + h^2\left(\hat{x}_j^-\right)^2 \right\} $$
$$=E\left\{h^2x_j^2 + 2hx_jv_j + v_j^2 -2h^2\hat{x}_j^-x_j -2h\hat{x}_j^-v_j + h^2\left(\hat{x}_j^-\right)^2 \right\} $$
$$=E\left\{h^2x_j^2 - 2h^2\hat{x}_j^-x_j + h^2\left(\hat{x}_j^-\right)^2 +v_j^2 + 2h\left(x_j - \hat{x}_j^-\right)v_j \right\}$$
$$=E\left\{h^2x_j^2 - 2h^2\hat{x}_j^-x_j + h^2\left(\hat{x}_j^-\right)^2 +v_j^2 + 2h\left(e_j^-\right)v_j \right\}$$
$$=E\left\{h^2x_j^2 - 2h^2\hat{x}_j^-x_j + h^2\left(\hat{x}_j^-\right)^2 +v_j^2\right\}$$
$$=h^2E\left\{x_j^2 - 2\hat{x}_j^-x_j + \left(\hat{x}_j^-\right)^2\right\} +E\left\{v_j^2\right\}$$
$$=h^2p_j^- + R\\
$$k = \frac{hp_j^-}{h^2p_j^- + R} $$
$$p_j^- = E\left\{\left(x_j-\hat{x}_j^-\right)^2\right\} $$
$$= E\left\{\left(ax_{j-1}+bu_{j} + w_{j}-\left(a\hat{x}_{j-1} +bu_{j} \right)\right)^2\right\} $$
$$= E\left\{\left(a\left(x_{j-1}-\hat{x}_{j-1}\right)+w_j\right)^2\right\} $$
$$= E\left\{a^2\left(x_{j-1}-\hat{x}_{j-1}\right)^2+ 2aw_j\left(x_{j-1}-\hat{x}_{j-1}\right) +w_j^2\right\} $$
$$= E\left\{a^2\left(x_{j-1}-\hat{x}_{j-1}\right)^2+ 2aw_je_{j-1} +w_j^2\right\} $$
$$= a^2E\left\{\left(x_{j-1}-\hat{x}_{j-1}\right)^2\right\}+ 2aE\left\{w_je_{j-1}\right\} +E\left\{w_j^2\right\}$$
$$= a^2p_{j-1} + Q$$
$$p_j = E\left\{\left(x_j-\hat{x}_j\right)^2\right\} $$
$$= E\left\{\left(x_j-\left(\hat{x}_j^- + k\left(z_j - h\hat{x}_j^-\right)\right)\right)^2\right\} $$
$$= E\left\{\left(x_j-\left(\hat{x}_j^- + k\left(\left(hx_j + v_j\right) - h\hat{x}_j^-\right)\right)\right)^2\right\} $$
$$= E\left\{\left(x_j-\hat{x}_j^- - khx_j-kv_j + kh\hat{x}_j^-\right)^2\right\} $$
$$= E\left\{\left(\left(x_j-\hat{x}_j^-\right)\left(1-kh\right)-kv_j\right)^2\right\} $$
$$= E\left\{\left(x_j-\hat{x}_j^-\right)^2\left(1-kh\right)^2-2kv_j\left(x_j-\hat{x}_j^-\right)\left(1-kh\right)+k^2v_j^2\right\} $$
$$= E\left\{\left(x_j-\hat{x}_j^-\right)^2\left(1-kh\right)^2\right\} - E\left\{2kv_j\left(x_j-\hat{x}_j^-\right)\left(1-kh\right)\right\} + E\left\{k^2v_j^2\right\}$$
$$= \left(1-kh\right)^2E\left\{\left(x_j-\hat{x}_j^-\right)^2\right\} - 2k\left(1-kh\right)E\left\{v_j\left(x_j-\hat{x}_j^-\right)\right\} + k^2E\left\{v_j^2\right\}$$
$$= \left(1-kh\right)^2E\left\{\left(e_j^-\right)^2\right\} - 2k\left(1-kh\right)E\left\{v_je_j^-\right\} + k^2E\left\{v_j^2\right\}$$
$$= \left(1-kh\right)^2E\left\{\left(e_j^-\right)^2\right\} - 2k\left(1-kh\right)E\left\{v_je_j^-\right\} + k^2E\left\{v_j^2\right\}$$
$$=\left(1-hk\right)^2p_j^- + k^2R$$
$$R=\frac{p_j^-\left(h-h^2k\right)}{k}$$
$$p_j=\left(1-hk\right)^2p_j^- + k^2\frac{p_j^-\left(h-h^2k\right)}{k}$$
$$=p_j^-(1-2hk + h^2k^2 +kh - h^2k^2)$$
$$=p_j^-(1-hk)$$
2.2. 칼만필터 알고리즘 형태
측정값을 $$z_k$$라고 하자
1. 초기값 선정 :
$$ x_0$$, $$P_0$$
2. 추정값과 오차 공분산 예측 :$$\hat{x}_k = A*x_{k-1}$$
$$\bar{P}_k = A*P_{k-1}A^T + Q$$
3. 칼만 이득 계산:$$\bar{P}_k = A*P_{k-1}A^T + Q$$
$$ K_k = \bar{P}_k H^T(H\bar{P}_k H^T + R)^{-1}$$
4. 추정값 계산 :$$ x_k = \hat{x}_k + K_k(z_k - H \hat{x}_k)$$
5. 오차 공분산 계산$$ P_k = \bar{P}_k - K_k H \bar{P}_k$$
이 이후로는 다시 2로 회귀하여 계산한다.2.3. 알고리즘 설명
$$ x_k $$와 $$ \hat{x}_k$$가 사용되고 있는데 이는 각각 추정값과 추정값의 예측값(혹은 텍스트북에 따라 사전 추정값이라는 용어도 사용한다.)을 의미한다. 윗 단락의 2, 4의 식을 보고 필터 관련 수업을 들었던 사람이라면 빠르게 눈치 챘을 것이다. 이는 LPF(저주파 통과 필터)의 알고리즘 식과 유사한 형태를 띄고 있다. 칼만필터의 기본적인 형태는 LPF와 크게 다르지 않다.
$$ x_k = \hat{x}_k + K_k(z_k - H \hat{x}_k)$$
일단, 이 식만 따로 떼어 놓고 보자. 이식을 다시 정리하면
$$ x_k = (1 - K_k H)\hat{x}_k + K_k z_k$$
의 형태가 된다. 여기서 만약 H 가 단위 행렬이라고 가정 한다면
$$ x_k = (1 - K_k)\hat{x}_k + K_k z_k$$
가 된다. 즉, 1차 LPF알고리즘
$$ \bar{x}_k = a \bar{x}_{k-1} + ( 1 - a )x_k$$
와 형태가 거의 동일함을 알 수있다. 즉, 칼만 필터는 LPF에 통계적 기법을 더한 알고리즘이라고 이해하면 쉽다. 반대로 LPF와 칼만 필터의 차이는 LPF에서는 a가 상수로 고정되어 있지만, 칼만 필터에서는 들어오는 입력값과 기존의 시스템 모델링, 통계적 추정을 통해 가중치를 유동적으로 조절하는 것에 차이가 있다.
3. 확장 칼만 필터
칼만 필터는 기본적으로 시스템이 선형이라는 가정하에 설계되었기 때문에 비선형 시스템에 적용하면 오차가 오히려 커지거나 발산해버리는 특성을 가지고 있다. 실제로 시스템중에 비선형 시스템이 굉장히 많기 때문에 이를 극복하기 위해 확장 칼만 필터가 등장하게 되었다. 이것이 발전하여 무향 칼만 필터 (unscented 칼만 필터)가 등장하게 된다. 실제로 확장 칼만과 무향 칼만 둘다 비선형 시스템 분야에서 쓰이는데, 이는 확장 칼만은 계산시간에서 이득을 얻을 수 있고 무향칼만은 정확도 면에서 이득을 볼 수있기 때문에 보통 두가지를 모두 적용해보고 성능을 비교해서 적용한다.
3.1. 확장 칼만 필터 알고리즘
기본적인 형태는 선형 칼만 필터와 거의 흡사하다.
측정값을 $$z_k$$라고 하자
1. 초기값 선정 :
$$ x_0$$, $$P_0$$
2. 추정값과 오차 공분산 예측 :$$\hat{x}_k = f(x_{k-1})$$
$$\bar{P}_k = A*P_{k-1}A^T + Q$$
3. 칼만 이득 계산:$$\bar{P}_k = A*P_{k-1}A^T + Q$$
$$ K_k = \bar{P}_k H^T(H\bar{P}_k H^T + R)^{-1}$$
4. 추정값 계산 :$$ x_k = \hat{x}_k + K_k(z_k - h( \hat{x}_k))$$
5. 오차 공분산 계산$$ P_k = \bar{P}_k - K_k H \bar{P}_k$$
이 이후 2로 회귀하여 계산한다.3.2. 알고리즘 설명
기본적인 원리는 선형 칼만 필터와 동일 하지만 가장 큰 차이는
$$\hat{x}_k = A*x_{k-1}$$
$$\hat{x}_k = f(x_{k-1})$$
우변의 식이 함수의 형태로 바뀌었다는 것을 알수 있다 또한 $$\hat{x}_k = f(x_{k-1})$$
$$ x_k = \hat{x}_k + K_k(z_k - H \hat{x}_k)$$
$$ x_k = \hat{x}_k + K_k(z_k - h( \hat{x}_k))$$
여기서도$$ h( \hat{x}_k)$$ 의 형태로 함수의 형태로 바뀜을 알 수있다. 그런데 여기서 가장 큰 문제점을 알 수있다. 앞에 A와 H는 선형 시스템의 모델링 행렬임을 알고 있었다. 하지만 확장 칼만에서는 f,h의 함수는 알고 있지만, A,H 행렬은 알지 못한다. 그래서 A,H를 자코비안을 이용하여 추정한다. 자코비안은 비선형 시스템을 근사할 때 아주 많이 쓰이는 고전적 방법 이니 간단하게 설명하고 넘어가면$$ x_k = \hat{x}_k + K_k(z_k - h( \hat{x}_k))$$
$$A \equiv \displaystyle \frac{ \partial f }{ \partial x } \displaystyle \mid_{\hat {x}_k} $$
$$H \equiv \displaystyle \frac{ \partial h }{ \partial x } \displaystyle \mid_{\hat{x}_k} $$
여기서 하나 이상한 점을 볼 수 있는데 $$ {\hat{x}_k} $$ 추정값의 예측값을 기준으로 자코비안을 구한다는 것 이다. 이는 생각보다 중요한 의미를 가지는 것인데 어느 점에서 자코비안을 구할지를 사전에 결정하기 어려운 시스템에서 쓰기 좋다는 것이다. 만약 사전에 어느점에서 자코비안을 구하기 쉬운 시스템이라면 그 자코비안을 이용하여 선형 칼만 필터를 사용하면 되는 것이다. 이를 선형화 칼만 필터라고 부른다.
즉, 값이 변할때 마다 그구간마다 선형화 작업을 해서 칼만 필터를 적용 하는 것이 확장 칼만 필터의 핵심이다.