[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의 실제 계산과정은 다음 글에서 자세히 설명한다.

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


[SLAM] Extended Information Filter(EIF) SLAM

on SLAM

또 다른 종류의 filter인 Extended Information Filter(EIF) SLAM에 대해서 설명한다.

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

Information Filter는 Kalman filter의 변형으로 추후 계산상의 이점을 갖기 위한 표현 방법이다. 두 표현법의 가장 직관적인 이해 방법은 두 filter의 matrix form의 의미를 이해하는 것이다. Kalman filter의 covariance matrix는 각 element간의 불확실성에 대한 정보를 표현한다. 즉 두 element들(로봇의 위치와 landmark, 혹은 landmark 끼리)의 관계가 명확할 수록 covariance matrix값은 작고, 불확실할수록 크다. 반면에 Information filter에서 Information matrix의 값은 covariance matrix와는 반대로, 두 관계가 정확할수록 값이 크다. 즉 두 element사이 정보의 정확도를 표현하는 matrix라고 생각할 수 있다.

Information Filter

위 식은 EKF와 EIF에서의 Gaussian 표현방법을 보여준다. $\Omega = \Sigma^{-1}$는 Information matrix라고 부르며, 각 성분간의 정보 정확성을 표현한다. $\xi = \Sigma^{-1}\mu$는 Information vector라고 부른다.

Gaussian Distribution in Information Form

그렇다면 위의 표현방법을 이용하여 Gaussian 분포를 표현해보자. \[\begin{aligned} p(x) &= det(2 \pi \Sigma)^{-\frac{1}{2}} exp(-\frac{1}{2} (x-\mu)^T \Sigma^{-1} (x-\mu))\\ &= det(2 \pi \Sigma)^{-\frac{1}{2}} exp(-\frac{1}{2}x^T\Sigma^{-1}x + x^T\Sigma^{-1}\mu -\frac{1}{2}\mu^T\Sigma^{-1}\mu)\\ &= det(2 \pi \Sigma)^{-\frac{1}{2}} exp(-\frac{1}{2}\mu^T \Sigma^{-1}\mu)exp(-\frac{1}{2}x^T\Sigma^{-1}x + x^T\Sigma^{-1}\mu)\\ &= \eta exp(-\frac{1}{2}x^T\Sigma^{-1}x + x^T\Sigma^{-1}\mu)\\ &= \eta exp(-\frac{1}{2}x^T \Omega x + x^T \xi) \end{aligned}\]

위 식은 mean과 covariance로 표현된 Gaussian 분포로 부터 Information matrix와 vector로 표현된 Gaussian 분포를 유도하는 과정을 보여준다. 여기서 $\eta$ 상수를 의미한다. 상수까지 모두 표현한 Gaussian 분포는 다음과 같다. \[p(x) = \frac{exp(-\frac{1}{2}\mu^T \xi)}{det(2 \pi \Omega^{-1})^{\frac{1}{2}}} exp(-\frac{1}{2}x^T \Omega x + x^T \xi)\]

Marginalization and Conditioning

위의 표는 covariance matrix와 information matrix가 block matrix로 표현 되었을 때, marginalization과 conditioning을 계산하는 식을 보여주고 있다. Covariance matrix 형태일 경우에 marginalization 계산식은 block matrix에서 바로 가져올 수 있으므로 간단하다. 반면 conditioning의 경우 상대적으로 복잡하며, 계산량이 많은 inverse($\Sigma_{\beta\beta}^{-1}$)가 포함되어 있다. Information matrix 형태의 경우 conditioning계산은 간단하지만, marginalization 계산은 상대적으로 복잡하다(이 계산 또한 inverse를 포함하고 있기 때문에). 따라서 어떤 연산을 주로 하느냐에 따라서 더 유리한 표현방법을 선택할 수 있다.

Information Filter Algorithm

Information filter 알고리즘은 Kalman filter 알고리즘에서 표현방법을 바꾼 알고리즘으로 생각하면 된다. 우선 선형 모델에서의 Kalman filter알고리즘은 아래와 같다. 비선형 모델을 고려한 Extended Information Filter(EIF)는 선형 모델을 설명 후 다루기로 한다. 아직 Kalman filter에 익숙하지 않으면 EKF (Extended Kalman Filter)를 우선 공부하기를 추천한다. \[\begin{aligned} 1: & \text{Kalman filter}(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t)\\ 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\\ 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}\]

KF에서 IF로 바꾸는 과정에는 Information matrix와 vector의 정의를 이용한다. \[\begin{aligned} \Omega &= \Sigma^{-1}\\ \xi &= \Sigma^{-1}\mu \end{aligned}\]

Prediction step

2,3번은 Kalman filter의 prediction step이다. Information의 정의에 의해 Information matrix는 다음과 같이 정의된다. \[\begin{aligned} \bar{\Omega}_t &= \bar{\Sigma}_t^{-1}\\ &= (A_t \Omega_{t-1}^{-1} A_t^T + R_t)^{-1} \end{aligned}\]

또한 information vector는 다음과 같다. \[\begin{aligned} \bar{\xi}_t &= \bar{\Sigma}_t^{-1}\bar{\mu_t}\\ &= \bar{\Omega}_t (A_t\mu_{t-1}+B_t u_t)\\ &= \bar{\Omega}_t (A_t \Omega_{t-1}^{-1} \xi_{t-1}+ B_t u_t)\\ \end{aligned}\]

Information matrix와 vector를 구하는 과정은 단순히 정의를 이용하여 유도를 하는 과정이므로 어렵지 않다. 이로써 쉽게 Information Filter의 prediction 과정을 유도하였다. 여기서 중요한점은 KF의 경우 prediction의 계산량이 크지 않았다. 하지만 IF로 바뀌면 prediction식에 Information matrix의 inverse가 포함되어 계산량이 증가하게 된다.

Correction step

IF의 correction step은 bayes filter의 measurement update를 이용하여 유도한다. bayes filter의 measurement update는 다음과 같다. \[bel(x_t) = \eta p(z_t \mid x_t) \bar{bel}(x_t)\]

위의 식에 prediction의 Gaussian 분포와 observation model의 Gaussian 분포를 대입하여 정리한다. \[\begin{aligned} bel(x_t) &= \eta p(z_t \mid x_t) \bar{bel}(x_t)\\ &= \eta' exp(-\frac{1}{2}(z_t-C_tx_t)^TQ_t^{-1}(z_t - C_tx_t))exp(-\frac{1}{2}(x_t-\bar{\mu}_t)^{T}\bar{\Sigma}_t^{-1}(x_t-\bar{\mu}_t))\\ &= \eta' exp(-\frac{1}{2}(z_t-C_tx_t)^TQ_t^{-1}(z_t - C_tx_t)-\frac{1}{2}(x_t-\bar{\mu}_t)^{T}\bar{\Sigma}_t^{-1}(x_t-\bar{\mu}_t))\\ &= \eta'' exp(-\frac{1}{2} x_t^TC_t^TQ_t^{-1}C_tx_t + x_t^TC_t^TQ_t^{-1}z_t - \frac{1}{2}x_t^T\bar{\Omega}_tx_t + x_t^T\bar{\xi}_t)\\ &= \eta'' exp(-\frac{1}{2}x_t^T [C_t^TQ_t^{-1}C_t + \bar{\Omega}_t]x_t + x_t^T [C_t^TQ_t^{-1}z_t + \bar{\xi}_t])\\ &= \eta'' exp(-\frac{1}{2} x_t^T \Omega_t x_t + x_t^T \xi_t) \end{aligned}\]

bayes filter의 measurement update식으로 정리한 식과, 위에서 정리했던 Information form의 Gaussian 분포의 식을 비교해 보자. 두 식을 비교해보면 아래와 같이 correction step에서의 information matrix와 vector의 계산 식을 구해낼 수 있다. \[\begin{aligned} \Omega_t &= C_t^TQ_t^{-1}C_t + \bar{\Omega}_t \\ \xi_t &= C_t^TQ_t^{-1}z_t + \bar{\xi}_t \end{aligned}\]

Information Filter Algorithm 정리

\[\begin{aligned} 1: & \text{Information Filter}(\xi_{t-1}, \Omega_{t-1} u_t, z_t)\\ &\text{[Prediction step]}\\ 2: & \ \ \bar{\Omega}_t = (A_t \Omega_{t-1}^{-1} A_t^T + R_t)^{-1}\\ 3: &\ \ \bar{\xi}_t = \bar{\Omega}_t (A_t \Omega_{t-1}^{-1} \xi_{t-1}+ B_t u_t)\\ &\text{[Correction step]}\\ 4: &\ \ \Omega_t = C_t^TQ_t^{-1}C_t + \bar{\Omega}_t\\ 5: &\ \ \xi_t = C_t^TQ_t^{-1}z_t + \bar{\xi}_t\\ 6: &\ \ \text{return} \ \ \xi_t, \Omega_t\\ \end{aligned}\]

위에서 유도했던 Information filter의 과정을 정리하면 다음과 같다. 대부분의 과정은 KF와 유사하며, 다른점이 있다면 Kalman gain을 구하는 과정이 필요없다. 또한 앞에서 언급했었지만 IF와 KF의 차이점은 KF의 경우 Kalman gain을 구하는 correction step에서 계산량이 많지만, IF의 경우 prediction step에서 Information matrix를 구할 때 inverse 계산때문에 계산량이 많다. IF의 correction step에 있는 $Q^{-1}$는 센서의 uncertainty이기 때문에 미리 계산 가능함으로 계산량에 영향을 주지 않는다.

Extended Information Filter

Extended Information Filter는 EKF와 마찬가지로 선형모델을 비선형 모델로 확장한 IF모델이다. 비선형 모델 및 Taylor expansion을 통한 선형화 방법, Jacobian 등에 대한 설명은 이전 글 (EKF, EKF 예제)에서 다루었으므로 생략한다.

EIF도 마찬가지로 Tayor expansion을 통한 1차 선형화를 하고, Jacobian을 이용하여 비선형 함수 출력의 Gaussian 분포를 계산한다. motion model과 observation model의 선형화한 함수는 다음과 같다. \[\begin{aligned} g(u_t, x_{t-1}) &\approx g(u_t,\mu_{t-1}) + G_t (x_{t-1}-\mu_{t-1})\\ h(x_t) &\approx h(\bar{\mu}_t) + H_t(x_t - \bar{\mu}_t) \end{aligned}\]

Prediction step

EKF의 prediction step은 다음과 같다. \[\begin{aligned} \bar{\Sigma}_t &= G_t \Sigma_{t-1} G_t^T + R^T\\ \bar{\mu}_t &= g(u_t,\mu_{t-1}) \end{aligned}\]

이를 Information matrix와 vector의 관계를 통해 변형시키면 다음과 같다. \[\begin{aligned} \bar{\Omega}_t &= (G_t \Omega_{t-1}^{-1} G_t^T + R^T)^{-1}\\ \bar{\xi}_t &= \bar{\Omega}_t g(u_t, \Omega_{t-1}^{-1}\xi_{t-1})\\ &= \bar{\Omega}_t g(u_t, \mu_{t-1}) \end{aligned}\]

이 EIF의 prediction step을 계산하는 과정에서 중요하게 보아야 할 부분은 비선형함수의 출력을 계산하기 위해서는 이전 step의 평균(mean, $\mu_{t-1}$)이 계산되어야 한다는 점이다.

Correction step

EIF의 correction step도 IF에서 계산했던것 처럼 아래의 bayes filter의 measurement update식으로 부터 계산한다. \[\begin{aligned} bel(x_t) = & \eta exp(-\frac{1}{2}(z_t - h(\bar{\mu}_t)-H_t(x_t-\bar{\mu}_t))^TQ_t^{-1}(z_t - h(\bar{\mu}_t)-H_t(x_t-\bar{\mu}_t))\\ &-\frac{1}{2}(x_t-\bar{\mu}_t)^T\bar{\Sigma}_t^{-1}(x_t-\bar{\mu}_t)) \end{aligned}\]

계산 과정은 따로 보이지 않겠다. IF에서 정리한것과 같이 정리하면 다음과 같이 correction step의 식으로 정리된다. \[\begin{aligned} \Omega_t & = \bar{\Omega}_t + H_t^T Q_t^{-1} H_t\\ \xi_t & = \bar{\xi}_t + H_t^T Q_t^{-1} (z_t - h(\bar{\mu}_t) + H_t\bar{\mu}_t) \end{aligned}\]

Extended Information Filter Algorithm 정리

위에서 계산된 식을 정리하면 다음과 같다. \[\begin{aligned} 1: & \text{Information Filter}(\xi_{t-1}, \Omega_{t-1} u_t, z_t)\\ &\text{[Prediction step]}\\ 2: & \ \ \mu_{t-1} = \Omega_{t-1}^{-1} \xi_{t-1}\\ 3: & \ \ \bar{\Omega}_t = (G_t \Omega_{t-1}^{-1} G_t^T + R^T)^{-1}\\ 4: & \ \ \bar{\mu}_t = g(u_t, \mu_{t-1})\\ 5: &\ \ \bar{\xi}_t = \bar{\Omega}_t \bar{\mu}_t\\ &\text{[Correction step]}\\ 6: &\ \ \Omega_t = \bar{\Omega}_t + H_t^T Q_t^{-1} H_t\\ 7: &\ \ \xi_t = \bar{\xi}_t + H_t^T Q_t^{-1} (z_t - h(\bar{\mu}_t) + H_t\bar{\mu}_t)\\ 8: &\ \ \text{return} \ \ \xi_t, \Omega_t\\ \end{aligned}\]

EKF와 다른점은 Kalman gain을 구하지 않아도 된다는 점이고, EIF의 prediction 과정에서 비선형 함수의 출력을 계산하기 위해서는 평균값을 다시 계산해야 한다는 것이다.

EIF Vs EKF

EIF를 EKF와 비교하여 정리하면 다음과 같다.

하지만 SLAM 분야에서는 EIF가 많이 사용된다. 그 이유는 다음 글에서 설명하도록 한다.

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


[SLAM] Unscented Kalman Filter(UKF)

on SLAM

EKF와 다른 선형과 과정을 이용하는 UKF를 설명한다.

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

이전 글에서는 Taylor expansion을 이용한 선형화를 이용하여 Kalman filter를 적용하는 EKF SLAM에 대해서 다루었다. 이번 글에서 다룰 내용인 Unscented Kalman Filter(UKF)는 Taylor expansion을 이용하여 선형화 하지 않고 다른 방법을 통해 Gaussian 분포를 유지하려는 노력이다.

Unscented Transform(UT)

아래 그림은 UKF가 비선형함수 출력의 Gaussian 분포를 추정하는 방법을 보여주고 있다. 이를 Unscented Transform(UT)라고 부른다. 왼쪽 그림의 타원은 입력의 Gaussian 분포를 의미하며, 타원안에 있는 point들은 UT에 의해 선택된 sigma point들이다. 이 point들을 비선형 함수의 입력으로 사용하여 출력을 계산하고, 이 과정을 그림으로 그린 것이 왼쪽 그림이다. 비선형 함수이므로 point들의 원래 형태가 깨지고 새로운 형태로 point들이 분포한 것을 알 수 있다. UT는 이렇게 새롭게 분포된 point들의 새로운 mean값과 covariance값을 계산함으로써 오른쪽 그림과 같이 새로운 Gaussian 분포를 생성한다.

UT의 과정을 정리하면 다음과 같다.

Sigma point Selection (시그마 포인트 선택)

Unscented transform을 하기 위해서는 가장 먼저 sigma point를 선정해야 한다. 시그마 포인트는 $\chi$로 표기하며 다음과 같이 선택한다. \[\begin{aligned} \chi^{[0]} &= \mu \\ \chi^{[i]} &= \mu + (\sqrt{(n+\lambda)\Sigma}) ^i\ \ for\ \ i = 1,\cdots,n \\ \chi^{[i]} &= \mu - (\sqrt{(n+\lambda)\Sigma}) ^{i-n}\ \ for\ \ i = n+1,\cdots,2n \\ \end{aligned}\]

위 식에서 $n$은 dimension의 크기며, $\lambda$는 scaling parameter이다. $()^{i}$ 는 covariance matrix의 i번째 열 vector를 의미한다. 첫번째 sigma point는 평균(mean) vector가 되며, 그 다음 sigma point는 dimension의 크기에 따라서 갯수가 결정된다. 2-dim 일 경우에는 4개의 point가 추가되어 총 5개가 되며, 3-dim인 경우에는 6개가 추가되어 총 7개가 된다. Sigma point를 계산하는 식에 covariance matrix의 square root를 계산해야 하는데, matrix의 square root는 다음과 같이 계산한다.

Matrix Square Root

Matrix의 square root를 구하기 위해서는 다음 식과 같이 matrix를 어떤 matrix의 제곱의 형태로 표현해야 한다. 이를 위해서 다음의 두가지 방법을 사용할 수 있다. \[\Sigma = S S\]

1. Eigen Value Decomposition

Covariace matrix는 정방행렬이기 때문에 Eigen value decomposition을 통해 다음과 같이 표현할 수 있다. \[\begin{aligned} \Sigma &= V D V^{-1}\\ &= V \begin{pmatrix} d_{11} & \cdots & 0\\ 0 & & 0 \\ 0 & & d_{nn} \end{pmatrix} V^{-1}\\ &= V \begin{pmatrix} \sqrt{d_{11}} & \cdots & 0\\ 0 & & 0 \\ 0 & & \sqrt{d_{nn}} \end{pmatrix} \begin{pmatrix} \sqrt{d_{11}} & \cdots & 0\\ 0 & & 0 \\ 0 & & \sqrt{d_{nn}} \end{pmatrix} V^{-1}\\ &= S S \end{aligned}\]

Matrix는 $D$는 $\Sigma$의 eigen value를 diagonal 값으로 갖는 matrix이다. 따라서 S는 다음과 같이 표현할 수 있다. \[S= V \begin{pmatrix} \sqrt{d_{11}} & \cdots & 0\\ 0 & & 0 \\ 0 & & \sqrt{d_{nn}} \end{pmatrix} V^{-1}\] \[SS = (V\sqrt{D}V)(V\sqrt{D}V) = VDV^{-1} = \Sigma\]

2. Cholesky Factorization

두번쨰 방법은 Cholesky factorization을 이용하는 방법이다. Cholesky factorization은 matrix A가 대칭(symmetric)이고 positive definite일 때 lower triangular matrix L의 \(LL^T\) 로 분해하는 방법이다. 자세한 유도과정은 wiki를 참고하자.

cholesky factorization의 결과가 numerical하게 더 stable하기 때문에 UKF에서 주로 사용된다.

Weight Selection (가중치 선택)

선택된 Sigma point들은 각각 weight를 갖고 있으며, Gaussian 분포를 다시 계산할 때 사용된다. Weight의 합은 1이 되며($\Sigma \omega^{[i]} = 1$) 다음과 같이 정의한다. \[\begin{aligned} \omega_m^{[0]} &= \frac{\lambda}{n+\lambda}\\ \omega_c^{[0]} &= \omega_m^{[0]} + (1 - \alpha^2 + \beta)\\ \omega_m^{[i]} &= \omega_c^{[i]} = \frac{1}{2(n+\lambda)} \ \ for \ \ i = 1, \cdots, 2n \end{aligned}\]

$\omega_m$은 mean 계산을 위한 weight이며, $\omega_c$ 는 covariance 계산을 위한 weight이다. $\lambda, \alpha, \beta$는 user parameter이며 값의 선택에 따라 결과가 달라진다.

Gaussian Distribution Calculation (가우시안 분포 계산)

위의 과정을 통해 dimension에 맞는 sigma point들과 weight가 계산되었다. 이제 계산된 sigma point들을 비선형 함수($g(x)$)의 입력으로 사용하고, 비선형 함수의 출력을 이용하여 Gaussian 분포를 추정한다. 출력 Gaussian 분포의 mean과 covariance는 다음과 같이 계산된다. \[\begin{aligned} \mu' &= \sum_{i=0}^{2n} \omega_m^{[i]} g(\chi^{[i]})\\ \Sigma' &= \sum_{i=0}^{2n} \omega_c^{[i]} (g(\chi^{[i]}) - \mu') (g(\chi^{[i]}) - \mu ')^T \end{aligned}\]

위 그림은 Unscented transform을 통해 추정한 Gaussian 분포를 보여준다.

위 그림은 선형함수와 비선형함수에 따른 sigma point들의 위치 변화를 보여준다. 왼쪽 그림은 선형함수를 통해 sigma point를 변형시키고, 오른쪽 그림은 비선형 함수를 통해 sigma point을 변형시켰다. 선형함수의 경우 sigma point들의 분포가 그대로 유지가 되므로 Gaussian 분포 또한 유지된다. 반면 비선형함수의 경우에는 Sigma point들의 분포가 바뀌므로 새로운 Gaussian 분포가 계산되었다.

Unscented Transformation (UT) Parameter

Unscented transform에는 여러개의 user parameter가 있는데 unique solution이 있는 것은 아니지만 일반적으로 다음과 같은 가이드 라인을 따른다. \[\begin{aligned} \kappa &\ge 0\\ \alpha &\in (0,1]\\ \lambda &= \alpha^2 (n+\kappa) -n\\ \beta &= 2 \end{aligned}\]

$\kappa$는 sigma point가 mean으로 부터 얼마나 떨어져 있는지를 조절하는 parameter이다. 아래 그림은 각 parameter의 변화에 따른 sigma point들의 변화를 보여준다.

Sigma point가 mean값과 매우 가까운 경우는 Taylor expansion을 통한 선형화와 유사하며, 너무 먼 경우는 비선형 함수를 재대로 반영하지 못하므로 적당한 값을 적용해야 한다.

Unscented Kalman Filter (UKF)

UKF는 EKF와 다르게 Taylor expansion의 선형화 과정을 이용하지 않는다. 전체적인 과정은 EKF와 비슷하지만 UKF에는 Jacobian이 없기 때문에 각 단계들이 조금씩 수정된다. 우선 EKF 과정을 살펴보자. \[\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}\]

2, 3번 과정은 비선형 함수 $g(u,\mu)$에 의한 mean과 covariance를 계산하는 과정이다. UKF에서는 Jacobian $G_t$가 없기 때문에 위에서 설명한 Gaussian distribution calculation 과정을 통해 $\bar{\mu}_t$와 $\bar{\Sigma}_t$를 계산한다.

그 다음에는 Kalman gain을 구하는 과정이 필요하다. 하지만 이 과정에서도 역시 Jacobian인 $H_t$가 없기 때문에 다음과 같이 4번부터의 과정이 수정된다. \[\begin{aligned} 1: & \ \ \text{Unscented Kalman filter}(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t)\\ 2: & \ \ \bar{\mu}_t = calculate \ \ mean \ \ using \ \ transformed \ \ sigma \ \ points\\ 3: &\ \ \bar{\Sigma_t} = calculate \ \ covariance \ \ of \ \ transformed \ \ sigma \ \ points\\ 4: &\ \ \bar{\chi}_t = (\bar{\mu}_t, \ \ \bar{\mu}_t+\sqrt{(n+\lambda)\bar{\Sigma}_t}, \ \ \bar{\mu}_t+\sqrt{(n+\lambda)\bar{\Sigma}_t})\\ 5: &\ \ \bar{Z}_t = h(\bar{\chi}_t)\\ 6: &\ \ \hat{z}_t = \sum_{i=0}^{2n}\omega_m^{[i]}\bar{Z}_t^{[i]}\\ 7: &\ \ \hat{S}_t = \sum_{i=0}^{2n}\omega_c^{[i]}(\bar{Z}_t^{[i]}-\hat{z}_t)(\bar{Z}_t^{[i]}-\hat{z}_t)^T + Q_t\\ 8: &\ \ \bar{\Sigma}_t^{x,z} = \sum_{i=0}^{2n}\omega_c^{[i]}(\bar{\chi}_t^{[i]}-\bar{\mu}_t)(\bar{Z}_t^{[i]}-\hat{z}_t)^T\\ 9: &\ \ K_t = \bar{\Sigma}_t^{x,z} S_t^{-1}\\ 10: &\ \ \mu_t = \bar{\mu}_t +K_t (z_t - \hat{z}_t)\\ 11: &\ \ \Sigma_t = \bar{\Sigma}_t - K_t S_t K_t^T \end{aligned}\]

위의 과정은 UKF의 전체 과정을 보여준다. EKF에 비해 다소 단계가 많아진 것 같지만 하나씩 보면 많은 부분이 유사함을 알 수 있다.

원래의 Kalman gain을 계산하는 식은 다음과 같다. \[K_t = \bar{\Sigma_t}H_t^T(H_t \bar{\Sigma_t}H_t^T + Q_t)^{-1}\]

여기서 $\bar{\Sigma_t}H_t^T$는 $\bar{\Sigma}_t^{x,z}$가 되며, $H_t \bar{\Sigma_t}H_t^T + Q_t$는 $S_t$가 된다. 조금 햇갈릴 수도 있으니 서로의 관계를 잘 생각해보자.

\[\begin{aligned} \Sigma_t &= (I-K_t H_t)\bar{\Sigma}_t\\ &= \bar{\Sigma}_t-K_t H_t\bar{\Sigma}_t\\ &= \bar{\Sigma}_t-K_t (\bar{\Sigma}_t^{x,z})^T\\ &= \bar{\Sigma}_t-K_t (\bar{\Sigma}_t^{x,z} S_t^{-1} S_t)^T\\ &= \bar{\Sigma}_t-K_t (K_t S_t)^T\\ &= \bar{\Sigma}_t-K_t S_t^T K_t\\ &= \bar{\Sigma}_t-K_t S_t K_t \end{aligned}\]

위 그림은 UKF와 EKF의 결과 비교그림이다. 왼쪽은 비선형 함수에 의한 실제 분포의 변화를 보여준다. 가운데는 EKF를 통한 transform을 보여주고, 오른쪽은 UT를 이용한 Gaussian 추정을 보여준다. 상황에 따라 UT가 EKF보다 true mean값에 가까운 Gaussian 분포를 추정할 수 있음을 알 수 있다. 하지만 user parameter의 선정이나 비선형 함수에 따라서 결과는 달라질 수 있다.

UKF Vs EKF

마지막으로 UKF와 EKF를 비교해보자.

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


[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 또한 줄어들게 된다.

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


[SLAM] Extended Kalman Filter(EKF) 예제

on SLAM

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

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

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

Motion Model

Robot의 Motion 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에 대해서 설명한다.

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


Pagination