SLAM 에서의 Kalman filter : 콘셉트부터 EKF 까지

Introduction

SLAM 은 State 의 확률 분포를 Estimation 하는 문제입니다. 

우리는 이때 Markov 가정을 적용해서 내가 현재의 State 를 알기 위해서는
1. 이 전 단계의 state 와 
2. 이 전 단계에서 현재의 state 로 오기 위한 명령
3. 현재의 state 에서 주변 환경으로부터 획득한 정보
이 세 가지의 값이 있으면 매 단계마다 state 를 update 할 수 있습니다. 
 
그래서 이러한 예측에 전통적으로 많이 사용되었던 Bayes 필터는 두 가지의 단계가 있습니다. 첫 번째는 Prediction, 두 번째는 Correction 인데요, 
Prediction 은 과거의 State 와 Control 을 이용해서 현재의 State 를 예측하는 단계입니다. 어떤 예측을 수행했으면 내가 잘 예측했는지 판단할 수 있는 기준이 있어야겠죠 ? 그 기준을 우리는 Measurement 를 통해서 내가 한 Prediction 이 옳은지 , 만약 틀렸다면 내가 어떻게 예측을 수정해야 하는지 알 수 있고, 이 단계를 우리는 Correction 단계에서 수행하게 됩니다. 
 
이것이 Bayes 필터의 기본적인 프레임워크입니다. 여기서 우리는 Prediction 을 어떻게 해서 확률 분포를 구할 것이냐, 또는 Correction 에서 어떤 방식으로 확률값을 수정할 것인가에 대한 고민을 하게 됩니다. 
 
이 중 대표적인 방법이 칼만 필터입니다. 칼만 필터는 베이즈 필터의 일종이라고 할 수 있습니다. 
 

Kalman filter

칼만 필터는 두 가지의 특징이 있습니다. 
첫 번째는 State 와 State 의 관계가 Linear 하다는 것입니다. Linear 라고 하는 것은 곱하기와 더하기로 이루어진 식을 통해서 새로운 State가 유도된다는 것입니다. 
두 번째는 이 모든 확률 분포를 Gaussian 분포로 나타낸다는 것입니다. 이 정규분포가 참이냐 아니냐를 떠나서, 정규분포가 가지고 있는 매우 큰 장점이 있습니다. 바로 어떤 분포가 두 개의 매개변수로 표현된다는 것입니다. 
 
그러면 이걸 왜 필터라고 부르는 것일까요 ? 

이 칼만 필터에는 어떤 가정과 Measurement 가 들어오게 됩니다. 다만 이 Measurement 는 세상의 모든 것이 그렇듯 노이즈를 포함할 수 밖에 없습니다. Initial Assumption, 즉 우리가 계산한 것과 측정한 것이 있을 때 칼만 필터를 통과시키면 노이즈와 불확실성은 제거하고 그나마 정확하다고 할 수 있는 값만 밖으로 빼낸다는 의미에서 우리는 칼만 필터를 "필터" 라고 얘기합니다.
 

1D Case

우선 간단한 이해를 돕기 위해 1차원을 먼저 짚고 넘어가도록 하겠습니다. Prediction은 이전 state 에서 현재 state 의 값을 추정하는 것ㅇ니데, 이때 Gaussian Distribution 의 합으로 추정하게 됩니다. 
 
그래서 다음 식이 뜻하는 것은, 이전의 분포들이 있을 때 새로운 분포는 어떤 값을 가지게 되는가? 하는 것입니다. 평균끼리는 더하고, 분산끼리 더하는 것이 새로운 분포의 평균과 분산이라는 것입니다. 따라서 이전 State에 모션을 준 새로운 State 는 두 평균 및 분산을 더함으로써 새로운 Distribution 으로 정의할 수 있습니다. 
 

추정을 했다면 이 값을 실제 Measurement 와 비교하여 최종적인 값을 update 해야 합니다. 좀 더 믿을 만한 쪽에 높은 가중치를 두어서 업데이트 해야겠지요 ?
그러므로 이때는 두 가우시안 분포를 곱합니다. 그림을 보면, 최종적으로 나온 Posterior belief 라는 것은 measurement 쪽에 가깝지요 ? 이는 Measurement 의 variance 가 prior belief 의 variance 보다 작기 때문에 measurement 에 더 가깝도록 평균이 정해진다는 것입니다. 

Multivariate

 

이제 실전, 즉 우리가 실제로 사용하는 Mutlivariate 상황을 다루어 봅시다.  이때 P 는 covariance matrix 인데, variance 가 scalar 에 대해서 분산을 나타낸 것이라면 어떤 random variable 이 vector 라면 variance 는 covariance 가 되는 것입니다.
 

EKF (Extended Kalman Filter)

 
가우시안 분포에는 매우 취약한 면이 있습니다. 모든 연산이 Linear 해야 한다는 것입니다. (그래야 가우시안 분포가 유지가 되니까요)
다만, 많은 분야에서 실제 문제를 모델링 할 때는 Linear 한 수식으로 표현하지 못 하는 경우가 대부분입니다. 삼각함수, 로그, Square root 같은 것들은 linear 하지 않으니까요. 
 
따라서 칼만 필터의 전제 조건인 , "모든 분포는 정규분포를 따른다" 라는 가정이 성립하지 않으니 많은 문제에 대해서 칼만 필터를 사용할 수 없게 되었습니다. 여기서 우리는 두 선택지가 있습니다. 1) Non-linear 할 때는 칼만 필터를 쓰지 말자. 2) 비선형 적인 문제를 선형적으로 바꿔 보자. 
 
EKF 는 이 중 두 번째 선택지를 따른다고 할 수 있습니다. EKF 는 다음과 같은 아이디어로 시작합니다. 
"전체 시스템 자체가 비선형적이라고 하더라도, 현재 x라고 하는 state 주변만 선형으로 가정하자."
이 아이디어는 다른 말로 Local Linearization 이라고도 합니다. 삼각 함수같은 곡선 형태의 수식이라도, 국소적으로는 직선으로 이루어져 있다는 것입니다.
우리는 어차피 state 에서 다음 state 로의 변환만 계산을 하는 것인데, 전체적인 식이 비선형이라고 하더라도 그 주변은 선형으로 근사해서 계산한다면 계속해서 칼만 필터를 사용할 수 있다는 것입니다. 
 

Taylor Series

 
Non-linear 한 함수를 linear 한 equation 으로 근사를 할 때 사용하는 것이 바로 테일러 급수입니다. 테일러 급수는 파라미터로 표현하기 어려운 함수도 급수를 통해서 근사화할 수 있다는 것을 의미합니다. 

 

 
이런 Non-linear 함수에 대해서 First-order 테일러 함수를 사용하기 위해서는 1차 도함수를 계산해야 하는데요, 이 1차 도함수를 우리는 자코비안 행렬이라고 이름 붙였습니다. 자코비안 함수는 모든 행, 열에 대해서 각각의 항으로 편미분 한 것입니다. 자코비안에서 몇 행, 몇 열이라고 하는 것은 몇 번째 Equation 을 몇 번째 변수로 미분한 것입니다. 

자코비안 행렬을 물리적인 의미로 보면 어떤 한 지점에서의 법면이라고 할 수 있습니다.

따라서 우리는 Gt, Ht 에 대한 자코비안 행렬을 구하고 거기에 (xt-ut) 를 곱함으로써 기존 입력의 Distribution 을 선형으로 변환하는 효과를 가지게 됩니다.

---

위 내용은 경희대학교 소프트웨어융합학과 황효석 교수님의 2021년 <로봇프로그래밍> 수업 내용을 요약한 것입니다.