[SLAM] Sparse Extended Information Filter(SEIF) SLAM

on SLAM

Filter의 연산량 개선을 위한 Sparse Extended Information Filter(SEIF) SLAM에 대해서 설명한다.

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

이 글에서는 Extended Information Filter(EIF)의 계산량을 줄이기 위한 방법인 Sparse Extended Information Filter(SEIF)에 대해서 설명하려고 한다. 앞의 글에서 EKF와 EIF에 대해서 설명하였다. EKF는 Gaussian 분포를 mean vector와 covariance matrix로 표현하였고, EIF는 information vector와 information matrix로 표현하였다.

Motivation of SEIF

EKF와 EIF는 모두 앞에서 설명했던것 처럼 prediction step과 correction step으로 나누어 진다. Matrix의 연산중에서 가장 많은 계산량을 요구하는 부분은 matrix의 inverse를 계산하는 부분이다. Matrix의 inverse 계산은 matrix 크기의 quadratic하게 증가한다. EKF는 correction 과정에 inverse의 계산이 필요하며, EIF는 prediction 단계에서 inverse 계산이 필요하다. 따라서 두 알고리즘 모두 순서는 다르지만 inverse의 계산이 필요하기 때문에 알고리즘의 연산 속도는 matrix의 크기에 종속적이게 된다.

알고리즘의 연산속도가 matrix의 크기에 종속적이라는 의미는 무슨 뜻일까? SLAM의 관점에서 본다면, SLAM에서 covariance matrix의 크기는 $3+2n$이다. 즉 landmark의 수가 많아질 수록 covariance matrix의 크기는 증가함을 의미한다. 따라서 로봇이 이동하는 범위가 넓을 수록, 즉 landmark의 수가 많을수록 filter의 속도는 느려지고, 알고리즘을 적용할 수 있는 한계가 발생하게 된다. 이러한 문제를 해결하기 위해서 사용하는 방법이 sparsification이며, 이를 적용한 EIF가 Sparse Extended Information Filter(SEIF)이다.

아래 그림은 로봇이 landmark가 있는 환경을 움직였을 때 covariance matrix와 information matrix를 보여준다. Covariance matrix는 많은 부분이 큰 값을 가지고 있다. 하지만 Information matrix는 diagonal 부분과 몇 개의 off-diagonal 부분만이 큰 값을 가지고 있음을 알수 있다(어두울수록 큰 값이다). 이러한 차이는 불확실함을 표현하는 covariance matrix와 확실한 정보를 나타내는 information matrix의 정의를 생각하면 이해할 수 있다. 하지만 information matrix의 경우 아래 그림에서 흰색에 가까운 영역도 정확히 0이 아닌 매우 작은 non-zero 값이다.

아래 그림은 information matrix와 실제 로봇, landmark의 관계를 보여주고 있다. Informatio matrix에서 가장 왼쪽 위의 $3 \times 3$ matrix는 로봇의 위치에 대한 matrix이며, 다른 off-diagonal 부분은 로봇-landmark 혹은 landmark-landmark의 관계를 나타낸다. Information matrix에서 그림과 같이 off-diagonal중에서 값이 큰 부분은 로봇과 landmark간의 link가 강함을 의미하며, 값이 작은 부분은 link가 약함을 의미한다.

Information matrix에 대해 정리해보자.

그렇다면 앞에서 이야기한 sparsification이란 무엇일까? sparsification은 filter의 연산량이 matrix의 크기에 종속적이지 않도록, information matrix의 non-zero off-diagonal의 수를 한정하는 방법이다.

Information Matrix의 계산 과정 및 Sparsification

로봇의 이동과 landmark의 관측과정에서 information matrix가 어떻게 update되는지 설명한다.

1. 초기상태

로봇의 초기상태에는 로봇 위치에 해당하는 좌상단의 $3\times3$ matrix를 제외하고 모든 element들은 0이다.

2. 첫번째 Landmark 관측

로봇이 첫번째 landmark를 관측했을 때에 해당 landmark의 diagonal information 값이 추가되며, 로봇과 첫번째 landmark간의 information이 추가된다.

3. 두번째 Landmark 관측

로봇이 두번째 landmark를 관측했을 때, 첫번째 landmark를 관측했을때와 마찬가지로 information matrix가 업데이트 된다.

4. 로봇 Motion Update

이제 2개의 landmark가 관측된 상태에서 로봇이 움직였다. 로봇이 움직이면 motion model에 의해 로봇의 위치가 업데이트 되는데 이때 control input의 uncertainty에 의해 process noise가 발생한다. Process noise에 의해 로봇 위치의 uncertainty가 커지고, 이에 따라 로봇위치에 대한 information이 줄어들게 된다(좌상단의 $3\times3$ matrix). 이때 landmark 자체의 information(diagonal 값)은 로봇 위치의 uncertainty에 영향을 받지 않으므로 값이 변하지 않는다. 로봇 위치에 대한 uncertainty가 증가하였기 때문에 로봇과 landmark사이에도 uncertainty가 증가하므로 둘 사이의 link가 약화되고, information값도 줄어들게 된다(off-diagonal 값). 그리고 로봇과 landmark사이의 information값이 줄어들면서, 이와 동시에 직접적으로 연결이 되지 않았던 landmark간에 direct link가 추가된다(information값이 추가된다). 이는 로봇의 움직임이 업데이트되기 전의 관측값에 의해 로봇의 위치로 부터 landmark의 상대위치를 알고 있었으므로 이는 landmark간의 상대위치를 간접적(indirectly)으로 알고있다는 의미이다. 따라서 로봇과 landmark간의 information이 줄어들면서 landmark간의 direct link(information)가 추가된다.

5. Sparsification

Sparsification의 의미는 이전에 관측되었었던 로봇과 landmark에 대한 link를 무시하는(conditional independent로 가정)것을 의미한다. 즉 그림에서는 로봇이 움직인 후 로봇과 landmark 1번의 link를 무시하며, 로봇과 landmark 1번에 대한 link정보를 로봇과 landmark2번 사이의 link, landmark 사이(1번과 2번)의 link에 추가한다.

이때 현재 관측하고 있는 landmark 혹은 관측하고 있다고 생각하는 landmark는 active landmark라고 하며, link가 없거나 끊어진 landmark를 passive landmark라고 한다. Sparsification은 이 active landmark의 숫자를 일정하게 유지하는 방법으로, active landmark 숫자를 적게하면 연산속도는 빠르나 강한 근사화에 따른 에러가 증가하고, 숫자를 크게하면 에러는 작아지나 연산속도가 증가한다. Sparsification은 SLAM의 framework에서 매 iteration마다 계산된다.

SEIF의 실제 계산과정은 다음 글에서 자세히 설명한다.

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