3.2 The Kalman Filter

이 글은 Sebastian thrun의 Probabilistic Robotics를 보고 내용을 정리한 글이며, 나름 쉽게 표현하기 위해서 의역을 한 부분이 있습니다.

​

앞으로 포스팅 시 가우시안 분포와 정규분포를 혼용해서 사용하는데, 같은 의미로 생각하시면 됩니다.

밀도와 확률밀도 또한 혼용해서 사용하는데, 이 또한 같은 의미로 생각하시면 됩니다.

 

칼만 필터의 유도 과정은 생략하였습니다.

 

1. 선형 가우시안 시스템(Linear Gaussian Systems)

칼만 필터는 Swerling과 Kalman에 의해 발명되었으며, 선형 가우시안 시스템에서 필터링과 예측(predcition)을 하기 위한 테크닉입니다. 칼만 필터는 belief 계산을 연속적인 상태(continuous state)에서 시행합니다. 칼만 필터는 불연속(discrete) 또는 혼합(hybrid) 상태 공간에서는 적절하지 않습니다.

​

칼만 필터는 belief를 moments parameterization에 의해 나타냅니다. 즉, 시간 $t$에서 belief는 평균 $\mu_t$, 공분산 $\Sigma _t$ 로 나타냅니다. 베이즈 필터(bayes filter)의 마르코프 가정(Markov assumption)에 더해 아래의 세 속성을 만족하면, 사후 확률은 가우시안 분포를 따른다고 합니다.

 

1. 상태 과도 확률(state transition probability) $p(x_t | u_t , x_{t-1})$은 가우시안 분포의 noise가 더해진 선형 함수이어야 하며, 다음과 같이 나타낼 수 있다.

 

여기서 $x$와 $x_{t-1}$은 상태 벡터이며, $u_t$는 시간 t에서의 컨트롤 벡터이며, 두 벡터 모두 열벡터이며, 다음과 같이 나타낸다(책에서 왜 볼드체로 쓰지 않았는지 모르겠습니다.)

 

여기서 $A_t$와 $B_t$는 행렬이며, $A_t$는 $n\times n$ 크기의 정사각행렬이고, $n$은 상태 벡터 $x_t$의 차원이다.

$B_t$는 $n\times m$의 행렬이고, $m$은 컨트롤 벡터 $u_t$의 차원이다. 행렬 $A_t$와 $B_t$를 각각 상태 벡터와 컨트롤 벡터에 곱함으로써 상태 과도 함수는 선형이 되고, 따라서 칼만 필터는 동적 선형 시스템(linear system dynamics)를 가정한다.

($A_t$는 컨트롤 입력 $u_t$와 노이즈 $\varepsilon _t$가 없을 때 $t-1$과 $t$의 상태(state)가 어떻게 관계되어있는지 나타내는 행렬입니다.

$B_t$는 컨트롤 입력 $u_t$가 어떻게 상태의 변화에 영향을 미치는지 나타내는 행렬입니다.)

 

식 3.2의 랜덤변수 $\varepsilon _t$ 는 과도 상태(state transition)의 불확실성(uncertainty)를 나타내는 가우시안 랜덤 벡터이며, 차원은 상태 벡터와 같다. 이 벡터의 평균은 0이고, 공분산은 $R_t$로 나타낸다. 식 3.2는 상태 과도 확률(state transition probability) $p(x_t | u_t , x_{t-1})$을 정의한 것이며, 이 확률은 식 3.2를 다변량 정규분포(multivariate normal distribution)의 정의에 대입한 것이다. 사후 확률의 평균은 $A_tx_{t-1} + B_tu_t$, 공분산은 $R_t$로 주어진다.

 

2. 측정 확률 $p(z_t | x_t)$ 또한 선형이어야 하며, 가우시안 노이즈가 더해진 것으로 다음과 같이 나타낸다.

 

여기서 행렬 $C_t$의 크기는 $k \times n$ 이며, 여기서 $k$는 측정 벡터 $z_t$의 차원이다. 벡터 $\delta _t$는 측정 노이즈를 나타낸 것이다. $\delta _t$는 평균이 0이고 공분산이 $Q_t$인 다변량 정규분포이다. 따라서 측정 확률은 다음과 같이 다변량 정규분포를 따른다.($C_t$는 현재 로봇의 상태와 센서의 관측 정보가 어떤 관계인지 나타내는 행렬입니다.)

 

 

3. 마지막으로, belief의 초깃값 $bel(x_0)$은 평균이 $\mu _0$이고 공분산이 $\Sigma _0$인 정규분포이다.

 

위 세 가지 가정은 사후확률 $bel(x_t)$가 어떤 시간 $t$에서도 항상 정규분포를 따른다고 확신하기에 충분합니다.

 

 

2. 칼만 필터 알고리즘(The Kalman Filter Algorithm)

Table 3.1. 선형 가우시안 상태 과도(linear Gaussian state transitions)와 측정의 칼만 필터 알고리즘

칼만 필터는 시간 $t$에서의 belief $bel(x_t)$를 평균 $\mu _t$와 공분산 $\Sigma _t$로 나타냅니다. 칼만 필터의 입력은 시간 $t-1$에서의 $\mu _{t-1}$와 $\Sigma _{t-1}$로 나타낸 belief입니다. 이 파라미터들을 업데이트하기 위해, 칼만 필터는 컨트롤 $u_t$와 측정값 $z_t$를 필요로 합니다. 출력은 시간 $t$에서의 $\mu _t$와 $\Sigma _t$로 나타내어진 belief입니다.

 

위 알고리즘의 line 2, 3에서 예측된 belief $\overline{\mu}$와 $\overline{\Sigma}$는 1 타임 스텝 후의 belief인 $\overline{bel}(x_t)$를 나타내지만, 아직 측정값 $z_t$는 포함되지 않았습니다. 이 belief는 컨트롤 $u_t$를 포함하는 것에 의해서 얻을 수 있게됩니다. 평균은 상태 과도 함수의 결정적(deterministic)버전인, 식 3.2에서 $x_{t-1}$을 평균 $\mu _{t-1}$로 치환한 것을 이용하여 업데이트됩니다. 공분산의 업데이트는 상태가 선형 행렬 $A_t$를 통해 이전의 상태에 의존한다는 것을 고려합니다.

 

belief $\overline{bel}(x_t)$은 다음으로 line 4부터 6을 통해, 측정값 $z_t$를 포함한 계산에 의해 우리가 원하는 belief $bel(x_t)$로 변환됩니다. line 4에서 계산된 변수 $K_t$는 칼만 이득(Kalman gain)이라고 합니다. line 5는 칼만 이득 $K_t$, 실제 측정값 $z_t$의 편차, 식 3.5의 측정확률에 따라 예측된 측정값에 비례하여 평균을 조정합니다. 여기서 중요한 컨셉은 line 5에서의 실제 측정값 $z_t$와 측정 기댓값 $C_t\overline{\mu}_t$의 차이 입니다.

 

쉽게 이해하도록 예를 들어보겠습니다(jinyongjeong.github.io/2017/02/14/lec03_kalman_filter_and_EKF/ 를 참조했습니다.) 주변 landmark까지의 거리를 측정할 수 있는 로봇이 있다고 가정하겠습니다. 만약 엔코더 데이터와 motion model에 의해 로봇의 예상 위치를 알고있고, 주변의 landmark의 위치를 이미 알고 있다면, 예상되는 landmark까지의 거리 또한 계산이 가능합니다. 만약 예상 거리가 10m인데 측정값이 9m라면, 이는 오도메트리를 통한 현재 위치의 계산에서 외란이나 슬립 등에 의한 오차가 생긴 것일 수 있고, 센서의 오차일 수도 있습니다. 여기서 칼만 이득이 10m와 9m중 어떤 데이터를 더 신뢰할지 결정하는 것입니다.

 

더 쉽게 이해하기 위해 관측값의 공분산인 $Q_t$를 무한대라고 하면, 센서의 데이터는 전혀 신뢰하지 못한다는 뜻 입니다. 이때 $K_t = 0$ 이되며, $\mu _t = \overline{\mu} _t$가 됩니다. 즉, 센서 데이터는 무시하고 예상되는 state를 그대로 현재 state로 정하겠다는 의미입니다. 반대로 $Q_t$ 가 0이라고 하면, 이는 센서 데이터를 100%신뢰한다는 의미입니다. 따라서 이 때는 $K_t = C_t^{-1}$이 되며, $\mu_t = C_t^{-1}z_t$입니다. 즉, 예상 위치는 무시하고 센서 데이터만으로 현재 state를 추정하겠다는 의미입니다.

​

마지막으로, 사후 확률 belief의 새로운 공분산은 6번째 줄에서 계산되며, 측정값으로부터의 결과의 정보 이득을 조정함으로써 계산됩니다.

​ 

 

3. 과정(Illustration)

그림 3.2. (a)는 초기의 belief, (b)는 불확실성(uncertainty)과 관련된 측정값(굵은 그래프), (c)는 칼만 필터 알고리즘을 사용한 후의 belief, (d)는 오른쪽으로 움직이는 동작 후의 belief(불확실성이 증가함), (e)는 불확실성과 관련된 새로운 측정값, (f)는 (c)와 비슷한 결과를 나타냄

 

그림. 3.2는 간단한 1차원 위치 추정(localization)의 과정을 나타냅니다. 로봇이 수평 축을 따라서 오른쪽으로 움직인다고 가정해보겠습니다. 그림 3.2의 (a)에서처럼 로봇의 초기 위치는 정규분포를 따른다고 하겠습니다. 그 후 로봇이 해당 위치를 중심으로 센서 측정을 하면, (b)와 같이 피크(peak)가 있는 가우시안 그래프를 반환(return) 합니다. 여기서 피크 값은 센서에 의해 예측되는 값이고, 이것의 넓이(분산, varience)는 이 측정에서의 불확실성을 나타냅니다. 측정값을 통해 사전 확률을 구성하는 데 있어서, 칼만 필터는 알고리즘의 4~6번째 줄을 통해 그림 3.2의 (c)와 같은 그래프를 만들어냅니다. 이 belief의 평균은 두 개의 원래 그래프(a와 b)의 평균 사이에 있으며, 이 그래프의 불확실성은 두 개의 원래 그래프보다 작습니다.

 

다음으로, 로봇이 오른쪽으로 움직였다고 가정해보겠습니다. 상태 과도(state transition)는 확률적이기 때문에 불확실성이 증가하게 됩니다. 칼만 필터 알고리즘의 2, 3번째 줄은 그림 3.2의 (d)의 굵은 그래프를 보여줍니다. 이 가우시간 그래프는 로봇이 움직인 만큼 이동하게 되며, 그만큼 분포 또한 더 넓어지게 됩니다. 그 후, 로봇은 두 번째 측정값을 바탕으로 그림 3.2의 (e)에서의 굵은 그래프와 같은 가우시안 그래프를 만들게 되며, 최종적으로 그림 3.2의 (f)와 같은 사후 확률을 얻게 됩니다.

​

이 예시는 칼만 필터 알고리즘이 측정 업데이트 스텝(measurement update step, 알고리즘 5~7줄)을 반복하며, 이때 센서의 데이터가 현재의 belief와 예측 스텝(predction step)에 사용되고, 이 값은 로봇의 동작(action)에 따라 belief를 바꿉니다. 따라서, 업데이트 스텝은 로봇의 belief의 불확실성을 감소시키고, 예측 스텝은 불확실성을 증가시키게 됩니다.