[Kalman filter - 2] 칼만 필터의 기초

https://talktato.tistory.com/72

[Kalman filter - 1] Average Filter, Moving Average Filter, Low-pass Filter

칼만 필터에 앞서 기본적인 필터에 대한 이전 포스팅입니다.

 

 

Kalman Filter

이전 포스팅에서 다뤘던 1차 low pass filter 수식은 아래와 같습니다.

$$\bar{x}_k = \alpha \bar{x}_{k-1} + (1 - \alpha)x_k, \space (0 < \alpha < 1)$$

해당 수식은 직전 추정값 ($\bar{x}_{k-1}$)과 현재 시점의 측정값($\bar{x}_k)으로만 이뤄져 있는 거에 비해 칼만 필터는 조금 더 복잡한 계산과 변수들이 등장합니다. 

 

먼저 칼만 필터의 수식을 제외하고 알고리즘의 흐름을 먼저 살펴보겠습니다.

흐름은 크게 두 가지로 분류할 수 있습니다.

1. Predict
   이전 시점(t-1)의 상태를 기반으로 현재 시점(t)의 상태를 예측하는 단계입니다.
   이때는 이전 시점의 상태를 기반으로 수학적으로 모델링 된 시스템 모델에 의해 현재 시점의 상태를 예측하게 됩니다. 따라서 이 시스템 모델이 실제 적용하려는 시스템과 유사도에 따라서 Predict 과정의 성능이 좌우됩니다.
   
   
2. Update
   현재 시점(t)의 실제 측정값(measurement)들을 기반으로 현재 상태를 추정하고 Predict step에서 예측한 현재 시점의 상태와 비교하여 실제로 현재 시점의 상태를 update 합니다. 이때 시스템 모델과 실제 측정값의 신뢰도를 고려해 예측 값과 추정 값을 반영한 최종적인 칼만 필터의 output인 추정 값을 계산합니다.

초기 값을 적절하게 선정한 뒤 Predict step과 Update step을 계속 반복하는 재귀적인 연산을 통해 현재 상태의 값을 추정하는 알고리즘입니다.

 

 

이제 칼만 필터의 계산 과정을 수식과 함께 알아보겠습니다.

 

그전에 수식에 사용되는 변수들을 정리하고 시작하겠습니다.

각각이 무엇을 의미하는지는 잠시 뒤 다룰 테니 지금은 어떤 변수들이 있고 각각 수식에 어떻게 사용되는지 정도만 보고 넘어가면 됩니다.

 

input으로 사용되는 현재 시점 k의 측정값 ($z_k,\space (m \times 1)$), 최종 output인 현재 시점의 추정 값 ($\hat{x}_k,\space (n \times 1)) $.

시스템 모델 행렬 A($n \times n$), 출력 행렬 H($m \times n$), 시스템 모델의 noise 공분산 행렬 Q($n \times n$), 측정값의 noise 공분산 행렬 R($m \times m$), k시점의 상태 $x_k$, k 시점의 오차 공분산 $P_k$.

 

먼저 알고리즘에 사용될 초깃값($\hat{x}_0$, $P_0$) 들을 설정 해줍니다.

그 이후는 predict step으로 현재 시점 k의 상태와 오차 공분산을 예측합니다.

$$\hat{x}^-_k = A\hat{x}_{k-1}$$

$$P^-_k=AP_{k-1}A^T+Q$$

위에서 위 첨자 '-'는 예측 값을 의미합니다.

예측 과정에서는 모델링한 시스템 모델(A)에 의해서 시각이 k-1에서 k로 바뀔 때 추정 값이 어떻게 변할지 예측하는 과정입니다.

또한 이때는 시스템 모델에도 noise가 존재할 수 있기 때문에 Q를 사용하여 예측을 진행합니다.

 

 

추정값 $\hat{x}_k$을 구합니다.

$$\hat{x}_k=\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k)$$

해당 과정에서는 현재 시점에서 실제로 측정한 측정값 $z_k$과 앞서 계산한 kalman gain $K_k$ 이용하여 추정 값을 계산합니다.

 

수식 $\hat{x}_k=\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k)$를 전개해서 다시 풀어보면 아래의 수식으로 변환할 수 있습니다.

$\hat{x}_k=(I-K_kH)\hat{x}^-_k+K_kz_k$로 표현할 수 있고 이때 H를 단위행렬로 가정한다면 $\hat{x}=(I-K_k)\hat{x}^-_k+K_kz_k$가 됩니다.

마지막 수식을 잘 보면 결국 현재 시점의 kalman gain $K_k$에 의해서 예측 값($\hat{x}^-_k$)와 측정값($z_k$)에 가중치가 곱해져 최종 추정 값($\hat{x}_k$)를 구합니다. 따라서 칼만 필터의 핵심은 각 예측 값과 측정값에 사용되는 가중치를 재귀적으로 매 step마다 조정을 하여 계산하여 사용한다는 점입니다.

 

이 수식을 조금 더 설명해 보면 $H\hat{x}^-_k$는 예측 값($\hat{x}^-_k$)을 통해 계산한 측정값을 의미합니다. $z_k-H\hat{x}^-_k$는 결국 k 시점에서 실제로 측정한 값과 예측한 측정값 간의 차이, 즉 측정값의 예측 오차를 의미합니다.

따라서 kalman gain과 예측 오차를 구해서 예측했던 값을 보정하여 최종 추정 값을 구하는 것이 됩니다.

 

따라서 추정 값을 구하기 전에 kalman gain을 먼저 구합니다.

$$K_k=P^-_kH^T(HP^-_kH^T+R)^{-1}$$

measurement의 noise인 R이 kalman gain을 구할 때 사용이 되어 반비례 관계를 가집니다.

따라서 R이 크다면 K가 작아지고 이는 최종 추정값을 구할 때 측정값($z_k$) 보다 예측 값($\hat{x}^-_k$)를 더 많이 반영한다는 의미입니다. 즉 측정값의 noise가 크다면 측정값보단 시스템 모델의 예측 값을 더 신뢰하고 가중치를 높입니다.

 

이와 비슷하게 Q의 경우를 살펴본다면, predict step에서 $P^-_k=AP_{k-1}A^T+Q$로 오차 공분산을 계산합니다. 따라서 시스템 노이즈 Q가 크다면 $P^-_k$가 커지게 되고 이는 곧 kalman gain의 값도 커져서 예측 값($\hat{x}^-_k$)보다 측정값($z_k$)을 최종 추정 값으로 더 많이 반영하게 됩니다.

 

마지막으로 오차 공분산 $P_k$을 구합니다.

$$P_k=P^-_k-K_kHP^-_k$$

해당 오차 공분산 $P_k$는 칼만 필터 결과의 추정 값이 실제 정답 값과 얼마나 차이가 나는지 보여주는 척도가 됩니다. $P_k$가 크다면 추정 오차가 크고, 반대로 작다면 추정 오차가 작다는 의미입니다.

 

이러한 과정을 재귀적으로 계산하여 실제 측정값들을 기반으로 실제 정답 값과 유사하도록 필터링하는 역할을 합니다.

 

시스템 모델

그렇다면 여기서 사용하는 시스템 모델에 대해서 알아보겠습니다.

시스템 모델은 현재 다루고자 하는 문제를 수학적으로 표현해 놓은 것을 의미합니다. 

칼만 필터는 linear 시스템 가정합니다. 따라서 아래와 같이 표현될 수 있습니다.

$$x_k=Ax_{k-1}+w_{k-1}$$

$$z_k=Hx_{k-1}+v_{k-1}$$

이때 $x_k$는 상태(state) 변수 벡터, $z_k$는 측정값 벡터, $A$는 시스템 모델 행렬, $H$는 출력 행렬, $w_k$는 시스템 noise 벡터, $v_k$는 측정치의 noise 벡터입니다.

 

행렬 $A$는 시간에 따라 시스템이 어떻게 움직이는지 나타내고 $H$는 측정값과 상태 변수 사이의 관계를 나타냅니다.

이렇게만 보면 뭔가 와닿지 않으니 예시를 하나 들어보겠습니다.

위치로 속도를 추정해야 하는 문제가 있다고 가정해 봅시다. 속도는 이동 거리를 시간으로 나누면 쉽게 구할 수 있습니다. 하지만 측정값의 noise로 인해서 속도 값은 매우 불안정하게 계산될 것입니다. 이러한 유형에서 칼만 필터는 안정적으로 속도를 구해낼 수 있습니다.

 

이제 칼만 필터를 위한 시스템 모델을 설계해 봅시다.

먼저 위치를 기반으로 속도를 추정해야 한다고 하면 위치와 속도 두 가지를 가지고 상태 변수를 정의합니다.

$$x = {position, velocity}$$

$$x_k=Ax_{k-1}+w_{k-1}$$

$$z_k=Hx_{k-1}+v_{k-1}$$

 

그다음 '현재 위치 = 이전 위치 + 속도 $\times$ 이동 시간'이라는 물리 법칙을 수식적으로 사용한 시스템 모델을 설계합니다.

$$A=\begin{bmatrix}1 & \Delta t \\ 0 & 1 \end{bmatrix}$$

$$H=\begin{bmatrix}1 & 0 \end{bmatrix}$$

이때 $\Delta t$는 측정하는 시간의 간격을 의미합니다.

 

위의 시스템 모델이 잘 동작하는지 풀어나가보며 확인해 봅시다.

$$x_k=Ax_{k-1}+w_{k-1} $$

$$\space = \begin{bmatrix}1 & \Delta t \\ 0 & 1\end{bmatrix} x_{k-1} +w_{k-1}$$

여기에 이전에 정의했던 상태 변수를 대입해 보면 직관적으로 이해가 쉽습니다.

 

$$\begin{bmatrix}position \\ velocity \end{bmatrix}_k = \begin{bmatrix}1 & \Delta t \\ 0 & 1\end{bmatrix} \begin{bmatrix}position \\ velocity \end{bmatrix}_{k-1}+\begin{bmatrix}0 \\ w_{k-1}\end{bmatrix}$$

$$=\begin{bmatrix}position + velocity\space \dot \space velocity \\ velocity + w\end{bmatrix}_{k-1}$$

위 식에서 $velocity_k=velocity_{k-1}+w_{k-1}$이 의미하는 바는 속도는 시스템 noise에 의해서만 변하고 등속 운동을 하고 있다는 의미입니다.

 

측정값에 대한 관계식에 대해서 보겠습니다.

$$z_k=Hx_k+v_k$$

$$=\begin{bmatrix}1 & 0\end{bmatrix}\begin{bmatrix}position \\ velocity\end{bmatrix}_k+v_k$$

$$=position_k + v_k$$

위 식의 의미는 시스템에서 우리가 측정할 수 있는 값은 위치(position) 값이고 여기에는 측정치에 대한 noise가 포함되어 있다는 의미입니다.

 

추가적으로 noise는 어떻게 설계해야 할까요?

칼만 필터에서는 noise는 평균이 0인 정규 분포를 따른다는 것을 가정하고 있기 때문에 잡음의 공분산을 구해서 사용하면 됩니다.

따라서 $w_k$의 공분산 행렬인 Q와 $v_k$의 공분산 행렬인 R(Q와 R은 모두 대각 행렬)을 구성해서 사용합니다.

시스템의 특성을 정확하게 반영하여 Q와 R을 구성하면 좋겠지만, 현실적으로 통계적으로 noise를 완벽하게 구성하는 것은 불가능에 가깝습니다. 따라서 앞서 설명했던 Q, R과 kalman gain의 관계를 통해 현재 시스템의 특성에 맞춰 noise를 구성하는 것이 필요합니다.

noise 또한 설계, 튜닝의 영역으로 생각하고 많은 trial & error를 겪으며 적합한 값을 찾아가는 것이 필요해 보입니다.