[SLAM] Kalman Filter & Extended Kalman Filter

Kalman Filter & Extended Kalman Filter에 관한 포스팅입니다.

Bayes’ Filter & Kalman Filter
Kalman Filter Example
Kalman Filter Concept
Kalman Filter : Application
Extended Kalman Filter (EKF)
참고자료 & 이미지 출처

Bayes’ Filter & Kalman Filter

Bayes’ Filter & Bayes’ Theorem

Kalman filter는 Bayes Filter의 한 종류이다.

하지만 Gaussian이고 linear한 case이다.
(가우시안 확률 분포를 따르고 선형 시스템 모델에 대한 Bayes filter)

마찬가지로 Recursive하게 작동한다.

그리고 Bayes Filter처럼 Prediction step과 correction step이 존재한다.

Kalman Filter Example

먼저 처음 initial position이 존재하고, 원의 크기는 uncertainty를 의미한다.

initial position에 대해 이동 후, 알고 있는 motion model을 통해 위치를 예측한다.

하지만 motion model의 uncertainty로 인해 원의 크기가 커진 것을 확인할 수 있다.

이때, 등대와 같은 어떠한 관찰을 통해(센서 등) 위치를 대략적으로 measure한다.

여기서 앞서 진행한 prediction과 measurement를 통해 update를 진행한다.

Kalman Filter Concept

• In discrete time domain

위 식은 Linear model이다.

motion은 로봇(시스템)의 움직임 등에 대해 표현한 것이고,

observation(measurement)는 센서를 모델링한 것이라 볼 수 있다.

---

행렬 $A_t$는 $t-1$시점의 상태 값이 현재 $t$시점의 상태 값에 어떻게 영향을 미치는지에 대한 선형 변환 행렬이다.
(이전 상태 값 -> 현재 상태 값)

행렬 $B_t$는 현재 시점 $t$시점의 움직임 $u_t$가 현재 $t$시점의 상태 값에 어떻게 영향을 미치는지에 대한 선형 변환 행렬이다.
(움직임 -> 상태 값)

행렬 $C_t$는 현재 $t$시점의 상태 값이 현재 $t$시점의 관찰 값 $Z_t$에 어떻게 영향을 미치는지에 대한 선형 변환 행렬이다.
(상태 값 -> 관찰 값)

벡터 $\epsilon_{t}$ 와 벡터 $\delta_{t}$ 는 움직임 모델과 관찰 모델이 포함된 불확실한 노이즈 값에 대한 확률 변수를 나타낸다.

이렇게 정의된 선형 시스템 모델이 아래 수식과 같은 가우시안 분포를 따른다고 가정한다.

따라서 Bayes 필터에서 살펴보았던 움직임 모델과 관찰 모델에 대한 확률 분포를 아래와 같이 정의할 수 있다.

(Detail: Probabilistic Robotics, Ch 3.2 (p.46-49))

Kalman Filter : Application

동작 방식을 알아보자.

먼저 Recursive한 filter이기에 입출력에 mean과 covariance값이 recursive하게 대입된다.

입력에 $u_t$, $z_t$도 포함된다.

---

위 알고리즘에서 2번과 3번은 prediction step이고, 4~6번은 correction step이다.

• 2번 : 로봇 파라미터의 가우시안 분포에 대한 평균값 계산

• 3번 : t시간의 로봇 파라미터의 가우시안 분포에 대한 공분산 행렬 계산

• 4~6

업데이트 과정을 도식화하면 아래와 같다.

그래프로 확인하면 아래와 같고,

Recursive한 모습은 아래와 같다.

Extended Kalman Filter (EKF)

• Kalman filter assumption : Gaussian, Linear

하지만 현실 세계의 모델은 보통 non-linear 모델이다.

Extended Kalman Filter는 선형 kalman filter를 움직임 모델과 측정 모델이 non-linear형 시스템인 경우에도 활용할 수 있도록 확장한 필터이다.

Problem

Linear에서는 가우시안 분포가 입출력에 대해 계속 가우시안 분포를 따르지만,

Non-linear라면 함수를 거치며 non-gaussian 분포가 나온다.

그렇다면 gaussian 분포에 기반한 kalman filter가 더 이상 동작할 수 없다.

EKF

Non-linear 모델은 다음과 같다.

EKF는 non-linear한 상황을 해결하기 위해 first-order Taylor series(1차 테일러 급수 전개)를 통해 linear화 시킨다.(근사:approximation)

위 이미지처럼 Local linearization을 수행한다.

이를 통해, non-linear 시스템의 문제를 해결할 수 있게 되는 것이 요점이다.
(하지만 입력 분포에 따라 문제가 생길 수도 있다.)

문제

---

알고리즘은 위와 같고, Kalman filter와 거의 동일하다.

2번과 5번에서 차이가 있는데,

2번, 5번은 non-linear식을 그대로 사용했고,

$G$와 $H$는 Local linearization화 하였다.

---

결론은 Local linearization을 통해 Non-linear모델도 linear모델화시켜 Kalman filter를 수행할 수 있도록 하는 것이 Extended Kalman Filter라고 할 수 있다.

참고자료 & 이미지 출처

SLAM Online Study | SLAM DUNK Season 2 | Kalman Filter & EKF

‘NVIDIA Jetson Nano와 함께하는 SLAM의 이해와 구현’