[SLAM-Course] Bayes Filter-04

 

참고 자료: Cyrill Stachniss 교수님의 Robot Mapping, Introduction to Mobile Robotics 강의                

아주 오래전부터 공부하려고 했었던 Robot Mapping 강의를 들으면서 그 내용을 정리하여 기록해두고자 합니다. 이 강의는 robot mapping (SLAM)에 관한 SOTA system의 내용들 (Kalman filter, EKF, UKF, Particle filter, Graph-based Approach...) 을 다루고 있기 때문에 SLAM에 관한 기초를 공부하기에 좋은 자료라고 생각 됩니다.

Bayes Filter란?

가장 간단하게 설명하면 이 한줄로 요약할 수 있다.

Posterior (p(x|z):z가 주어졌을 때의 x) 를 Prior (p(x)) 를 통해서 update 하는 것

이를 SLAM문제를 예로 들면, IMU등으로 추정된 현재 위치 P(x)가 있을 때, 이 값을 LiDAR 또는 Camera로 측정되는 주변환경 정보 관측값을 반영해서 $P(X|Z)$로 update 하는 것이다.

Sensor로 부터 관측되는 sensor observation data $\textbf{z}$와, 로봇에 전달되는 제어명령 control data $\textbf{u}$가 있을 때, 로봇의 현재 상태 $\textbf{x}$ (주로 위치, 속도)를 Bayes' Theorem(베이즈정리)에 따라 추정하는 것이고 이를 수식으로 나타내면 다음과 같다. (bel은 belief)

$bel(x_t) = p(x_t | z_{1:t}, u_{1:t})$

이때 t는 현재 time stpe을 의미한다. 따라서 현재시점 $x_t$의 확률 분포는 처음부터 현재 시점까지의 ($1:t$) 연속된 센서 측정값과, 제어명령으로 부터 추정된다. 이제 위의 식을 Bayes' rule에 따라 변환해보자

b가 주어 졌을 때, a가 일어날 확률 $P(a|b)$를 베이즈 정리에 따라 다시 쓰면,

SLAM문제를 예로 들면, 특정위치에 있을 확률 $p(x)$와특정위치 $\textbf{x}$에 있을 때 랜드마크 $\textbf{y}$가 측정될 확률 $p(y|x)$를 구하여 곱하고 그 값을 랜드마크 $\textbf{y}$가 측정될 확률 $p(y)$로 나눠준 값이 랜드마크 $\textbf{y}$가 측정되었을 때, 특정위치 $\textbf{x}$에 있을 확률 $p(x|y)$ 이다.

이를 normalize 된 형태로 표현하면,

이로부터 $bel(x_t) = p(x_t | z_{1:t}, u_{1:t})$는 다음의 형태로 다시 쓸 수 있다.

그럼 이제부터 이 $bel(x_t)$를 시스템의 현재상태를 추정할 때 한번에 하나의 measurement와 하나의 제어명령만을 사용하는 Recursive형태로 바꾸어 보도록 하겠다.

먼저 $p(z_t | x_t, z_{1:t-1}, u_{1:t})$ 부분을 보자, 현재의 state $x_t$가 주어졌을 때 특정한 measurement $z_t$를 얻을 확률은 Markov assumption(과거와 현재 상태가 주어졌을 때의 미래 상태의 조건부 확률 분포가 과거 상태와는 독립적으로 현재 상태에 의해서만 결정된다는 것)에 따라 이전 측정값 $z_{1:t-1}$과 이전 제어명령 $u_{1:t}$는 무시할 수 있다. 따라서, 다음과 같이 간단한 형태로 바꿀 수 있다.

이제 두번째 term인 $p(x_t | z_{1:t-1}, u_{1:t})$를 보자, 식의 의미를 보면, 현재상태 $x_t$를 추정하고 싶은데 주어진 것은 마지막 관측값 $z_t$를 제외한 $z_{1:t-1}$, 그리고 마지막 명령이 포함된 $u_{1:t}$이다. 이것은 결국 직전 step (t-1) 까지의 추정치에다가 현재 step의 제어명령에 따른 모션을 더한 것으로 생각할 수 있다. 이 식을 law of total probability를 이용해서 확장할 수 있는데 law of total probability는 다음과 같다.

모든 B에 대해서, B가 일어날 확률 * B가 주어 졌을 때 A가 일어날 확률
$P(A) = \sum_B P(A|B)P(B)$ <- discrete case
$P(A) = \int_B P(A|B)P(B)dB$ <- continuous case

이때 $p(x_t | z_{1:t-1}, u_{1:t})$ 를 확장하기 위해 새로운 변수인 $x_{t-1}$ (이전 time step에서의 시스템 state)을 도입해서 low of total probability를 적용해 보면 다음과 같다. 이때, A에 해당하는 변수는 $x_t$이고, B에 해당하는 변수는 $x_{t-1}$ 이다.

이제 한번 더 $\int_{x_{t-1}}$의  $p(x_t |x_{t-1}, z_{1:t-1}, u_{1:t})$에 Markov assumption을 적용 하는데, $u_t$ 는  $x_{t-1}$에서 $x_t$로 이동하게하는 제어명령이기 때문에 system의 state가 time step $t-1$에서 $t$로 어떻게 변화했는지를 보여주는 정보이다. 따라서 $u_t$는 남겨두고 나머지($z_{1:t-1}, u_{1:t-1}$)는 markov assumption에 따라 제거하면 $bel(x_t)$는 다음과 같이 한번 더 간단해 진다.

여기서 $p(x_t |x_{t-1}, u_{t})$ term이말하는 것은 로봇의 time $t-1$에서의 위치를 알고 있고, 제어명령 $u_t$ (1m 전진과 같은..)가 있으면 현재 time step에서 포지션에 대한 확률분포를 가질 수 있다는 의미. 이때 또 한번 Markov assumption에 따라 $p(x_{t-1} | z_{1:t-1}, u_{1:t})$는 다음과 같이 바뀌는데 미래의 제어명령은 현재의 위치를 추정하는데 의미가 없다고 가정을 하고 문제를 풀고 있기 때문이다. 이것은 어디까지나 가정이므로 만약 SLAM문제를 해결하다가 이 가정때문에 문제가 잘 풀리지 않는다면 이 가정에 대한 문제를 해소하고 넘어가야 한다.

$p(x_{t-1} | z_{1:t-1}, u_{1:t-1})$

 이때 

$bel(x_{t-1}) = p(x_{t-1} | z_{1:t-1}, u_{1:t-1})$

이므로 전체식은 다음과 같은 recursive 형태로 표현된다.

이로부터 bayes filter는 이전 상태 $bel(x_{t-1})$와 현재 모션 명령 $u_t$ 및 현재 관측 값 $z_t$를 기반으로 시스템 상태를 추정 할 수 있는 recursive 업데이트 방식으로 표현 됨을 알 수 있다. 또한 어떠한 future information도 필요로 하지 않는 현재상태에서의 센서 관측값과 명령만 필요하기 때문에 Online state estimation이 가능하다. 즉, 시스템의 이전 상태에 대한 확률 분포가 있는 상태에서 제어 명령이 실행되고 이에 따른 센서 관측 값을 얻는다면 새로운 시점에서 시스템의 상태를 계산할 수 있다는 것. (it's a recursive update scheme which allows you to update your probability distribution based on a command and the observation)