[SLAM] Extended Kalman Filter(EKF) SLAM

on SLAM

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

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

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

이 글에서는 첫번째 방법인 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을 위한 가정

초기화

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을 정리하면 다음과 같다.

이 예제에서는 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 또한 줄어들게 된다.

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