[SLAM] Extended Kalman Filter(EKF) SLAM

실제 EKF를 SLAM에 어떻게 적용시키는지에 대해서 설명한다.

본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.

SLAM(Simultaneous Localization and Mapping) 문제를 풀기위한 방법은 크게 3개로 나눌 수 있다.

• Kalman Filter

• Particle Filter

• Graph-based

이 글에서는 첫번째 방법인 Kalman filter를 이용한 방법에 대해서 설명한다.

EKF for Online SLAM

위 그림은 EKF를 이용한 online SLAM을 표현하였다. online이라는 이름은 로봇의 이전 위치는 저장하지 않고, 현재의 위치만 추정하기 때문에 붙은 이름이다. $\mathbf{x_t}$는 로봇의 위치, $\mathbf{m}$은 landmark를 의미하며, $z$와 $u$는 각각 observation과 control input이다. 이 문제를 식으로 표현하면 다음과 같다. \[p(\mathbf{x_t}, \mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t})\]

즉, 처음부터 현재시간 t까지의 control input과 센서로부터 얻은 observation을 알고 있을 때 현재 로봇의 위치인 $\mathbf{x_t}$와 맵을 구성하는 landmark인 $\mathbf{m}$를 구하는 문제이다.

State 표현방법

우선 설명의 시작에 앞서 로봇이 이동하는 공간은 2D로 한정한다. 즉 로봇의 위치는 x좌표와 y좌표, 그리고 로봇의 방향(heading)으로 정의된다. 또한 landmark는 방향 없이 x,y좌표로 정의된다. 이 설명에서는 센서로부터 얻어진 lanemark의 정보가 map상의 어떤 landmark인지는 알고 있다는 가정을 한다. 왜냐하면 센서정보를 이용하여 landmark의 correspondence를 찾는 것 또한 하나의 연구 분야이기 때문이다. 위의 식 $p(\mathbf{x_t}, \mathbf{m} \mid \mathbf{z}{1:t}, \mathbf{u}{1:t})$에서 알 수 있듯이, control input과 observation을 이용하여 우리가 추정하고자 하는 것은 로봇의 위치와 landmark의 위치이다. 따라서 EKF SLAM에서는 다음과 같이 이 두 정보를 하나의 state vector로 정의한다. \[\mathbf{x}_t = (x, y, \theta, m_{1,x},m_{1,y}, m_{2,x},m_{2,y},\cdots, m_{n,x},m_{n,y})\]

vetor($\mathbf{x}_t$)의 앞의 3개항($x,y,\theta$)는 로봇의 x,y위치와 heading의 방향을 의미하며, 나머지 항은 landmark의 위치의 x,y좌표이다. 만약 landmark의 갯수가 n개 일 때 $\mathbf{x}_t$ vector의 크기는 $3+2n$이다. 이렇게 state vector가 정의되었을 때 Covariance matrix는 다음 그림과 같이 정의된다.

맨 위의 그림을 보자, $\mu$는 state vector를 의미하며, $\Sigma$는 covariance matrix이다. 노란색으로 표시된 부분은 로봇 위치에 대한 covariance이며, 파란색으로 표시된 부분은 landmark끼리에 대한 covariance이다. 초록색으로 표시된 부분은 로봇의 위치와 landmark간의 covariance를 나타낸다. 표현의 편리성을 위하여 일반적으로 맨 위의 matrix를 맨 아래와 같이 단순화 시켜서 표시한다. 즉 $\mathbf{x}$는 로봇의 위치에 대한 정보를 갖고 있는 $3\times1$ 크기의 vetor이며, $\mathbf{m}$는 모든 landmark의 위치 정보를 갖고 있는 $2n\times1$ 크기의 vector이다. $\Sigma_{xx}$와 $\Sigma_{mm}$은 각각 $3\times3, 2n\times2n$크기를 갖는 로봇위 위치와 landmark 위치에 대한 covariance이다.

Prediction step

아래 그림은 로봇이 이동하였을 때 prediction step단계를 보여주고 있다. prediction step에서는 control input을 이용하여 예상되는 로봇의 위치를 추정하는 과정이다. 따라서 로봇의 위치인 $x_R$과 로봇의 위치에 대한 covariance인 $\Sigma_{x_R,x_R}$이 변한다. 또한 로봇의 위치에 대한 covariance가 변하였기 때문에 로봇과 landmark간의 부정확성을 나타내는 $\Sigma_{x_R,m_n}$도 변한다. 아래 그림에서 초록색으로 표시된 부분이 prediction step에서 update되는 부분이다. prediction step에서는 covariance matrix에서 가장 큰 block matrix인 landmark의 covariance($\Sigma_{mm}$)은 변하지 않기 때문에 계산량이 크지 않으며, 계산량은 landmark의 갯수에 선형적으로 증가한다.

Correction step

Correction step은 예측된 로봇의 위치에서 예상되는 센서데이터와 실제 센서데이터와의 차이를 이용하여 로봇의 위치, 그리고 landmark의 위치를 보정한다. 이 과정에서 실제 observation의 uncertainty가 state에 반영이 되며, landmark의 covariance matrix에도 영향을 준다. 따라서 correction 단계에서는 mean vector와 covariance matrix의 모든 영역이 업데이트 된다. correction 단계의 계산량은 landmark의 숫자에 quadratic하게 증가한다. 왜냐하면 Kalman gain을 구할 때 matrix의 inverse를 구하는데, matrix inverse의 계산은 matrix 크기에 quadratic하게 증가하기 때문이다.

EKF SLAM Example

Example을 위한 가정

• 로봇은 2D plane 상에서 움직인다.

• Velocity-based motion model을 사용한다.

• 로봇은 landmark를 point로 인식한다.

• Observation sensor는 LiDAR와 같은 Range-bearing 센서이다.

• 관측된 landmark와 알고있는 map상의 landmark와의 대응 관계는 알고있다고 가정한다.

• landmark의 갯수는 미리 알고 있다.

초기화

SLAM의 가정에서 landmark의 갯수는 미리 알고 있다고 하였으므로, mean vector와 covariance matrix를 고정된 크기로 초기화 할 수 있다. mean vector($\mu_t$)의 크기는 $(3+2n)\times1$이며 covariace matrix의 크기는 $(3+2n)\times(3+2n)$이다. 이때 로봇의 시작 위치는 원점이 되므로 로봇의 위치에 대한 정보는 0으로 초기화 한다. 또한 landmark에 대한 정보를 모르고 있으므로 나머지 mean vector 데이터도 0으로 초기화 한다. \[\mu_0 = \begin{pmatrix} 0 & 0 & 0 & 0 & 0 & \cdots & 0 \end{pmatrix}^T\]

covariance matrix의 경우는 조금 다르다. 로봇의 위치는 시작할 때 확실히 알고 있으므로 uncertainty가 0 이다. 따라서 로봇의 위치에 해당하는 covariance matrix는 0으로 초기화 한다. 반면, landmark에 대해서는 아무런 정보가 없기 때문에 uncertainty가 매우 크므로 diagonal 항을 다음과 같이 무한대로 초기화 한다. \[\Sigma_0 = \begin{pmatrix} 0 & 0 & 0 & 0 & 0 & \cdots & 0 \\ 0 & 0 & 0 & 0 & 0 & \cdots & 0 \\ 0 & 0 & 0 & 0 & 0 & \cdots & 0 \\ 0 & 0 & 0 & \infty & 0 & \cdots & 0 \\ 0 & 0 & 0 & 0 & \infty & \cdots & 0 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 0 & 0 & 0 & 0 & 0 & \cdots & \infty \end{pmatrix}\]

Extended Kalman Filter (EKF) 과정

우선 이전글에서 설명했던 EKF algorithm은 다음과 같다. 2~3번은 prediction step, 4~6번은 correction step이다. 이전 글에서 설명한 EKF는 mean vector와 covariance matrix는 로봇의 위치에 대한 정보만을 갖고 있었지만($3\times1$ vector, $3\times3$ matrix), EKF SLAM에서는 landmark의 정보도 함께 갖고 있다($(3+2n)\times1$ vector, $(3+2n)\times(3+2n)$ matrix). 따라서 앞에서 설명한 EKF과정에 vector와 matrix의 크기를 맞춰주는 과정이 EKF SLAM에는 추가된다. \[\begin{aligned} 1: & \text{Extended Kalman filter}(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t)\\ 2: & \ \ \bar{\mu}_t = g(u_t, \mu_{t-1})\\ 3: &\ \ \bar{\Sigma_t} = G_t \Sigma_{t-1} G_t^T + R_t\\ 4: &\ \ K_t = \bar{\Sigma_t}H_t^T(H_t \bar{\Sigma_t}H_t^T + Q_t)^{-1}\\ 5: &\ \ \mu_t = \bar{\mu_t} + K_t(z_t - h(\bar{\mu_t}))\\ 6: &\ \ \Sigma_t = (I - K_t C_t)\bar{\Sigma_t}\\ 7: &\ \ \text{return} \ \ \mu_t, \Sigma_t\\ \end{aligned}\]

Mean Prediction (2번 step)

로봇의 Velocity-based motion model은 다음과 같다. \[\begin{bmatrix} x_{t}\\y_{t}\\ \theta_{t} \end{bmatrix} = \begin{bmatrix} x_{t-1} \\ y_{t-1} \\ \theta_{t-1} \end{bmatrix} + \begin{bmatrix} -\frac{v_t}{\omega_t} \sin \theta_{t-1} + \frac{v_t}{\omega_t}\sin(\theta_{t-1} + {\omega_t}\vartriangle t)\\ -\frac{v_t}{\omega_t} \cos \theta_{t-1} - \frac{v_t}{\omega_t}\cos(\theta_{t-1} + {\omega_t}\vartriangle t)\\ {\omega_t} \vartriangle t \end{bmatrix}\]

위의 motion model은 로봇의 위치에 대한 정보만을 갖고 있는 3 dim의 vector이다. 3 dim의 vector를 $3+2n$ dim의 vector에 적용시키기 위해서 아래와 같이 $F_x$ matrix를 이용한다. \[F_x = \begin{bmatrix} 1 & 0 & 0 & 0 & \cdots & 0\\ 0 & 1 & 0 & 0 & \cdots & 0\\ 0 & 0 & 1 & 0 & \cdots & 0 \end{bmatrix}^T\] \[\begin{bmatrix} x_{t}\\y_{t}\\ \theta_{t} \end{bmatrix} = \begin{bmatrix} x_{t-1} \\ y_{t-1} \\ \theta_{t-1} \end{bmatrix} + F_x \begin{bmatrix} -\frac{v_t}{\omega_t} \sin \theta_{t-1} + \frac{v_t}{\omega_t}\sin(\theta_{t-1} + {\omega_t}\vartriangle t)\\ -\frac{v_t}{\omega_t} \cos \theta_{t-1} - \frac{v_t}{\omega_t}\cos(\theta_{t-1} + {\omega_t}\vartriangle t)\\ {\omega_t} \vartriangle t \end{bmatrix}\]

위와 같이 $F_x$ matrix를 이용함으로써 matrix 크기를 맞춤과 동시에, control input에 의한 로봇 위치의 mean값만 변경되었다.

Covariance Prediction (3번 step)

이전 글의 EKF 설명에서 velocity-based motion model의 Jacobian($G_t^x$)는 $3\times3$ matrix였다. 하지만 SLAM 문제에서 state의 covariance matrix는 $(3+2n)\times(3+2n)$ matrix이다. 따라서 Jacobian의 크기를 맞추기 위해 다음과 같이 matrix를 수정한다. \[G_t = \begin{pmatrix} G_t^x & 0 \\ 0 & I \end{pmatrix}\]

$G_t^x$는 $3\times3$크기의 motion model의 Jacobian matrix이고, $G_t$는 $(3+2n)\times(3+2n)$의 matrix이다.

크기가 수정된 covariance matrix $G_t$를 이용하여 3번 step을 계산하면 다음과 같다. \[\begin{aligned} \bar{\Sigma_t} &= G_t \Sigma_{t-1} G_T + R_t\\ &= \begin{pmatrix}G_t^x & 0 \\ 0 & I \end{pmatrix} \begin{pmatrix}\Sigma_{xx} & \Sigma_{xm} \\ \Sigma_{mx} & \Sigma_{mm} \end{pmatrix} \begin{pmatrix}(G_t^x)^T & 0 \\ 0 & I \end{pmatrix} + R_t\\ &=\begin{pmatrix} G_t^x \Sigma_{xx} (G_t^x)^T & G_t^x \Sigma_{xm}\\ (G_t^x \Sigma_{xm})^T & \Sigma_{mm} \end{pmatrix} + R_t \end{aligned}\]

위에서 설명했던 것 처럼 Jacobian matrix ($G_t^x$)는 로봇의 위치와 관련이 있는 부분에만 영향을 미친 것을 알 수 있다.

여기서 Jacobian matrix ($G_t^x$)는 다음과 같다. \[G_t^x = \frac{\partial g(u_t,\mu_{t-1})}{\partial \mathbf{x_{t-1}}} = \begin{pmatrix} 1 & 0 & -\frac{v_t}{\omega_t}\cos \theta_{t-1} + \frac{v_t}{\omega_t}\cos(\theta_{t-1}+\omega_t \vartriangle t)\\ 0 & 1 & -\frac{v_t}{\omega_t}\sin \theta_{t-1} + \frac{v_t}{\omega_t}\sin(\theta_{t-1}+\omega_t \vartriangle t)\\ 0 & 0 & 1 \end{pmatrix}\]

한가지 더 살펴보면, process 노이즈인 $R_t$도 원래 $3\times3$ matrix이다. 따라서 matrix의 크기를 맞추기 위해서 $R_t = F_x R_t^x F_x^T$ 와 같이 $F_x$ matrix를 이용한다. 따라서 최종적으로 EKF의 4번 식은 다음과 같이 수정된다. \[\bar{\Sigma_t} = G_t \Sigma_{t-1} G_t^T + F_x R_t^x F_x^T\]

Correction step (4,5,6번 step)

EKF SLAM의 correction step을 정리하면 다음과 같다.

• $c_t^i = j$ 는 t시점에서 i번째로 측정된 landmark이며 전체 landmark에서 j index를 갖는다.

• 만약 landmark가 이전에 관측되지 않았던 landmark이면 현재 로봇의 위치를 기반으로 초기화 한다. 즉 landmark의 위치를 등록한다.

• 예상되는 observation을 계산한다.

• 비선형 함수 $h(x_t)$의 Jacobian matrix를 계산한다.

• Kalman gain을 계산하고 correction process를 진행한다.

이 예제에서는 LiDAR센서와 같은 Range-bearing observation을 기반으로 한다. Range-bearing observation model의 관측데이터는 다음과 같다. \[z_t^i = \begin{pmatrix} r_t^i \\ \phi_t^i \end{pmatrix}\]

$r_i^i$는 landmark와의 거리, $\phi_t^i$는 로봇의 헤딩 각도로 부터 landmark 까지의 방향을 의미한다. 센서로부터 landmark의 데이터가 획득되었으나 이전에 관측되지 않았던 landmark라면 아래 식을 통해 landmark의 global 위치를 계산하여 state에 등록한다(Initialize과정에서 모든 landmark의 위치는 0으로 초기화 했었다). \[\begin{pmatrix} \bar{\mu}_{j,x} \\ \bar{\mu}_{j,y} \end{pmatrix} = \begin{pmatrix} \bar{\mu}_{t,x} \\ \bar{\mu}_{t,y} \end{pmatrix} + \begin{pmatrix} r_t^i \cos (\phi_t^i + \bar{\mu}_{t,\theta}) \\ r_t^i \sin (\phi_t^i + \bar{\mu}_{t,\theta}) \end{pmatrix}\]

$\bar{\mu}j$는 j번째 landmark의 위치이며, $\bar{\mu}{t,x}, \bar{\mu}{t,y}, \bar{\mu}{t,\theta}$는 각각 현재 시점에서 로봇의 x, y, heading을 의미한다.

그렇다면 Range-bearing의 observation model은 어떻게 되는지 계산해 보자. \[\delta = \begin{pmatrix} \delta_x \\ \delta_y \end{pmatrix} = \begin{pmatrix} \bar{\mu}_{j,x} - \bar{\mu}_{t,x}\\ \bar{\mu}_{j,y} - \bar{\mu}_{t,y} \end{pmatrix}\] \[q = \delta^T \delta\] \[\hat{z}_t^i = \begin{pmatrix} \sqrt{q}\\ \arctan (\frac{\delta_y}{\delta_x}) - \bar{\mu}_{t,\theta} \end{pmatrix} = h(\bar{\mu}_t)\]

$\delta$는 로봇의 위치와 landmark의 위치 차를 의미하며 $\sqrt{q}$는 로봇과 landmark의 거리를 의미한다. 따라서 $h(\bar{\mu}t)$는 비선형 observation model이며, 현재 로봇의 위치와 landmark의 위치를 알고 있을 때 이 observation model을 이용하여 예상되는 센서의 observation을 계산할 수 있다. 여기서 $\bar{\mu}_t$는 로봇의 현재 위치정보($\bar{\mu}{t,x}, \bar{\mu}{t,y}, \bar{\mu}{t,\theta}$)와 landmark의 위치($\bar{\mu}{j,x}, \bar{\mu}{j,y}$)를 포함하고 있는 $5\times1$크기의 vector이다.

이제 observation model의 linearization과정을 통해 Jacobian matrix를 구해보자 \[\begin{aligned} ^{low}H_t^i &= \frac{\partial h(\bar{\mu}_t)}{\partial \bar{\mu}_t}\\ &= \begin{pmatrix} \frac{\partial \sqrt{q}}{\partial \bar{\mu}_{t,x}} & \frac{\partial \sqrt{q}}{\partial \bar{\mu}_{t,y}} & \frac{\partial \sqrt{q}}{\partial \bar{\mu}_{t,theta}} & \frac{\partial \sqrt{q}}{\partial \bar{\mu}_{j,x}} & \frac{\partial \sqrt{q}}{\partial \bar{\mu}_{j,y}} \\ \frac{\partial (\arctan (\frac{\delta_y}{\delta_x}) - \bar{\mu}_{t,\theta})}{\partial \bar{\mu}_{t,x}} & \frac{\partial (\arctan (\frac{\delta_y}{\delta_x}) - \bar{\mu}_{t,\theta})}{\partial \bar{\mu}_{t,y}} & \frac{\partial (\arctan (\frac{\delta_y}{\delta_x}) - \bar{\mu}_{t,\theta})}{\partial \bar{\mu}_{t,theta}} & \frac{\partial (\arctan (\frac{\delta_y}{\delta_x}) - \bar{\mu}_{t,\theta})}{\partial \bar{\mu}_{j,x}} & \frac{\partial (\arctan (\frac{\delta_y}{\delta_x}) - \bar{\mu}_{t,\theta})}{\partial \bar{\mu}_{j,y}} \end{pmatrix}\\ &= \frac{1}{q} \begin{pmatrix} -\sqrt{q}\delta_x & -\sqrt{q}\delta_y & 0 & \sqrt{q}\delta_x & \sqrt{q}\delta_y\\ \delta_y & -\delta_x & -q & -\delta_y & \delta_x \end{pmatrix} \end{aligned}\]

$^{low}H_t^i$에서 low는 아직 matrix의 크기를 조정하기 전의 matrix를 표시하기 위해 사용하였다. Jacobian matrix $^{low}H_t^i$ 의 크기는 $2\times5$이므로 EKF의 update과정에 적용시키기 위해 $F_{x,j}$ matrix를 사용하여 matrix의 크기를 조절한다. 크기가 조절된 matrix인 $H_t^i$를 observation model의 Jacobian으로 사용한다 . \[H_t^i = ^{low}H_t^i F_{x,j}\] \[F_{x,j} = \begin{pmatrix} 1 & 0 & 0 & 0 & \cdots & 0 & 0 & 0 & 0 & \cdots & 0\\ 0 & 1 & 0 & 0 & \cdots & 0 & 0 & 0 & 0 & \cdots & 0\\ 0 & 0 & 1 & 0 & \cdots & 0 & 0 & 0 & 0 & \cdots & 0\\ 0 & 0 & 0 & 0 & \cdots & 0 & 1 & 0 & 0 & \cdots & 0\\ 0 & 0 & 0 & 0 & \cdots & 0 & 0 & 1 & 0 & \cdots & 0 \end{pmatrix}\]

$F_{x,j}$에서 1~3행은 로봇의 위치에 대한 Jacobian term을, 4~5행은 landmark에 대한 jacobian term을 원하는 위치에 입력하기 위함이며, landmark의 index가 j인 경우 4~5행의 $2\times2$의 identity matrix는 $3+2(j-1)$ 열 다음에 위치하게 된다. $F_{x,j}$가 곱해진 $H_t^i$의 크기는 $2 \times (3+2n)$이다.

이제 위에서 크기를 조절한 $H_t^i$ Jacobian matrix를 이용하여 K (Kalman gain)을 구하고, EKF의 5, 6번 과정을 통해 최종 mean과 covariance를 계산할 수 있다.

Loop Closure

Loop closure는 이전에 방문했던 위치를 다시 방문했을 때, 같은 위치임을 인식하고 현재 위치에 대한 uncertainty를 줄이는 방법을 말한다. 아래그림은 loop closing이 발생하기 전의 uncertainty를 보여주고 있다. 시작 위치에서부터 로봇이 이동을 할수록 control input과 observation의 uncertainty에 의해 로봇의 pose와 landmark 위치의 covariance가 증가한 것을 볼 수 있다.

아래그림은 시작 위치로 로봇이 돌아와서 처음 관측했었던 landmark를 보았을 때, 즉 시작 위치에서 loop closing이 발생하였을 때를 보여준다. 로봇이 처음 출발하였을 때는 uncertainty가 작기 때문에 그때 관측된 landmark 또한 작은 covariance를 갖고 있다. loop가 발생하면 uncertainty가 작은 landmark의 위치에 의해 로봇의 위치와 landmark들의 위치가 보정되고, uncertainty 또한 줄어들게 된다.

본 글을 참조하실 때에는 출처 명시 부탁드립니다.

[SLAM] Extended Kalman Filter(EKF) 예제

Robot의 실제 모델을 이용하여 EKF를 설명.

본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.

이번 글에서는 이전 글에서 설명한 Extended Kalman Filter(EKF)를 실제 모델을 이용해서 설명한다. 이전 글을 통해 EKF의 선형화 과정과 bayes filter 과정을 이해했지만 실제 어떻게 적용을 하는지에 대해서는 이해가 잘 되지 않을 수 있다. 이 글에서는 velocity motion model과 observation model을 이용하여 EKF과정을 설명한다.

Motion Model

Robot의 Motion model은 크게 두가지로 나뉜다.

• Odometry-based model

• Velocity-based model

이 글에서는 velocity-based model을 이용하여 설명한다. 아래 그림은 velocity-based model을 보여준다. $x,y,\theta$는 로봇의 x,y좌표 및 방향을 의미하며, 로봇의 선속도는 $v$, 각속도는 $\omega$이다.

이때 로봇의 상태(state)는 $\mathbf{x_t} = \begin{bmatrix} x \ y \ \theta \end{bmatrix}$이며, control input은 $u_t = \begin{bmatrix} v \ \omega \end{bmatrix}$이다. 실제 로봇을 이동시키기 위해서는 모터를 구동시켜야 하기 때문에 로봇에 들어가는 실제 입력은 모터를 구동하기 위한 제어값이다. 하지만 여기서 control input은 로봇이 얼마나 이동했는지를 측정하기 위한 센서값을 의미한다. Velocity-based model에서 motion model은 다음과 같다. \[\begin{bmatrix} x_{t}\\y_{t}\\ \theta_{t} \end{bmatrix} = \begin{bmatrix} x_{t-1} - \frac{\hat{v_t}}{\hat{\omega_t}} \sin \theta_{t-1} + \frac{\hat{v_t}}{\hat{\omega_t}}\sin(\theta_{t-1} + \hat{\omega_t}\vartriangle t)\\ y_{t-1} - \frac{\hat{v_t}}{\hat{\omega_t}} \cos \theta_{t-1} - \frac{\hat{v_t}}{\hat{\omega_t}}\cos(\theta_{t-1} + \hat{\omega_t}\vartriangle t)\\ \theta_{t-1} + \hat{\omega_t} \vartriangle t \end{bmatrix}\]

즉 velocity-based model은 비선형 함수로 정의된다. 위 식에서 $\hat{}$은 노이즈를 포함한 control input을 의미한다. control input의 covariance가 $M_t$일 때 다음과 같이 노이즈를 분리할 수 있다. \[\begin{bmatrix} \hat{v} \\ \hat{\omega} \end{bmatrix} = \begin{bmatrix} v \\ \omega \end{bmatrix}+\mathcal{N}(0,M_t)\]

따라서 노이즈 항을 따로 분리하면 다음과 같이 다시 쓸 수 있다. \[\begin{bmatrix} x_{t}\\y_{t}\\ \theta_{t} \end{bmatrix} = \begin{bmatrix} x_{t-1} - \frac{v_t}{\omega_t} \sin \theta_{t-1} + \frac{v_t}{\omega_t}\sin(\theta_{t-1} + \omega_t\vartriangle t)\\ y_{t-1} + \frac{v_t}{\omega_t} \cos \theta_{t-1} - \frac{v_t}{\omega_t}\cos(\theta_{t-1} + \omega_t\vartriangle t)\\ \theta_{t-1} + \omega_t \vartriangle t \end{bmatrix} + \mathcal{N}(0,R_t)\]

여기서 $R_t$는 process noise로 control input의 uncertainty에 의해 발생하며, $R_t$에 대해서는 뒤에서 다시 자세히 설명한다.

이제 motion model을 알고 있으므로 앞에서 설명한 Jacobian matrix를 이용하여 선형화된 model을 계산할 수 있다. \[\mathbf{x_t} = G_t \mathbf{x_{t-1}} + V_t \mathbf{u_t}\]

위 식에서 $G_t, V_t$는 각각 $\mathbf{x_{t-1}}$와 $\mathbf{u_t}$로 편미분을 통해 계산한 Jacobian matrix이다. \[G_t = \frac{\partial g(u_t,\mu_{t-1})}{\partial \mathbf{x_{t-1}}} = \begin{pmatrix} 1 & 0 & -\frac{v_t}{\omega_t}\cos \theta_{t-1} + \frac{v_t}{\omega_t}\cos(\theta_{t-1}+\omega_t \vartriangle t)\\ 0 & 1 & -\frac{v_t}{\omega_t}\sin \theta_{t-1} + \frac{v_t}{\omega_t}\sin(\theta_{t-1}+\omega_t \vartriangle t)\\ 0 & 0 & 1 \end{pmatrix}\] \[V_t = \frac{\partial g(u_t,\mu_{t-1})}{\partial \mathbf{u_{t}}} = \begin{pmatrix} \frac{-\sin \theta_{t-1} + \sin (\theta_{t-1}+\omega_t \vartriangle t)}{\omega_t} & \frac{v_t(\sin \theta_{t-1} - \sin (\theta_{t-1}+\omega_t \vartriangle t))}{\omega_t^2} + \frac{v_t \vartriangle t \cos (\theta_{t-1} + \omega_t \vartriangle t)}{\omega_t}\\ \frac{\cos \theta_{t-1} - \cos (\theta_{t-1}+\omega_t \vartriangle t)}{\omega_t} & \frac{v_t(-\cos \theta_{t-1} + \cos (\theta_{t-1}+\omega_t \vartriangle t))}{\omega_t^2} + \frac{v_t \vartriangle t \sin (\theta_{t-1} + \omega_t \vartriangle t)}{\omega_t} \end{pmatrix}\]

따라서 위에서 계산한 Jacobian matrix을 이용하여 EKF의 prediction step을 다음과 같이 계산할 수 있다. \[\bar{\Sigma_t} = G_t \Sigma_{t-1} G_t^T + V_t M_t V_t^T = G_t \Sigma_{t-1} G_t^T + R_t\]

Observation Model

비선형 observation model을 EKF에 적용해 보기 위해서 가상의 로봇을 이용한다. 이 로봇은 3개의 센서를 갖고 있다. 첫번째 센서는 로봇의 위치에서 부터 landmark까지의 euclidean distance를 측정할 수 있다. 두번째, 세번째 센서는 landmark까지의 x방향의 거리와 y방향의 거리를 각각 측정할 수 있다. 로봇의 state는 $\mathbf{\bar{x}_t} = \begin{bmatrix} \bar{x}_t \ \bar{y}_t \ \bar{\theta}_t \end{bmatrix}$로 표시하며, landmark의 위치는 $\mathbf{m} = \begin{bmatrix} m_x\m_y \end{bmatrix}$라고 하자. 이때의 각 센서의 데이터입력 $\mathbf{z}$는 다음과 같다. \[\mathbf{z_t} = \begin{bmatrix} z_1\\z_2\\z_3 \end{bmatrix} = \begin{bmatrix} \sqrt{(m_x - x_t)^2+(m_y - y_t)^2}\\ m_x - x_t\\ m_y - y_t \end{bmatrix}\]

따라서 위 비선형 observation model을 선형화 하기 위해서 Jacobian을 구하면 다음과 같다. \[H_t = \frac{\partial \mathbf{z_t}}{\partial \mathbf{\bar{x}_t}} = \begin{pmatrix} \frac{-m_x+\bar{x}_t}{\sqrt{(m_x-\bar{x}_t)^2 + (m_y-\bar{y}_t)^2}} & \frac{-m_y+\bar{y}_t}{\sqrt{(m_x-\bar{x}_t)^2 + (m_y-\bar{y}_t)^2}} & 0 \\ -1 & 0 & 0 \\ 0 & -1 & 0 \end{pmatrix}\]

따라서 위에서 계산한 Jacobian $H_t$를 이용하여 EKF의 correction step을 수행할 수 있다.

Observation model에서 $Q_t$는 measurement 노이즈로, 데이터를 얻는 센서의 부정확성으로 인해 발생한다. 따라서 observation model에서 $Q_t$는 센서의 uncertainty자체를 의미한다. 추가적으로 Jacobian matrix는 선형화 포인트에서만 유효하기 때문에 매 step마다 다시 계산해 주어야 한다는 점을 기억해야 한다.

다음 글은 EKF를 이용한 SLAM에 대해서 설명한다.

본 글을 참조하실 때에는 출처 명시 부탁드립니다

[SLAM] Kalman filter and EKF(Extended Kalman Filter)

Kalman filter와 Extended Kalman filter에 대한 설명.

본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.

Kalman Filter & EKF (Extended Kalman Filter)

이번 글에서는 Kalman filter와 Kalman filter의 확장판인 EKF(Extended Kalman Filter)에 대해서 설명한다. 앞의 글에서 설명한 Bayes filter는 로봇의 상태(state)를 추정하기 위한 방법 중에 한가지 이며, 예측(prediction)단계와 보정(correction)단계의 두 단계로 나뉘어 진다.

• Prediction step

• Correction step

Bayes filter에 대한 자세한 설명은 이전 글을 참고하기 바란다.

• Kalman filter는 로봇의 state를 추정하기 위해 가장 흔히 사용되는 방법이며, Bayes filter이다. 즉 control input에 의한 prediction 단계와, 센서의 observation를 이용한 correction의 두 단계로 나누어 진다.

• KF (Kalman Filter)와 EKF (Extended Kalman Filter)는 공통적으로 Gaussian 분포를 가정한다. 즉, 위의 Bayes filter는 모든 확률분포에 대한 식이며, 그 중에서 KF와 EKF는 모든 분포(control input, observation 등)를 gaussian으로 가정한다.

• KF는 선형 Gaussian 모델의 경우이며, EKF는 비선형 Gaussian 모델이다.

Gaussian 분포

• Gaussian distribution (normal distribution)

• Multi variable Gaussian distribution

Gaussian 분포는 single variable과 multi variable의 Gaussian 분포로 표현할 수 있으며, SLAM에서는 Vector를 이용하여 로봇의 상태(state), 센서 입력, 관찰 값 등을 표현하므로 multi variable의 Gaussian을 많이 사용한다. 따라서 Multi variable의 Gaussian 식은 숙지하는 것이 좋다.

선형 모델에서 Gaussian 분포의 변환

가장 기본적인 선형 모델은 다음과 같다. \[Y = AX+B\]

이 때, 확률변수 X가 Gaussian 분포를 갖고 있으며 다음과 같을 때, \[X \sim \mathcal{N}(\mu_x,\Sigma_x)\]

선형 변환 후의 확률변수인 Y의 분포는 다음과 같다. \[Y \sim \mathcal{N}(A \mu_x+B,A \Sigma_x A^T)\]

유도과정

X의 평균인 $\mu_x$ 는 선형 변환에 의해서 $A\mu_x+B$가 되는 것은 직관적으로 이해 할 수 있다. 그렇다면 covariance matrix은 어떻게 유도가 될까? 우선 covariance matrix의 정의로 부터 시작한다. \[\begin{aligned} \Sigma_y &= E((y-\mu_y)(y-\mu_y)^T)\\ &= E((y-(A\mu_x+B))(y-(A\mu_x+B))^T)\\ &= E(((AX+B)-(A\mu_x+B))((AX+B)-(A\mu_x+B))^T)\\ &= E([A(X-\mu_x)][A(X-\mu_x)]^T)\\ &= E(A(X-\mu_x)(X-\mu_x)^TA^T)\\ &= AE((X-\mu_x)(X-\mu_x)^T)A^T\\ &= A \Sigma_x A^T \end{aligned}\]

위의 유도처럼 Gaussian 분포의 선형변환에서의 covariance는 covariance의 정의로부터 $\Sigma_y = A \Sigma_x A^T$ 로 정의된다. 이 관계는 Kalman filter 뿐만 아니라 Gaussian을 사용하는 여러 분야에서 자주 사용되므로 기억해 두는것이 좋다.

Kalman Filter (KF)

• Kalman filter는 선형 모델(Linear model)에서 uncertainty의 분포를 Gaussian으로 가정하였을 때의 solution이다.

• Kalman filter는 motion 모델과 observation 모델을 선형으로 가정한다.

• 노이즈는 평균(mean)이 0인 Gaussian 분포로 가정한다.

• $A_t$ : control input($u_t$)과 노이즈($\epsilon_t$)가 없을 때 t-1와 t의 state가 어떻게 관계되어 있는지를 의미하는 n x n matrix.

• $B_t$ : control input($u_t$)이 어떻게 state 변화에 영향을 미치는지를 나타내는 n x l matrix.

• $C_t$ : 현재 로봇의 상태를 나타내는 state($x_t$)와 센서의 관측 정보(observation)이 어떤 관계인지를 나타내는 k x n matrix.

• $\epsilon_t, \delta_t$ : 평균이 0이며 covariance가 각각 $R_t, Q_t$인 확률변수이며, process noise와 measurement noise를 의미한다.

여기서 구별해야 할 점은 $R_t$는 input의 noise가 아닌 process의 noise이다. $R_t$는 control input에서 들어오는 Gaussian noise가 한번의 선형 변환을 거친 전체 state인 $x_t$의 noise 이므로 process noise라고 부른다. 이때 control input인 $u_t$의 covariance는 $M_t$라고 표기한다.

위의 선형 모델을 이용한 motion model과 observation model은 다음과 같다.

• Motion model

• Observation model

Motion model은 prediction step에서, observation model은 correction step에 적용된다.

• Prediction step

• Correction step

t-1에서의 state의 확률은 motion model에 의해 t의 state의 확률이 결정되며(prediction step), prediction step에서 계산된 t에서의 state의 확률은 observation model에 의해서 보정된다. 이와같은 bayes filter식은 여러 확률 모델 분포를 모두 포함하고 있는 식이며, 그 중에서 모든 확률 분포를 Gaussian 확률 분포로 가정하는 모델이 Kalman filter이다. Gaussian으로 확률분포를 표현할 때, 간단히 평균(mean, $\mu$)와 분산(variance, $\Sigma$)으로 표현하기 때문에 두개의 파라미터만으로 분포를 표현할 수 있는 장점이 있다. Kalman filter 알고리즘은 다음과 같다. \[\begin{aligned} 1: & \text{Kalman filter}(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t)\\ &[Prediction step]\\ 2: & \ \ \bar{\mu}_t = A_t \mu_{t-1} + B_t u_t\\ 3: &\ \ \bar{\Sigma_t} = A_t \Sigma_{t-1} A_t^T + R_t\\ &[Correction step]\\ 4: &\ \ K_t = \bar{\Sigma_t}C_t^T(C_t \bar{\Sigma_t}C_t^T + Q_t)^{-1}\\ 5: &\ \ \mu_t = \bar{\mu_t} + K_t(z_t - C_t \bar{\mu_t})\\ 6: &\ \ \Sigma_t = (I - K_t C_t)\bar{\Sigma_t}\\ 7: &\ \ \text{return} \ \ \mu_t, \Sigma_t\\ \end{aligned}\]

위 식은 Kalman filter algorithm을 보여주고 있다. Kalman filter는 bayes filter이기 때문에 prediction과 correction의 두 단계로 이루어 지며, 다소 복잡해 보이지만 한단계씩 이해하면 어렵지 않다.

• Prediction step (Kalman filter)

첫번째 prediction 단계는 복잡하지 않다. 직관적으로 t-1의 평균은 motion model을 통해 t의 평균으로 계산되어 진다(2). 이때 $\mu, \Sigma$에 붙어있는 bar($\bar{\mu}, \bar{\Sigma}$)는 prediction step임을 의미한다. 그 다음으로 covariance는 위에서 설명한 Gaussian linear transformation의 의해 계산되어 진다(3). 이때 $R_t$는 process noise 이며, control input($u_t$)의 covariance가 $M_t$일 때 $R_t = B_t M_t B_t^T$이다. 일반적인 로봇시스템이나 자동차 시스템에서 control input은 wheel encoder로 부터 얻어지는 odometry 정보를 많이 이용하며, encoder 센서의 uncertainty가 $M_t$가 된다.

• Correction step (Kalman filter)

Correction 단계에서는 새로운 변수인 K(Kalman gain)이 추가된다. K는 현재 관측 데이터($z_t$)의 정확도에 따라 predicted state와 관측된 state(observation model을 이용하여 관측값으로부터 추정된 state)의 보정 비율을 결정하는 역활을 한다. 이때 $Q_t$가 observation의 covariance이다. 5번 식에서 ($z_t - C_t\bar{\mu_t}$)는 현재 실제로 관측된 데이터($z_t$)와 현재 위치로 예상되는 위치($\bar{\mu_t}$)에서 기대되는 관측값($C_t\bar{\mu_t}$)과의 차이를 Kalman gain(K)의 크기만큼 보정함으로써, 최종 Gaussian의 평균을 계산한다. 쉬운 이해를 돕기 위해 예를 들어보자. 우리의 로봇은 로봇의 위치에서 부터 주변 lanemark까지의 거리를 측정할 수 있는 로봇이라고 하자. 만약 encoder data와 motion model에 의해서 예상되는 로봇의 위치를 알고 있고, 주변의 lanemark의 위치를 이미 알고 있을 때, 예상되는 lanemark까지의 거리를 계산할 수 있다. 만약 이 예상되는 거리가 10m인데, 실제 lanemark까지의 거리가 9m로 측정이 된다면, 두 값이 오차인 1m는 odometry sensor로 부터 발생한 것일 수 도 있고, 거리를 측정하는 센서로 부터 발생한 것일 수도 있다. 이때 Kalman gain은 10m와 9m중 어느 데이터를 더 신뢰할지를 결정하는 파라미터로 볼 수 있다.

조금 더 쉽게 이해하기 위해 observation의 covariance인 $Q_t$가 무한대라고 해보자. covariance가 무한대라는 의미는 거리측정 센서로부터 얻어진 데이터는 전혀 신뢰 할 수 없다는 것을 의미한다. 이때 K는 0이 되며, $\mu_t = \bar{\mu_t}$가 된다. 즉, 관측된 센서 데이터는 신뢰할 수 없으므로, 예측된 로봇의 위치를 전적으로 신뢰하겠다는 것이다. 반대로 $Q_t$가 0라고 해보자. Covariance가 0이라는 의미는 센서 데이터를 100% 신뢰할 수 있음을 의미한다. 따라서 $Q_t$가 0이라면 $K = C_t^{-1}$이 되며, 5번식은 $\mu_t = C_t^{-1} z_t$가 된다. 즉 로봇의 거리 측정센서로 부터 얻어진 데이터($z_t$)를 전적으로 신뢰하여, 이로부터 로봇의 state를 추정하겠다는 의미이다. 6번식 covariace를 계산하는 부분도 이와 마찬가지로 $Q_t$가 무한대 일때는 최종 covariace는 prediction의 covariance를 그대로 사용하며, $Q_t$가 0일때는 관측데이터가 100%신뢰할 수 있음을 의미하므로 covariace는 0이 된다.

위 그림은 Kalman filter의 과정을 그림으로 표현하였다. 빨간색은 그래프는 prediction step에서 계산한 state의 Gaussian, 초록색은 observation으로 추정한 state의 Gaussian 분포이다. Kalman filter algorithm의 계산에 의해 두 Gaussian분포는 파란색의 최종 Gaussian 분포로 state가 결정된다. 이때 초록색 Gaussian의 variance가 빨간색보다 작기 때문에, 최종 결과는 measurement에 더욱 dominant하다.

Extended Kalman Filter (EKF)

• KF와 마찬가지로 노이즈는 평균(mean)이 0인 Gaussian 분포로 가정한다.

• KF와의 차이점은 motion 모델과 observation 모델을 선형으로 가정하지 않고 비선형 함수로 확장한 것이다.

• 거의 대부분의 실제 시스템은 비선형이다.

Extended Kalman Filter는 아래와 같이 기존 KF의 선형 모델을 비선형 함수인 $g(u_t,x_{t-1})$와 $h(x_t)$로 바꿈으로써 비선형으로 확장한 모델이다. \[\begin{aligned} x_t &= g(u_t, x_{t-1}) + \epsilon_t &\leftarrow &x_t = A_t x_{t-1} + B_t u_t + \epsilon_t\\ z_t &= h(x_t) + \delta_t &\leftarrow &z_t = C_t x_t + \delta_t \end{aligned}\]

하지만 motion 모델과 observation 모델을 비선형으로 확장한 경우 문제가 발생한다. 다음 그림은 이러한 문제를 보여준다.

많은 문제에서 Gaussian 분포를 사용하는 이유는 평균(mean)과 분산(variance) 두개의 파라미터로 분포를 표현함과 동시에 데이터들의 분포를 정확히 반영할 수 있기 때문이다. 따라서 반복적인 계산을 통해 state를 추정하는 문제에서 입력이 Gaussian 분포일 때 출력 또한 Gaussian 분포이여야 한다. 왼쪽 그림은 선형 시스템에서의 입력과 출력을 보여준다. 선형 시스템이기 때문에 입력이 Gaussian 분포일 때 출력 또한 Gaussian 분포가 된다. 하지만 오른쪽 그림과 같이 비선형 시스템의 경우, 입력은 Gaussian 분포이지만 시스템의 비선형성에 의해 출력은 Gaussian 분포가 아니다. 따라서 이런 경우 출력을 평균과 분산으로 표현 할 수 없다. 이러한 문제를 풀기 위해서는 비선형함수를 선형화(Linearization) 시키는 과정이 필요하다.

선형화(Linearization)

EKF에서 비선형 함수를 선형화 시키기 위해서는 1차 Taylor 근사법(First order Talyer Expansion)을 사용한다. 선형 근사화된 model은 다음과 같다.

• Motion model

• Observation model

이떄 비선형 함수들을 state로 편미분하여 matrix를 생성하는데 이 matrix를 Jacobian 이라고 부르며, 두 matrix는 $G_t = \frac{\partial g(u_t, \mu_{t-1})}{\partial x_{t-1}}$, $H_t = \frac{\partial h(\bar{\mu_t})}{\partial x_t}$로 표기한다.

Jacobian Matrix

• Jacobian matrix는 non-square matrix이다.

• 비선형 함수 vector가 $g(x)$일 때 Jacobian $G_x$는 다음과 같이 계산된다.

아래 그림은 Talyer 근사화를 통해 선형화를 하였을 때의 특징을 보여준다.

외쪽 그림은 입력의 분산(variance)가 큰 경우를 보여주며, 오른쪽 그림은 분산이 작은 경우를 보여준다. 분산이 큰 경우 실제 비선형 함수 출력의 평균값과 선형화를 통해 계산된 평균값의 차이가 큰 것을 알 수 있다. 반면 분산이 작은 경우는 선형화를 통해 계산된 평균값이 실제 평균값과 유사함을 알 수 있다. 따라서 선형화 시 선형화 지점으로 부터 멀수록(분산이 클수록) 실제 함수를 반영하지 못한다.

EKF Algorithm

선형화된 motion 모델과 observation 모델을 이용한 bayes filter는 다음과 같다.

• linearized prediction model

• linearized correction model

KF와 마찬가지로 $R_t, Q_t$는 process noise와 measurement noise이다. EKF 알고리즘은 다음과 같다. \[\begin{aligned} 1: & \text{Extended Kalman filter}(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t)\\ &[Prediction step]\\ 2: & \ \ \bar{\mu}_t = g(u_t, \mu_{t-1})\\ 3: &\ \ \bar{\Sigma_t} = G_t \Sigma_{t-1} G_t^T + R_t\\ &[Correction step]\\ 4: &\ \ K_t = \bar{\Sigma_t}H_t^T(H_t \bar{\Sigma_t}H_t^T + Q_t)^{-1}\\ 5: &\ \ \mu_t = \bar{\mu_t} + K_t(z_t - h(\bar{\mu_t}))\\ 6: &\ \ \Sigma_t = (I - K_t H_t)\bar{\Sigma_t}\\ 7: &\ \ \text{return} \ \ \mu_t, \Sigma_t\\ \end{aligned}\]

EKF 알고리즘과 KF 알고리즘의 차이는 KF에서 선형함수를 통해 평균($\mu$)를 구하는 2,5번 식에서 선형함수 대신 비선형 함수가 사용되었다. 그리고 3,4번 식에서 선형함수의 $A_t, C_t$ Matrix는 Jacobian matrix인 $G_t, H_t$로 수정되었다. 여기서 $R_t$는 process noise이며, control input의 covariance matrix가 $M_t$일 때 $R_t = V_t M_t V_t^T$이다. 여기서 $V_t$는 $g(u_t,\mu_{t-1})$를 control input인 $u_t$로 편미분한 Jacobian이다.

여기까지 EKF에 대한 설명을 마친다. 다음 글에서는 실제 robot의 모델을 통해 EKF를 이해해보자.

본 글을 참조하실 때에는 출처 명시 부탁드립니다.

[SLAM] Motion Model & Observation model

Motion model과 observation model 설명.

본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.

Motion Model & Observation Model

이번 글에서는 SLAM의 framework에서 중요한 Motion model과 Observation model에 대해서 설명한다. 이전 post(Bayes Filter)에서 설명한 것처럼 SLAM은 다음과 같이 recursive bayes filter의 식으로 표현할 수 있다. \[\begin{aligned} bel(x_t) &= p(x_t \mid z_{1:t},u_{1:t}) \\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t}) bel(x_{t-1}) dx_{t-1}\\ \end{aligned}\]

bayes filter식은 prediction step 과 correction step 으로 나눌 수 있다.

Prediction step

$\overline{bel}(x_t)$ 는 prediction 단계의 state를 나타낸다. prediction step은 control input 데이터($u_t$)와 이전 step의 로봇의 state에 대한 데이터($x_{t-1}$)를 이용하여 현재의 로봇 state($x_t$)의 확률을 추정하는 과정이다. 이 과정에서 $p(x_t \mid x_{t-1}, u_{t})$ 는 입력($u_t$)에 의한 로봇의 움직임을 추정하여 현재의 state의 확률을 계산하는 model이기 때문에 Motion model 이라고 부른다.

Correction step

Correction step은 prediction step에서 예상한 로봇의 위치를 보정하는 단계이다. 즉, input data($u_t$)를 이용하여 현재의 로봇의 위치를 예측하고, 그 위치에서 얻어진 센서 데이터($z_t$)로 부터 예측된 로봇의 위치를 보정하는 단계이다.

이때 $p(z_t \mid x_t)$ 는 observation model 이라 하며, 현재 예측한 state($x_t$)에서 실제 얻어진 센서 데이터($z_t$)가 얻어질 확률을 의미한다.

Motion Model

Motion model은 control input($u_t$)이 이전 state($x_{t-1}$)을 현재 state($x_t$)로 변화시킬 사후확률(posterior probability)를 의미한다. 실제 로봇의 응용에서 motion model은 크게 2가지로 분류된다.

• odometry-based model

• velocity-based model

odometry model은 로봇 혹은 자동차의 바퀴에 달린 wheel encoder의 센서 데이터를 이용한 모델이며, velocity model은 imu와 같은 관성 센서를 이용한 model이다. velocity model은 wheel encoder와 같은 odometry model을 사용할 수 없을 때 주로 사용하며, odometry model이 velocity model보다 더 정확한 편이다.

위의 그림은 motion model에 의한 state의 uncertainty를 나타낸다. 점이 많이 분포되거나, 어두운 부분일 수록 확률이 높은 부분이다. 로봇의 진행방향과 회전방향에 대한 uncertainty의 크기에 따라서 다른 확률 분포를 보인다. 맨 왼쪽의 그림은 고르게 분포되었을 경우이며, 가운데 그림은 진행방향의 uncertainty가 더 큰경우이며 마지막의 오른쪽 그림은 회전방향의 uncertainty가 진행방향보다 큰 경우를 보여주고 있다.

Observation Model

observation model은 현재 state에서의 센서 데이터의 확률을 의미한다. 로봇 분야에서 많이 사용하는 Laser Scanner데이터를 이용하여 model을 표현하면 다음과 같다. \[\mathbf{z_t} = \{z_t^1,...,z_t^k\}\]

$\mathbf{z_t}$ 는 각 laser scan 데이터의 그룹을 나타내는 vector이며, $z_t^1$ ~ $z_t^k$는 t 시점에서의 각 scan 데이터를 의미한다. 따라서 최종 observation model은 다음과 같이 표현된다. \[p(z_t \mid x_t) = \prod_{i=1}^{k} p(z_t^i \mid x_t)\]

즉 observation은 현재 state에서의 각 센서 데이터들의 확률의 곱으로 표현된다.

자세한 Motion & Observation model에 대해서는 Freiburg 강의-bayes filter를 참조하기 바란다.

[SLAM] Bayes filter(베이즈 필터)

SLAM framework에서 Bayes filter에 대한 설명.

본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.

SLAM(Simultaneous Localization and Mapping)

SLAM은 simultaneous localization and mapping의 줄임말로 위치추정(localization)과 지도생성(mapping)을 동시에 하는 연구분야를 의미한다. 이는 닭이 먼저냐 달걀이 먼저냐의 문제와 비슷하다. 자기의 위치를 추정하기 위해서는 주변환경에 대한 정보가 필요하다. 반면에 로봇이 얻을 수 있는 데이터를 이용해서 지도를 만들기 위해서는 로봇이 자신의 위치가 어디에 있는지를 정확히 알아야 한다. 따라서 위치를 알 수 없으면 지도를 만들 수 없고, 반대로 지도가 없으면 위치를 알 수 없다. 이러한 문제를 풀기 위해서 지도의 생성과 위치 추정을 동시에 수행하는 것이 SLAM이다.

State Estimation

State estimation은 로봇에 주어지는 입력과, 로봇의 센서로부터 얻어지는 데이터로부터 현재의 로봇의 위치인 state와 주변환경에 대한 지도를 추정 방법이다. \[p(\mathbf{x}\mid \mathbf{z}, \mathbf{u})\]

위의 식은 기본적인 state estimation을 의미한다. $\mathbf{x}$ 는 로봇의 위치 및 지도(주변의 land mark들의 위치)를 의미하는 vector이며, $\mathbf{z}$는 로봇의 센서로부터 얻어지는 데이터로 observation이라고 부르며, $\mathbf{u}$ 는 센서의 움직임을 제어하는 입력으로 control input이라고 부른다. state estimation은 이러한 control input과 observation의 데이터를 통해 현재의 위치와 지도를 추정한다.

Bayes Theorem

베이즈 정리는 확률론과 통계학에서 두 확률변수의 사전확률(prior)과 사후확률(posterior) 사이의 관계를 나타내는 정리이다. \[P(A \mid B) = \frac{P(B \mid A)P(A)}{P(B)}\]

$P(A)$ 는 A의 prior로, 사건 B에 대한 어떠한 정보를 알지 못하는 것을 의미한다. $P(A \mid B)$ 는 B의 값이 주어진 경우 A의 posterior이다. $P(B \mid A)$ 는 A가 주어졌을 때 B의 조건부 확률이다.

bayes 정리의 자세한 내용은 wiki를 참고한다.

Recursive Bayes Filter

위에서 설명한 state estimation은 bayes filter의 과정으로 설명할 수 있으며, 각 step의 state를 반복적으로 계산함으로써 계산할 수 있기 때문에 recursive bayes filter로 부른다. 전체적인 recursive bayes filter의 식은 다음과 같다. \[\begin{aligned} bel(x_t) &= p(x_t \mid z_{1:t},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}) \\ &= \eta p(z_t \mid x_t)p(x_t \mid z_{1:t-1},u_{1:t}) \\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}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}\\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t})p(x_{t-1} \mid z_{1:t-1}, u_{1:t}) dx_{t-1}\\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t})p(x_{t-1} \mid z_{1:t-1}, u_{1:t-1}) dx_{t-1}\\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t}) bel(x_{t-1}) dx_{t-1}\\ \end{aligned}\]

위의 식은 recursive bayes filter를 유도하는 과정을 모두 표현하고 있기 때문에 다소 복잡해 보인다. 우선 전체적인 식을 이해하기 위해서 맨 처음과 맨 마지막 식만을 보면 다음과 같다. \[\begin{aligned} bel(x_t) &= p(x_t \mid z_{1:t},u_{1:t}) \\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t}) bel(x_{t-1}) dx_{t-1}\\ \end{aligned}\]

$bel(x_t)$ 는 처음부터 현재까지의 observation( $z$ )와 control input( $u$ )을 알고 있을 때 현재 state( $x_t$ )의 확률을 의미한다. 위의 식에서 $bel(x_t)$ 의 식은 $bel(x_{t-1})$ 의 integral로 표현되어 있기 때문에 만약 $p(z_t \mid x_t)$ 와 $p(x_t \mid x_{t-1}, u_t)$ 에 대한 정보를 알고 있다면 반복적인 계산을 통해 현재 state의 확률을 계산할 수 있음을 알 수 있다. 여기서 $p(z_t \mid x_t)$ 는 현재의 state에서 센서 데이터의 확률인 observation model이며, $p(x_t \mid x_{t-1}, u_t)$ 은 현재의 control input에 대해 이전 state에서 현재 state로의 update를 나타내는 motion model를 의미한다. 위의 식을 Recursive bayes filter라고 한다. Recursive bayes filter는 Kalman filter의 기본이 되는 식이다. 다음은 recursive bayes filter의 유도과정을 간단하게 살펴본다. 유도에 관심이 없고 전체적인 흐름만 보고자 한다면 다음 설명은 넘어가도 좋다.

Recursive Bayes Filter의 유도과정

control input과 observation을 알고 있을 때 현재 state의 확률을 의미하는 belief의 정의 \[\begin{aligned} bel(x_t) &= p(x_t \mid z_{1:t},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}) \\ \end{aligned}\]

기본적인 bayes rule이다. 현재 시점 t의 observation은 현재의 state에서 얻어진 data이므로 따로 분리하여 위와 같이 정의한다. $p(x_t \mid z_{1:t-1},u_{1:t})$ 는 t시점까지의 control input, 그리고 t-1시점 까지의 observation을 알고 있을 때의 현재 시점 t의 state, $p(z_t \mid x_t, z_{1:t-1}, u_{1:t})$ 는 현재 state에서 얻어진 observation의 확률이다. $\eta$ 는 normalize term이다. \[\begin{aligned} bel(x_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}) \\ &= \eta p(z_t \mid x_t)p(x_t \mid z_{1:t-1},u_{1:t}) \\ \end{aligned}\]

Markov Assumption은 현재의 state는 바로 이전 state에 의해서만 영향을 받는다는 것이다. 즉 이전 state를 결정하기 위한 데이터들은 알지 못해도 이전 state만 알고 있다면 현재 state를 결정할 수 있다는 것이다. Markov assumtion에 의해 $p(z_t \mid x_t, z_{1:t-1}, u_{1:t})$ 는 $p(z_t \mid x_t)$로 표현 할 수 있다. 왜냐하면 현재의 observation은 현재의 state인 $x_t$에만 영향을 받으며, $z_{1:t-1}$ 와 $u_{1:t}$는 현재 state $x_t$에만 영향을 미치기 때문이다. \[\begin{aligned} bel(x_t) &= \eta p(z_t \mid x_t)p(x_t \mid z_{1:t-1},u_{1:t}) \\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}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}\\ \end{aligned}\]

다음 식은 total probability(전체확률) 법칙에 의해 위와같이 전개된다. 전체확률 법칙은 간단하게 표현하면 다음과 같다. \[P(A) = \int_B P(A \mid B)P(B) dB\]

즉 A는 $x_t$ 이며, B는 $x_{t-1}$ 이다. \[\begin{aligned} bel(x_t) &= \eta p(z_t \mid x_t)\int_{x_{t-1}}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}\\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t})p(x_{t-1} \mid z_{1:t-1}, u_{1:t}) dx_{t-1}\\ \end{aligned}\]

위 과정은 앞에서 설명한 Markov assumtion에 의해서 $x_t$ 에 영향을 미치는 $x_{t-1}$ 과 $u_t$만 남기고 정리된다. \[\begin{aligned} bel(x_t) &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t})p(x_{t-1} \mid z_{1:t-1}, u_{1:t}) dx_{t-1}\\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t})p(x_{t-1} \mid z_{1:t-1}, u_{1:t-1}) dx_{t-1}\\ \end{aligned}\]

이 과정 또한 Markov assumption으로 $p(x_{t-1} \mid z_{1:t-1}, u_{1:t})$ 에서 $u_t$는 t-1시점의 state인 $x_{t-1}$ 에 영향을 미치지 않기 때문에 제거 될 수 있다. \[\begin{aligned} bel(x_t) &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t})p(x_{t-1} \mid z_{1:t-1}, u_{1:t-1}) dx_{t-1}\\ &= \eta p(z_t \mid x_t)\int_{x_{t-1}}p(x_t \mid x_{t-1}, u_{t}) bel(x_{t-1}) dx_{t-1}\\ \end{aligned}\]

따라서 위와 같은 과정을 통해 최종적으로 식은 위와같이 정리되며, recursive bayes filter의 식으로 정리된다.