Slam 7강 (Kalman Filter & EKF) 요약
Cyrill 교수님의 SLAM 강의를 듣고 정리해보자!
0. Today’s Goal
- Kalman Filter란?
- Kalman Filter 가정 증명 및 설명
- EKF란?
- Reference
1. 강의 및 강의 자료
강의 List는 SLAM KR의 SLAM DUNK seson 2에서 정해준 강의를 따라 정리를 할 것이다.
SLAM DUNK season 2의 Reference는 여기서 확인해 볼 수 있다.
SLAM KR에서 이 강의를 듣고 요약을 세미나 형식으로 준비를 해준다!
이번 포스팅은 다음의 강의들을 참고하여 요약을 해보았다.
Cyrill Stachniss의 강의 자료는 pdf로 다운 받을 수 있다.
2. Kalman Filter란?
Kalman Filter(칼만 필터)를 이해하려면 우선 Bayes Filter(베이즈 필터)에 대한 이해가 있어야 한다. Bayes Filter에 대한 내용은 Slam 6강 (Bayes Filter) 요약에서 확인할 수 있다.
칼만 필터는 Bayes Filter의 한 종류이고, 모든 분포가 가우시안 확률 분포로 되어있고 모델이 Linear system인 경우 사용할 수 있는 Filter이다. Bayes Filter처럼 Recursive한 Filter로 이전의 예측 값을 현재 예측을 하는데 사용을 하게 된다. Bayes Filter와 마찬가지로 Prediction step과 Correction step으로 두 단계로 나누어지게 된다.
칼만 필터는 trajectory estimation 분야에서 제안된 알고리즘이다. 이를 확장해서 현재는 Control, navigation 등등 다양한 분야에서 칼만 필터가 쓰이고 있다.
예를 들어 칼만 필터를 이해해보자. 아래의 그림처럼 검은색 점에 배의 현재 위치가 있고, 다음에 어디로 가야할지 예측하는 문제가 있다고 가정하자.
Prediction을 통해서 검은색 (X) 표시가 되어있는 곳으로 갈 것이라 예측했다.
이때 등대를 통한 Observation을 했고, 그 결과 Correction 과정을 통해 초록색 선까지의 길이를 관찰한 결과 아래 그림과 같이 초록색 (X) 표시가 내가 있는 위치라고 알게 되었다. 이 때 칼만필터 알고리즘을 활용해서 아래 그림과 같이 초록색 (X) 표시와 검은색 (X) 표시의 Weighted sum 계산을 통해 위치를 빨간색 (X) 표시로 나타낼 수 있다.
즉, 한번 더 정리를 하면 다음과 같다.
- 모든 확률 분포가 가우시안 분포를 가진다.
- 모델을 Linear하다.
두 가지 가정 조건을 만족할 때 사용하는 Bayes Filter의 한 종류이다.
Bayes Filter와 틀이 비슷하다.
3. Kalman Filter 가정 증명 및 설명
가정 증명
앞서 이야기했지만, 칼만 필터를 사용할 때는 두 가지 가정이 따른다.
- 모든 확률 분포가 가우시안 분포를 가진다.
- 모델을 Linear하다.
우선 가정을 만족해야 하므로 Kalman Filter를 설명하기 전 알고 있어야 할 선수 지식에 대해서 언급을 해주셨다. 칼만 필터를 방정식을 써서 풀어서 동작 원리를 설명해주신 느낌이다.
Marginalization을 했을 때도, Contditional probability를 따졌을 때도 모두 가우시안 확률 분포를 따르고 있다는 것을 명심해야 한다.
확률에 대한 개념은 아래의 포스팅들을 참고하자.
Linear model이란 모델이 Linear한 함수를 활용해서 표현이 가능한 모델을 말한다.
만약 Input이 가우시안 분포를 가진다면, Linear model의 Output도 가우시안 분포를 가지게 된다.
위 슬라이드에서 수식이 칼만 필터를 방정식으로 풀어 쓴 것이다. 뜻하는 것이 무엇인지 좀 더 자세하게 알아보자.
위 슬리아드에서 $n$은 state vector의 차원을 의미한다.
$l$은 control command($u$)의 차원을 의미한다.
강의에서는 Gaussian Distribution을 풀어서 쓰게 되는데 이를 이해하려면 아래와 같은 공식을 알고 있어여 한다.
자세한 내용은 The Multivariate Gaussian Distribution pdf 파일을 참고하자.
위 식을 이해하고 칼만 필터를 방정식으로 만든 것을 대입하면 아래와 같은 두 식을 만들어낼 수 있다.
위 두 슬라이드에 의해
\[p(x_t | u_t, x_{t-1}) , p(z_t | x_t)\]위 식이 가우시안 정규 분포를 따른 다는 것을 확인했다.
그렇다면 $bel$ 함수는 가우시안 분포를 따를까?
아래 슬라이드에서 알 수 있다시피 우리는 $\bar{bel}$ 함수가 가우시안 분포를 따른다고 가정했기 때문에 $bel$ 함수도 가우시안 분포를 따른다고 이야기 할 수 있다. 가우시안 분포의 곱은 가우시안 분포로 나오기 때문이다.
그렇다면 $\bar{bel}$ 함수는 가우시안 분포를 따를까?
아래 슬라이드에서 알 수 있다시피 $\bar{bel}$ 함수의 정의도 가우시안 분포의 곱으로 정의하기 때문에 가우시안 분포를 따른다고 할 수 있다. 하지만 여기는 초기의 $bel$ 함수도 가우시안 분포를 따른다는 것을 보여주어야 $\bar{bel}$ 함수의 가우시안 분포를 따른다는 것이 성립이 된다.
강의에서는 $\bar{bel}$ 함수가 가우시안 분포를 가지는 것에 대해 가우시안 분포의 곱과 Marginalization을 활용하여 풀어서 증명을 해준다.
결과만 이야기하면 모든 성분들은 가우시안 분포를 가진다는 것이다!
가우시안 분포를 표현할 때는 2가지의 parameter가 존재한다.
바로 평균(mean : $\mu$)과 공분산 행렬(covariance matrix : $\Sigma$)이다.
따라서 Bayes Filter에서 $bel$ 함수로 표현했던 부분을 단 두개의 매개변수 $\mu , \Sigma $로 나타낼 수 있다.
정리
위 모든 내용을 종합하여 칼만필터를 증명하는데 쓰인 특징들은 다음과 같다.
- 두 가우시안 확률 분포의 곱은 가우시안 확률 분포이다.
- Linear system에서 Input이 가우시안 확률 분포일 경우 Output도 가우시안 확률 분포이다.
- 가우시안의 Marginal and conditional distribution도 가우시안 분포이다.
- 가우시안 분포를 표현할 땐 평균과 공분산 행렬만 있으면 표현 가능하다.
- 역행렬 연산에 대한 성질도 쓰인다. - (참고 - 위키디피아)
따라서 칼만 필터의 psudo code를 서보면 아래와 같다.
Input으로는 t-1의 mean값, t-1의 covariance matrix 값,
control command 값 ($u_t$), observation 값 ($z_t$)이 들어간다.
Line 2,3에는 Prediction step이고, Line 4~6은 Correction step이다.
$A_t$는 앞서 정의했던 것과 마찬가지로 따른 Control 및 Noise를 제외한 $[t-1,t]$에서의 state 관계를 나타낸 Matrix이다.
$B_t$는 Control Input $u_t$와 state vector와의 관계를 나타낸 Matrix이다.
Prediction step에서 현재의 mean값과 covariance matrix 값을 예측하개 된다.
Correction step에서는 Observation 값을 이용하여 Prediction step에서 예측된 mean과 covariance matrix를 update하게 된다.
$K_t$는 Kalman Gain으로 Line 4에서 정의를 하고 (Weight sum을 위한 ratio라고 설명을 하셨다.), $C_t$는 앞서 정의했던 state vector와 observation 값의 관계를 설명하는 matrix이다.
Line 4에서 구한 Kalman Gain값을 이용하여 Prediction step에서 구했던 mean값과 covariance matrix 값을 현재의 mean, covariance matrix 값을 구한다.
Line 5에서는 mean값을 구하게 되는데 현재 알게 된 observation $z_t$와 이전에 구한 mean 및 $C_t$를 이용하여 현재 상태의 mean 값을 update한다.
Line 6에서는 covariance matrix를 업데이트 하는데, observation으로 인해 uncertainty가 줄어드는 방향으로 update를 진행하게 된다.
4. EKF란?
Kalman Filter는 Model이 Linear하고, 모든 확률 분포가 가우시안 확률 분포를 가질 때 사용하는 Filter이다. 따라서 이 가정이 깨지게 된다면 Kalman Filter는 제대로 동작하지 않는다.
하지만 실제 세계는 이러한 가정을 지키지 못하는 경우가 훨씬 많다. 예를 들어 2D plane에서의 Localization을 생각해보더라도 state vector에 방향에 대한 값을 추가해 주는데, 이때 sine과 cosine 값이 들어가기 때문에 model이 Non-linear하게 된다.
따라서 Kalman Filter를 확장시켜 Non-linear한 상황에서도 쓸 수 있게 해주는 Filter가 EKF이다.
EKF는 Extended Kalman Filter의 줄임말이다.
EKF는 말 그대로 Kalman Filter의 확장 버전이다. 단지 Model이 Non-linear하게만 정의 되었을 뿐 알고리즘의 흐름은 Kalman Filter와 비슷하다.
우선 Non-linear한 model은 아래 슬라이드와 같이 정의할 수 있다.
이러한 Non-linear한 function이 문제가 되는 이유는 가우시안 확률 분포를 가지는 Input을 model에 넣었을 때 가우시안 확률 분포를 가지지 않는 Ouput이 나오기 때문이다.
따라서 이러한 문제를 해결하기 위해 Local Linearization이라는 과정을 거친다.
Linearization은 1차 테일러 급수식(first order taylor expansion)으로 선형화를 진행하게 된다.
EKF도 칼만 필터와 마찬가지로 Prediction step과 Correction step을 거치는데, 이 때 필요한 값을 아래 슬라이드와 같이 정의한다. 이 때 Jacobian도 쓰이게 된다.
Local linearization을 하는데 영향을 미치는 요소는 크게 두 가지가 있다.
- 1차 테일러 급수로 선형화한 값과 실제 Non-linear한 model의 차이
- Input의 uncertainty (= Covariance Matrix)
Input의 uncertainty가 작으면 표준편차가 작으므로, 확률 분포가 좁게 형성되고, 이는 테일러 급수로 선형화한 모델과 실제 Non-linear한 model의 차이를 적게 만들게 된다.
EKF를 식으로 조금 더 자세하게 알아보자.
칼만 필터와 마찬가지로
\[p(x_t | u_t, x_{t-1}) , p(z_t | x_t)\]두 개의 확률 분포를 구해보면, 아래 슬라이드와 같이 Model을 Linearized하게 만들어줬기 때문에 가우시안 확률 분포로 나오는 것을 알 수 있다.
EKF를 pseudo code로 살펴보면 아래와 같다.
기존의 칼만 필터와 엄청 유사하지만 $A_t, C_t$가 Jacobian matrix $G_t, H_t$로 바뀌었다는 것을 주의하자!
만약 Observation model(Sensor)가 Noise가 없다면 계산은 어떻게 될까?
Line 4~5를 계산해보면 알겠지만, $\mu_t = (H^T_t)^{-1} * z_t$ 이 된다.
이 말은 현재 들어온 observation vector만을 이용하여 현재의 mean 값을 update한다는 이야기이다.
반대로 Observation model(Sensor)가 Noise가 엄청 많다면 계산은 어떻게 될까?
Noise를 나타내는 matrix인 $Q_t$가 무한한 값을 가지게 되고, 그 말은 Kalman Gain 값($K_t$)이 0이라는 이야기이다.
따라서 이전에 prediction step에서 예측한 mean값이 현재의 mean값으로 update가 된다.
EKF는 다양한 예로 쓰일 수 있는데 Localization을 할 때 다음과 같이 쓰일 수 있다.
마지막으로 EKF를 정리하면 다음과 같다.
- Kalman Filter의 확장 버전이다.
- 1차 테일러 급수식을 활용하여 Non-lieanr model을 Local linearization을 시도했다.
- Input sensor의 Uncertainty가 커진다면, 선형화 값이 부정확할 수 있다.
댓글남기기