Chapter 5-2. Localization(Bayes Filter)

이진한·2025년 12월 25일

모빌리티 지능

목록 보기
11/11

지난 포스트에서는 기초적인 Filter들에 대해서 살펴보았다. 이번에는 KF(Kalman Filter)와 EKF(Extended Kalman Filter)의 기반이 되는 Bayes Filter에 대해서 살펴보고자 한다.

처음 해당 내용을 들었을 때, Filter라는 것을 한 문장으로 정리해보자면, 개인적으로 다음과 같이 이해하였다.

Noise가 존재하는 Motion과 Sensor Data를 적절히 조합하여 로봇이 있는 위치를 적절히 추정한다.
(물론 교수님이나 다른 석박사님들께서 이에 대해서 틀렸다고 하실 수는 있지만, 수식만 보고 직관적으로 혼자 생각해본 내용이다.)

많은 동기들이 해당 내용이 Probalistic Robotics가 어렵다고 찡찡대길래...최대한 쉽게 이해할 수 있도록 예시(?)를 활용하여 수식을 쉽고 직관적으로 설명해보고자 한다. (다시 한 번 이 글의 목적을 정리하자면 최대한 쉽고 단순하게 이해할 수 있도록 하는 것이다.)

Markov Assumption

Markov Assumption이란 현재의 상태가 직전 상태에서 결정된다라는 가정이다.Reinfocement Learning에서 가장 기초가 되는 내용이기도 하지만, 여기서 수식을 정리하면서 얘기해보도록 하겠다. 일단 해당 내용을 설명하기에 앞서서 Term들에 관련된 내용부터 정리해보도록 하겠다.

Measurement

Measurement는 GPS와 같은 Sensor로 수집되어지는 Data이다. time t1t_1에서 부터 time t2t_2까지 얻어지는 Sensor Data를 수식으로 표현해보자면, 다음과 같다.

Zt1:t2=Zt1,Zt1+1,Zt1+2,⋯ ,Zt2Z_{t_{1}:t_{2}} = Z_{t_1}, Z_{t_{1} + 1}, Z_{t_{1} + 2}, \cdots, Z_{t_2}

Motion

Motion 명령이란 예를들어서 차량 혹은 로봇에게 몇 m/sm/s로 어디 방향으로 이동하라고 보내는 제어 명령이다. time t1t_1에서 time t2t_2까지 보내는 제어명령을 수식으로 표현해보면 다음과 같다.

ut1:t2=ut1,ut1+1,ut1+2,⋯ ,ut2u_{t_{1}:t_{2}} = u_{t_1}, u_{t_{1} + 1}, u_{t_{1} + 2}, \cdots, u_{t_2}

Measurement Model

Measurement Model은 Sensor data가 어떠한 과정을 통해서 얻어지는지를 의미한다. 이때, Markov Assumption이 들어가게 된다. 우선 수식을 적고 아래에서 설명해보겠다.

p(zt∣x0:t,z1:t−1,u1:t)=p(zt∣xt)p(z_t \mid x_{0:t}, z_{1:t-1}, u_{1:t}) = p(z_t \mid x_t)

우측의 Term의 의미는 현재의 State(위치) x0:tx_{0:t} 에서 어떠한 motion 명령 u1:tu_{1:t} 을 받았고, 과거의 Sensor dataz1:t−1z_{1:t-1}가 어떻게 들어왔을 때, 현재의 Sensor data ztz_t가 나올 것인가에 관한 수식이다. 아까도 설명하였다시피 Markov Assumption이란 이미 현재의 State(위치)xtx_t에 이미 과거의 Data가 포함되어있다는 가정이다. 따라서 현재의 Sensor Data 값 ztz_t를 얻기 위해서 현재의 State xtx_t만이 필요하다. 따라서 왼쪽의 수식과 같이 적을 수 있을 것이다.

State Transfer Model

State Transfer Model이란 어떻게 움직였을 때, 새로운 State xtx_t에 도달할 것인가를 나타내는 model이다. 여기서도 Markov Assumption을 통해서 수식을 표현한다면 다음과 같이 표현할 수 있을 것이다.

p(xt∣x0:t−1,u1:t,z1:t−1)=p(xt∣xt−1,ut)p(x_t \mid x_{0:t-1}, u_{1:t}, z_{1:t-1}) = p(x_t \mid x_{t-1}, u_t)

즉, 이전에 들어온 State Data인 xt−1x_{t-1}과 motion 명령인 utu_t만 있다면 현재의 State xtx_t에 대한 계산이 가능하다는 것이다.

Belief

belief 란 현재 xtx_t에 있을 확률이다. belief는 크게 2가지로 나눌 수 있는데 1개는 움직일 때의 위치를 예측하는 bel‾(xt)\overline{bel}(x_t)와 움직인 후 새롭게 얻은 Sensor data로 State를 보정하는 bel(xt)bel(x_t)가 있다. 해당 정의를 수식으로 써보면 다음과 같다.

  • Prediction
bel‾(xt)=p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1,ut) bel(xt−1) dxt−1\overline{bel}(x_t) = p(x_t \mid z_{1:t-1}, u_{1:t}) = \int p(x_t \mid x_{t-1}, u_t)\, bel(x_{t-1})\, dx_{t-1}
  • Correction
bel(xt)=p(xt∣z1:t,u1:t)=η p(zt∣xt) bel‾(xt)bel(x_t) = p(x_t \mid z_{1:t}, u_{1:t}) = \eta \, p(z_t \mid x_t)\, \overline{bel}(x_t)

Bayes filter는 결국 이러한 Belief를 계산하여 로봇의 현재 위치를 추정하는 것이다. 이때 수식을 보면 알겠지만, bel‾(xt)\overline{bel}(x_t)에는 bel(xt−1)bel(x_{t-1})이 들어가고 bel(xt)bel(x_t)에는 위치를 예측한 확률값인 bel‾(xt)\overline{bel}(x_t)가 들어간다. Bayes Filter에서는 Prediction과 Correction이 서로 반복적으로 일어나게 되고, 로봇 혹은 자율주행 차량의 위치를 정확하게 추정할 수 있게 되는 것이다.

아래에서 수식을 유도하면서 어떤 의미인지 자세히 살펴보겠지만, 다시 한 번 수식을 직관적으로 이해해보자.

bel‾(xt)=∫p(xt∣xt−1,ut) bel(xt−1) dxt−1\overline{bel}(x_t) = \int p(x_t \mid x_{t-1}, u_t)\, bel(x_{t-1})\, dx_{t-1}

먼저 Prediction하는 식부터 직관적으로 보면 State Transfer model과 이전 Step의 Belief를 곱해서 다음 State를 예측하는 식이라는 것일 이해할 수 있을 것이다. 즉, motion이 들어왔을 때 어떻게 움직일 지를 나타내는 State Transfer Model과 이전 Step xt−1x_{t-1}에 있을 확률을 곱하여 모두 합해줌으로써 다음 State를 예측할 수 있다는 것을 직관적으로 이해할 수 있다.

bel(xt)=η p(zt∣xt) bel‾(xt)bel(x_t) = \eta \, p(z_t \mid x_t)\, \overline{bel}(x_t)

Correction하는 식을 직관적으로 살펴보면 Mesurement model과 바로 직전에 Prediction 과정에서 얻은 bel‾(xt)\overline{bel}(x_t)를 곱해주고 있는 것을 확인할 수 있다. 즉, 방금 전 예측한 bel‾(xt)\overline{bel}(x_t)에 Sensor data를 결합해 줌으로써 현재의 State를 보정해주는 것이다.

η\eta 는 Normalizer이다.

Bayes Filter의 유도과정

우선 Correction하는 Belief인 bel(xt){bel}(x_t)를 정의부터 시작해보자.

bel(xt)=p(xt∣z1:t,u1:t){bel}(x_t) = p(x_t \mid z_{1:t}, u_{1:t})

여기서 Bayes Rule에 의해서 다음과 같은 수식을 만들어 낼 수 있다.

p(xt∣z1:t,u1:t)=p(zt∣xt,z1:t−1,u1:t)  p(xt∣z1:t−1,u1:t)p(zt∣z1:t−1,u1:t)p(x_t \mid z_{1:t}, u_{1:t}) = \frac{ p(z_t\mid x_t, z_{1:t-1},u_{1:t})\; p(x_t\mid z_{1:t-1},u_{1:t}) }{ p(z_t\mid z_{1:t-1},u_{1:t}) }

여기서 분모는 Normalizar로 들어가게 되고 정리하면 다음과 같다.

p(zt∣xt,z1:t−1,u1:t)  p(xt∣z1:t−1,u1:t)p(zt∣z1:t−1,u1:t)=η p(zt∣xt,z1:t−1,u1:t)  p(xt∣z1:t−1,u1:t)\frac{ p(z_t\mid x_t, z_{1:t-1},u_{1:t})\; p(x_t\mid z_{1:t-1},u_{1:t}) }{ p(z_t\mid z_{1:t-1},u_{1:t}) } = \eta \ p(z_t\mid x_t, z_{1:t-1},u_{1:t})\; p(x_t\mid z_{1:t-1},u_{1:t})

그리고 아까전 Markov Assumption을 활용한다면 최종적으로 다음과 같은 식을 만들어낼 수 있다.

bel(xt)=η p(zt∣xt) bel‾(xt)bel(x_t) = \eta \, p(z_t \mid x_t)\, \overline{bel}(x_t)

Bayes Rule에 대해서 잘 모른다면 다음 영상을 참고하길 바란다.
https://www.youtube.com/watch?v=HZGCoVF3YvM

두 번째로 Prediction하는 bel‾(xt)\overline{bel}(x_t)를 유도해보도록 하자. 마찬가지로 정의로부터 시작해보도록 하겠다.

bel‾(xt)  =  p(xt∣z1:t−1,u1:t)\overline{bel}(x_t) \;=\; p(x_t \mid z_{1:t-1}, u_{1:t})

여기서 전체확률의 정리(law of total probability)을 활용한다면,

p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t) dxt−1p(x_t \mid z_{1:t-1}, u_{1:t}) = \int p(x_t \mid x_{t-1} z_{1:t-1}, u_{1:t}) p(x_{t-1} \mid z_{1:t-1}, u_{1:t}) \, dx_{t-1}

와 같이 수식을 정리할 수 있다.

전체 확률의 정리를 잘 모른다면 다음 영상을 참고하길 바란다. 쉽게 설명하자면, 여러 가지 경우를 거쳐서 발생할 수 있는 확률을 모두 고려해 준다는 의미이다. 한 마디로 정리하자면 “지금 확률은, 가능한 ‘이전 상태’들을 전부 가정해 보고, 각 경우에 대한 확률을 가중합(연속이면 적분)해서 더한 값이다.”(말로 설명하는 것이 너무 어렵다...)
https://www.youtube.com/watch?v=U3_783xznQI

이후에 다시 한 번 Markov 가정을 활용하고, bel(xt)bel(x_t)의 정의식을 활용해 본다면 다음 수식은 최종적으로 다음과 같이 만들어짐을 알 수 있다.

bel‾(xt)=∫p(xt∣xt−1,ut) bel(xt−1) dxt−1\overline{bel}(x_t) = \int p(x_t \mid x_{t-1}, u_t)\, bel(x_{t-1})\, dx_{t-1}

Bayes Filter Example

유도과정만을 보면 Bayes Rule과 Law of total Probability 등 너무 복잡해 보일 수도 있지만, 사실 실제 Example을 보다보면 아주 쉽다는 것을 알 수 있다.

다시 한 번 Bayes filter가 어떠한 과정으로 자신의 위치를 추정해나가는지 상기해보자.

1. bel‾(xt)\overline{bel}(x_t)를 통해서 위치를 예측(Prediction)

2. Sensor를 통해서 위치를 보정(Update)

결국 2가지 과정을 계속 Reculsive!
하게 반복하면서 로봇 혹은 차량의 위치를 정확하게 추정해 나가는 것이다.
그렇다면 아래 예제를 통해서 문제를 풀어보면 확실하게 감이 올 것이다.

아래 예제에서의 Bayes Fileter에서의 목표는 문의 개폐여부이다.
여기서 Robot의 Motion 2가지를 할 수 있다. (Push, do_nothing)
Sensor는 문이 열려있는지 잠겨있는지를 감지할 수 있다.(open, close)

t = 1 아무 액션도 취하지 않았지만 문이 열려있는 경우를 감지

  • t = 1에서의 Prediction
bel‾(x1)=∫p(x1∣x0,u1) bel(x0) dx0=∑x0p(x1∣x0,u1)bel(x0)\overline{bel}(x_1) = \int p(x_1 \mid x_{0}, u_1)\, bel(x_{0})\, dx_{0} = \sum_{x_0} p(x_1 \mid x_{0}, u_1) bel(x_{0})
=p(x1∣x0=open,u1=nothing)bel(x0=open)+p(x1∣x0=closed,u1=nothing)bel(x0=closed)= p(x_1 \mid x_{0} = open, u_1 = nothing)bel(x_0 = open)+ p(x_1 \mid x_{0} = closed, u_1 = nothing)bel(x_0 = closed)

bel‾(x1=open)=0.5,bel‾(x1=close)=0.5\overline{bel}(x_1 = open) = 0.5,\overline{bel}(x_1 = close) = 0.5

  • t = 1에서의 Update
    bel(x1)=η p(z1=open∣x1) bel‾(x1)bel(x_1) = \eta \, p(z_1 =open \mid x_1)\, \overline{bel}(x_1)
bel(x1=open)=η p(z1=open∣x1=open) bel‾(x1=open)=η ∗0.6 ∗0.5=η ∗0.3bel(x1=closed)=η p(z1=open∣x1=closed) bel‾(x1=closed)=η ∗0.2 ∗0.5=η ∗0.1bel(x_1 =open) = \eta \, p(z_1 =open \mid x_1 = open)\, \overline{bel}(x_1 =open) = \eta \ * 0.6\ * 0.5 = \eta \ * 0.3\\ bel(x_1 =closed) = \eta \, p(z_1 =open \mid x_1 = closed)\, \overline{bel}(x_1 =closed) = \eta \ * 0.2\ * 0.5 = \eta \ * 0.1\\
η=10.3+0.1\eta = \frac{1}{0.3 + 0.1}
bel(x1=open)=0.75bel(x1=closed)=0.25bel(x_1 =open) = 0.75\\ bel(x_1 =closed) = 0.25

t = 2 미는 동작을 취하고 문이 열려있다고 가정하는 경우

  • t = 2에서의 Prediction

    bel‾(x2=open)=1 ∗0.75+0.8∗0.25=0.95bel‾(x2=close)=0 ∗0.75+0.2∗0.25=0.05\overline{bel}(x_2 = open) = 1\ * 0.75 + 0.8 * 0.25 = 0.95 \\ \overline{bel}(x_2 = close) = 0\ * 0.75 + 0.2 * 0.25 = 0.05

  • t =2에서의 Update

bel(x2=open)=η ∗0.75≈0.983bel(x2=closed)=η ∗0.2≈0.017bel(x_2 = open) = \eta \ * 0.75 \approx 0.983\\ bel(x_2 = closed) = \eta \ * 0.2 \approx 0.017

이번 포스트에서는 Bayes Filter를 직접 유도해보고 예제를 통해서 Bayes filter가 어떤 과정으로 수행되는지 쉽게 설명하고자 하였으나,어렵다고 느낄 수도 있을 거 같다는 생각이 들었다. 어렵다면 다음 github를 첨부할테니 코드 상으로 이해할 수 있으면 좋겠다.

https://github.com/JINANJINO/Localization

다음 번 포스트에서는 KF, EKF에 대해서 다뤄보도록 하겠다.

profile
이진한
Department of Robotics(Kyungpook national univercity)
이전 포스트

Chapter5-1. Localization(Filter Basic)

0개의 댓글