Kalman filter란??

잡음이 포함된 선형 시스템에서 대상체의 상태(State)를 추적하는 재귀 필터. 쉽게 말하면 피드백을 통해 대상의 파라미터를 계속해서 수정하는 역할을 하는 것이다.

선형 칼만 필터

칼만 필터는 예측 단계와 추정(업데이트) 두개의 시퀀스를 가진다.

1. 이전의 데이터를 가지고 대강 다음에 들어올 입력값을 예상. (예측 단계)
2. 입력값이 들어온다. (입력 단계)
3. 입력값과 예측값을 비교해서 최적의 출력값을 추정(업데이트 단계)
   이때 나온 출력값은 다시 1번에서의 ‘이전의 데이터’가 된다.

1. 이전의 데이터를 가지고 다음에 들어올 입력값을 예상. (예측 단계)

칼만 필터의 예시

Constant Velocity Model
아주 간단하게 상태 모델을 $x = [r, v]$ (거리: r, 속도: v) 라고 한다. 우리는 연속적인 세계에 살지만, 디지털 세계 사람을 위해 샘플링 속도를 dt($\triangle t$)로 정의.

$\large{ x_{k-1}=[r_{k-1}, v_{k-1}] \; \; \; \; \; \; \; \Phi _{k}=\begin{bmatrix} 1 & \triangle t\\ 0 & 1 \end{bmatrix} }$ $\large{ x_{k}=\Phi_{k} x_{k-1} =\begin{bmatrix} 1 & \triangle t\\ 0 & 1 \end{bmatrix} \begin{bmatrix} r_{k-1}\\ v_{k-1} \end{bmatrix}=\begin{bmatrix} r_{k-1}+v_{k-1}\triangle t\\ v_{k-1} \end{bmatrix} }$

이 모델을 관찰하면 $x_{k-1}$과 $x_{k}$의 $v$ 차이가 없다. 이런 이유로 이 모델을 Constant Velocity Model(이하 CVM)이라고 부르는 것이다.

Constant Acceleration일땐?

$\large{ x_{k}=\Phi_{k} x_{k-1} =\begin{bmatrix} 1& \triangle t& \frac{1}{2}\triangle t^{2}\\ 0& 1& \triangle t\\ 0& 0& 1 \end{bmatrix} \begin{bmatrix} r\\ v\\ a \end{bmatrix} }$

물리에서 배웠던 등가속도 공식이 생각나지 않는가? 그거 맞다.

칼만필터의 역할이란??

예측값 $x_{k-1}$과 계측(관찰)값(나중에 나옴)이 차이(노이즈)를 가질때, 이것을 어떻게 보정할까??
그것이 칼만필터의 역할이다.

1.5 예측값의 분산의 계산 (공분산 계산단계)

공분산 행렬 $P_{k}$에 대해서 알아보자.

Process Noise Q

$\large{ x_{k}=\Phi_{k-1}x_{k-1}+w_{k-1}, w_{k} \sim N(0, Q_{k}) }$

어떤 정밀한 기계도 측정/예측에는 반드시 오차가 발생한다. 그래서 우리는 True Value를 알 수 없고, 불확실한 측정에 의해 True Value를 추정할 수 밖에 없다. 즉 어떠한 노이즈가 발생한다는 뜻이다. 노이즈를 이 식에서 $w_{k}$로 나타낸다.
여기서 잠깐, 아까 CVM에선 $x_{k}=\Phi_{k}x_{k-1}$ 였는데 왜 위의 식에서는 $\Phi_{k-1}$를 사용하는가? 이건 위에 공식이 등속도 운동이라 $\Phi$의 값이 항상 일정하기 때문이다.

State (co)variance $P$ (공분산 $P$)

$\large{ x_{k} \leftarrow P, x_k \sim N(x_k, P) }$

$P$는 칼만필터의 최종 추정치의 분산을 나타낸다. 즉, 우리는 $P$가 최소가 되도록 우리들의 추정치와 예측치를 사용해서 최적의 추정값을 찾아야 한다. 분산 $P$가 최소가 될때, 즉 중앙값에 수렴할 때 우리가 원하는 추정값(실제값과 일치하는)에 점점 더 가까워질 것이다. 공분산 $P$는 관측 전과 관측 후로 나뉘는데, 앞으로 관측 전의 $P$값을 $P(-)$, 관측 후의 $P$값을 $P(+)$라고 하기로 한다. 이는 $x_k$에도 마찬가지다.

공분산 행렬 $P_k$의 계산

오차의 정의: $e=$예측값-관측값
예측 오차

$\large{ \hat{x}_k(-)-x_k=\Phi_{k-1}(\hat{x}_{k-1}(+)-x_{k-1})+w_{k-1} }$ $\large{ x_k-E(x_k)=\Phi_{k-1}(x_{k-1}-E(x_{k-1}))+w_{k-1} }$

$\hat{x}$는 $x$의 예측값을 나타낸다. $E$ 함수는 Expected, 즉 기댓값,예측값을 말하는 것이다.

$Var[X]=E[ee^T]$: 분산은 오차의 제곱의 기댓값과 같다.

$\large{ P_k=E[(\hat{x}_k(-)-x_k)(\hat{x}_k(-)-x_k)^T] }$ $\large{ =E[\Phi_{k-1}(\hat{x}_{k-1}(+)-x_{k-1})(\hat{x}_{k-1}(+)-x_{k-1})^T {\Phi_{k-1}}^T+w_{k-1}{w_{k-1}}^T (C=AB, C^T=B^TA^T) }$

(이부분 수정필요) 여기부터 오차도 포함한다.

$\large{ P_{k-1}=\Phi_{k-1} P_{k-1} {\Phi_{k-1}}^T+Q_{k-1} (E[w_{k-1}{w_{k-1}}^T]=Q_{k-1}) }$

모델 $\Phi_{k-1}$와 ${\Phi_{k-1}}^T$ 사이에 공분산행렬 $P_{k-1}$을 넣으면, 다음 상태의 공분산행렬 $P_k$가 나온다.

2. 입력이 들어왔다. (관측)

관측 모델

$\large{ z_k=H_{k}x_{k}+v_{k} \; \; \; \; \; \; \; \; \; H=\begin{bmatrix} 1& 0\\ 0& 1 \end{bmatrix} \; \; \; \; \; \; \; \; \; v_{k} \sim N(0, R_{k}) }$

엥? 관측에 모델이 필요한가요? 그냥 본거 그대로 적으면 안되나요?
관측 노이즈값에 대한 표현이 필요하기 때문에 노이즈값을 포함한 모델을 고안한 것이다. 여기서 $v_k$가 노이즈를 의미한다. 좌표변환(자이로 센서, 가속도 센서)이 일어난다던지, 값이 뻥튀기 되어서 나온다던지, 아무튼 우리가 생각한대로 측정이 안된다.
위의 첫번째 공식에 $H$를 대입하면, $z_k=\begin{bmatrix} 1& 0\ 0& 1 \end{bmatrix}x_{k}+v_{k}$이다. $H$가 단위행렬인 이유는 거의 자기 자신을 의미하는 것이다. …왜 곱해주는거지? 보통은 다른 행렬이 들어가나 보다. 여기서는 쉽게 표현하기 위해 단위행렬로 표현한 듯 하다.

알파 베타 함수와 칼만 게인

$\large{ F_{t+1}=\alpha A_{t}+(1-\alpha)F_{t} \; \; \; \; \; \; (\alpha+\beta=1) }$

알파 베타 함수의 일종인 지수이동 평균 필터. $A$는 실측치, $F$는 필터링 후의 값. $\alpha$값만 가지고 들어오는 데이터의 가중치를 계산, 최종필터치를 계산하는 방식.
마치 저울질 하는거 같다?
$\alpha \beta$ 함수와 칼만 게인 구하는 법이 유사하다.

$\large{ \hat{x}_{k}(+)=\hat{x}_k(-)+K_{k}[z_{k}-H\hat{x}_{k}(-)] }$

여기서 $H$를 1로 바꾸면?? (단위행렬이니까 그래도 된다.)

$\large{ \hat{x}_{k}(+) = K_{k}z_{k} + (1-K_{k}) \hat{x}_{k}(-) }$

• 칼만 게인이 커지면 계측치 $z_k$를 신뢰
• 칼만 게인이 작아지면 예측치 $\hat{x}_{k}(-)$를 신뢰

3. 입력값과 예측값을 비교해서 최적의 출력값을 추정한다. (update)

관측 시퀀스 복습

$\large{ z_{k}=H_{k}x_{k}+v_{k} \; \; (v_k는 \; 노이즈) }$

이제 우리들은 앞서 설명한 칼만추정식에 관측 모델의 식을 대입해보려 한다.

$\large{ \hat{x}_{k}(+)=\hat{x}_{k}(-)+K_{k}[z_{k}-H_k \hat{x}_{k}(-)] }$

$K_k$는 칼만 게인, 결국 정체는 가중치다!! 즉, 퍼센트, 비율이다. 0~1 사이의 값이란 뜻이다…. “필터”는 값을 보정해준다는 뜻이었구나..
여기에 관측모델 $z_k$를 대입하면,

$\large{ \hat{x}_{k}(+)=\hat{x}_{k}(-)+K_{k}[H_{k}x_{k}-H_{k}\hat{x}_{k}(-)+v_k] }$

이 된다.

추정치와 오차의 공분산 행렬

오차 $e=\hat{x}_{k}(+)-x_k \; \; \; \; \; \; \; $ (관측후-실제값)
분산의 식에 대입하면

$\large{ P_{k}(+)=E(ee^T)=E[(\hat{x}_{k}(+)-x_{k})(\hat{x}_{k}(+)-x_{k})^T] }$

전개하면,

$\large{ P_{k}(+)=E[{(I-K_{k} H)(x_{k}=\hat{x}_{k}(-))-K_{k}v_{k}}{(I-K_{k} H)(x_{k}=\hat{x}_{k}(-))-K_{k}v_{k}}^T] }$

$\large{ P_k(+)=P_k(-)-K_{k}H_{k}P_{k}(-)-P_{k}(-)H_{K}^T K_{k}^T +K_{k}(H_{k}P_{k}H_{k}^T + R_{k})K_{k}^T }$

휴우 적느라 힘들었다. 이런 복잡한 식을 적은 이유가 무엇일까…. 그것은 이 식에 최소 공분산 행렬을 찾는 방법이 있기 때문이다.
$P_{k}$를 $K_{k}$에 대한 식으로 나타낸 것… 최소..공분산 행렬.. 감이 오는가? 함수의 최솟값을 찾는 방법은 바로 미분이다. 미분을 통해 미분값이 0이 되는 $x$점이 바로 함수의 최솟값이 되는 지점이다. 이 식에서 $x$축은 바로 $K_{k}$이다.

거의 다 왔다 근데 우리의 목표가 뭐였지?

추정치와 진치의 오차공분산행렬인 $P_{k}(+)$의 크기 최소화이다. 따라서 여기서 선형대수학으로부터의 꿀팁. trace 함수를 사용하는 것이다.
trace 함수는 대각성분의 합을 나타내는 함수이다. 대각성분의 스칼라합을 취한 후, 그 1차 미분이 0이 되는 부분을 취하면 최소지점을 찾을 수 있다.

$\large{ \frac{d}{dK_k} trace(P_{k}(+))=-2(H_{k}P_{k}(-))^T+2K_{k}(H_{k}P_{k}(-)H_{k}^T+R_k)=0 }$

이때의 $K_k$값이 $P_k(+)$가 최소가 되는 값이다.

$\large{ K_{k}=P_{k}(-)H_{k}^T \cdot [H_{k}P_{k}(-)H_{k}^T+R_{k}]^{-1} }$ $\large{ \frac{P_{k}(-)H_{k}^T}{H_{k}P_{k}(-)H_{k}^T+R_{k}} }$

여기서 $R_{k}$는 measurement error의 공분산이다. 아직 이게 뭔지 잘 모르겠으니 나중에 다시 보도록 하자..
아무튼 더 간단하게, $H_k=\begin{bmatrix} 1& 0\ 0& 1 \end{bmatrix}$로 두면,

$\large{ K_{k}=\frac{P_k(-)}{P_k(-)+R_{k}} }$

식이 아주 간단해졌다!

최종 정리

지금까지 공부했던 공식을 한 장에 정리해 보겠다.

이건 그냥 공식의 총집편 같은거고… 정말 중요한건 이거다.
칼만 필터 알고리즘을 사진 한 장만에 정리했다. 물론 내가 한건 아니다…
음 정말 알아듣기 쉽게 잘 요약되있다. 지금까지 공부한게 한번에 이해되는 느낌이다.
아직 예정은 없지만 칼만 필터에 대해 더 배운다면 더 올리도록 하겠다.

참고 사이트

Youtube: 손가락 TV 채널의 선형 칼만 필터의 기초 재생목록
사실 이 강의의 요약정리본이었다. 순전히 내가 공부하기 위해 작성한 게시글이므로 이 채널로 보는게 빠를지도?

Refstop

Deep Learning 및 SLAM을 공부하고 있습니다.

Comments

  Write a comment ...