[SLAM] Occupancy Grid Maps

on SLAM

Landmark 기반이 아닌 volumetric 기반의 grid map을 소개한다.

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

이번 글에서는 EKF와 EIF등과 같이 landmark나 feature를 기반의 map표현 방법이 아닌, 센서의 raw 데이터를 이용하여 volumetric하게 map을 표현하는 방법에 대해서 설명하도록 한다.

Feature

Grid Map

위 그림은 grid map을 보여준다.

Grid Map의 가정 1

첫번째 가정: cell을 구성하는 영역은 완전히 binary random variable로 occupied이거나 free이다.

Grid Map의 가정 2

두번째 가정: map은 움직이지 않는다. 즉 static world이다. 대부분의 mapping system은 이러한 가정을 한다.

Grid Map의 가정 3

세번째 가정: map을 이루는 각 cell은 서로 독립적(independent)이다. 따라서 전체 map의 확률은 각 cell 확률의 곱으로 계산되어진다. \[p(m) = \prod_i p(m_t)\]

위 식에서 m은 map전체를 의미하며, $m_i$는 각 cell을 의미한다.

Data를 이용한 Map의 추정

주어진 센서 데이터 \(z_{1:t}\)와 로봇의 위치 \(x_{1:t}\)를 이용하여 map을 추정하면 다음과 같다. \[p(m \mid z_{1:t}, x_{1:t}) = \prod_i p(m_i \mid z_{1:t},x_{1:t})\]

여기서 각 cell을 의미하는 \(m_i\)는 binary random variable이다.

각 cell의 확률을 계산하면 다음과 같다.

위 식의 유도과정에는 bayes rule과 markov assumption이 사용된다. 위에서 계산된 $p(m_i \mid z_{1:t},x_{1:t})$는 cell이 occupied일 확률이며, cell이 free space일 확률을 구하면 다음과 같다.

이제 위에서 구한 식을 이용하여 cell이 occupied확률을 계산해보자. 아래 식의 binary random variable의 특징을 보자. \[\begin{aligned} \frac{p(x)}{1-p(x)} &= Y \\ p(x) &= Y - Yp(x)\\ p(x) (1+Y) &= Y\\ p(x) &= \frac{Y}{1+Y}\\ p(x) &= \frac{1}{1+\frac{1}{Y}} \end{aligned}\]

즉 binary random variable의 두 확률의 ratio를 이용하면 probability를 계산할 수 있다. 이러한 특징을 이용하여 cell의 occupied확률을 계산해 보자. \[\frac{p(m_i \mid z_{1:t}, x_{1:t})}{p(\lnot m_i \mid z_{1:t}, x_{1:t})} = \frac{p(m_i \mid z_{1:t}, x_{1:t})}{1-p(m_i \mid z_{1:t}, x_{1:t})}\]

즉 probability를 정리하면 3개의 항으로 생각할 수 있다. 가운데 항은 recursive term으로 볼 수 있으며, 오른쪽 항은 맵에대한 prior term으로 볼 수 있다.

여기서 우리가 계산해야 하는 cell의 확률 $p(m_i \mid z_{1:t},x_{1:t})$을 효율적으로 계산하기 위해 Log odds notation을 이용한다.

Log Odds Notation

log odds notation은 다음과 같이 정의된다. \[l(m_i \mid z_{1:t},x_{1:t}) = log\big( \frac{p(m_i \mid z_{1:t},x_{1:t})}{1 - p(m_i \mid z_{1:t},x_{1:t})}\big)\]

log odds notation을 이용하여 식을 정리하면 다음과 같다. \[l(m_i \mid z_{1:t},x_{1:t}) = l(m_i \mid z_t,x_t) + l(m_i \mid z_{1:t-1},x_{1:t-1}) - l(m_i)\]

여기서 $l(m_i \mid z_{1:t-1},x_{1:t-1})$은 recursive하게 계산되는 항이며, $l(m_i)$은 prior 확률이다. 따라서 inverse sensor model인 $l(m_i \mid z_t,x_t)$가 계산된다면 효율적으로 단순한 더하기로 $l(m_i \mid z_{1:t},x_{1:t})$를 계산할 수 있다. 이렇게 계산된 log odds notation으로 부터 cell의 확률을 아래와 같이 계산할 수 있다. \[p(m_i \mid z_{1:t},x_{1:t}) = 1- \frac{1}{1+ exp(l(m_i \mid z_{1:t},x_{1:t}))}\]

전체적인 grid map의 algithm은 다음과 같다.

Inverse Sensor Model

앞에서 grid map의 update algorithm에 대해서 살펴보았으며, inverse sensor model인 $p(m_i \mid z_t, x_t)$를 알아야 grid map을 생성할 수 있다. $p(m_i \mid z_t, x_t)$은 로봇의 위치와 센서 관측값이 있을 때 각 cell의 occupied 되어 있을 확률이다. 여기서는 2가지 모델을 예를 보여준다.

1. Sonar Sensor Model

첫번째 sensor는 sonar 센서이다. sonar센서는 상대적으로 거리 측정값에 대한 noise가 크다. 아래 그림에서 $z$는 센서의 측정값이다. $z-d_1$보다 가까운 구간은 free space로 생각하며, $z$ 부터 $z+d_3$구간은 object가 있는 영역으로 생각한다. $z+d_3$보다 먼 영역은 알수 없는 영역이므로 확률은 0.5이다. sonar센서는 측정값의 noise가 크기 때문에 $d$가 상대적으로 크다.

아래 그림은 sonar센서를 이용하여 생성한 grid map이다.

2. Laser Range Finder(LiDAR sensor)

두번째 센서는 Lidar sensor이다. Lidar는 sonar에 비해 거리측정 오차가 매우 적다. 따라서 occupied로 생각하는 영역이 sonar모델에 비해 매우 좁다.

아래 그림은 LiDAR센서를 이용하여 생성한 Grid map이다. Sonar를 이용한 map에 비해 상대적으로 매우 정확한 것을 알 수 있다.

Occupancy Grid Map 정리

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


[SLAM] Sparse Extended Information Filter(SEIF) SLAM 계산

on SLAM

Filter의 연산량 개선을 위한 Sparse Extended Information Filter(SEIF)를 이용한 SLAM의 계산과정을 설명한다.

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

이번 글에서는 이전 글에서 설명한 Sparse Extended Information Filter(SEIF) SLAM의 과정을 차근차근 한단계씩 이해해 보려고 한다. SEIF에 대해서 설명한 이전 글에서 언급했듯이 SEIF는 적은 갯수의 큰 값을 갖는 information matrix의 sparsification과정을 통해 filter의 일정한 계산속도(constant computation time)를 보장하는 방법이다. SEIF의 과정은 전체적으로 개념은 EIF와 유사하나, 각 단계마다 계산량을 줄이기위한, 즉 일정한 계산속도를 보장하기 위한 테크닉이 들어있다. 이는 large scale의 환경에서 SLAM을 구현하기 위한 꼭 필요한 테크닉이므로 정확히 이해하고 넘어가는 것이 좋다.

SEIF는 크게 4개의 과정으로 나뉘어 진다. \[\begin{aligned} & \text{SEIF SLAM}(\xi_{t-1}, \Omega_{t-1}, \mu_{t-1}, u_t, z_t)\\ 1: \ \ & \bar{\xi}_t, \bar{\Omega}_t, \bar{\mu}_t = \text{SEIF motion update}(\xi_{t-1}, \Omega_{t-1},\mu_{t-1}, u_t)\\ 2: \ \ & \xi_t, \Omega_t = \text{SEIF measurement update}(\bar{\xi}_t, \bar{\Omega}_t, \bar{\mu}_t, z_t)\\ 3: \ \ & \mu_t = \text{SEIF updated state estimate}(\xi_t, \Omega_t, \bar{\mu}_t)\\ 4: \ \ & \tilde{\xi}_t, \tilde{\Omega}_t = \text{SEIF sparsification}(\xi_t, \Omega_t, \mu_t)\\ 5: \ \ & \text{return} \ \ \tilde{\xi}_t, \tilde{\Omega}_t, \mu_t \end{aligned}\]

앞으로 각 단계를 하나씩 유도해 볼 것이며, 각 단계에서 constant한 computation time을 위해 어떤 테크닉을 사용하였는지 설명하도록 하겠다.

SEIF Motion Update

Matrix Inversion Lemma

sparse한 matrix의 inverse를 빠르게 계산하기 위해서 matrix inversion lemma를 이용한다. 이 lemma는 많은 부분에서 사용되니 기억해 두면 좋다. \[(R+PQP^T)^{-1} = R^{-1} - R^{-1}P(Q^{-1}+P^T R^{-1}P)^{-1}P^TR^{-1}\]

위 식에서 R은 크기가 큰 matrix, Q는 크기가 작은 matrix이며 P가 low dimension matrix를 high dimension으로 변형하는 직사각 행렬이다. 이때 만약 큰 matrix인 R의 inverse를 간단하게 계산할 수 있으면 전체 matrix의 inverse도 쉽게 구할 수 있다.

Motion Update step(prediction step)

우선 EKF SLAM에서 prediction 단계를 다시 보도록 하자. velocity-based model의 경우 위와같이 정의할 수 있다.

편의를 위하여 vector와 matrix를 $\delta, \triangle$로 표현한다.

Prediction step에서의 information matrix를 계산하기 위해 EKF의 5번 식에서 부터 계산을 시작한다. \[\begin{aligned} \bar{\Omega}_t &= \bar{\Sigma}_t^{-1}\\ &= [G_t\Omega_{t-1}^{-1}G_t^T + R_t]^{-1}\\ &= [\Phi_t^{-1} + R_t]^{-1} \end{aligned}\]

우리가 최종적으로 계산하고자 하는 information matrix는 위와 같이 계산할 수 있다. prediction step에서 가장 큰 계산량을 차지하는 부분은 $\Omega_{t-1}^{-1}$을 계산하여 $\bar{\Omega}_t$를 계산하는 과정이다. 여기서 $\Phi_t$는 다음같이 정의된다. \[\begin{aligned} \Phi_t &= [G_t \Omega_{t-1}^{-1} G_t^T]^{-1}\\ &= [G_t^T]^{-1} \Omega_{t-1} G_t^{-1} \end{aligned}\]

이제 위에서 설명한 matrix inversion lemma를 이용하여 전개하면 다음과 같다. \[\begin{aligned} \bar{\Omega}_t &= [\Phi_t^{-1} + R_t]^{-1}\\ &= [\Phi_t^{-1} + F_x^TR_t^x F_x]^{-1}\\ &= \Phi_t - \Phi_t F_x^T({R_t^{x}}^{-1}+F_x \Phi_t F_x^T)^{-1} F_x \Phi_t\\ &= \Phi_t - \kappa_t \end{aligned}\]

이제 우리가 prediction step에서 구하고자 하는 information matrix를 계산하는 식을 자세히 들여다 보도록 하자. $F_x$는 $3\times3$ block을 제외하고는 전부 0인 matrix이다. 그리고 inverse 계산을 해야하는 ${R_t^{x}}^{-1}+F_x \Phi_t F_x^T$는 $3\times3$ matrix이므로 연산량이 많지 않다. 따라서 만약

$\Phi_t$의 computation time이 constant하다면 전체 process의 computation time도 constant해 진다.

그러면 이제 $\Phi_t$의 계산과정을 들여다 보도록 하자. \[\begin{aligned} \Phi_t &= [G_t \Omega_{t-1}^{-1} G_t^T]^{-1}\\ &= [G_t^T]^{-1} \Omega_{t-1} G_t^{-1} \end{aligned}\]

$\Phi_t$를 계산하는 과정에서 가장 계산량이 많은 부분은 $G_t^{-1}$을 계산하는 부분이다. 이 부분을 자세히 살펴보도록 하자. \[\begin{aligned} G_t^{-1} &= (I+F_x^T \triangle F_x)^{-1}\\ &= \begin{pmatrix} \triangle + I_3 & 0 \\ 0 & I_{2N} \end{pmatrix}^{-1}\\ &= \begin{pmatrix} (\triangle + I_3)^{-1} & 0 \\ 0 & I_{2N} \end{pmatrix} \end{aligned}\]

즉 위 계산결과를 보면 $G_t^{-1}$의 계산속도에 영향을 주는 부분은 $(\triangle + I_3)^{-1}$이다. 이 부분은 전체 matrix의 크기에 상관없이 항상 $3\times3$ matrix이기 때문에 $G_t^{-1}$의 계산속도는 일정하다는 것을 알 수 있다. 위 식을 조금 더 정리하면 다음과 같다. \[\begin{aligned} G_t^{-1} &= (I+F_x^T \triangle F_x)^{-1}\\ &= \begin{pmatrix} \triangle + I_3 & 0 \\ 0 & I_{2N} \end{pmatrix}^{-1}\\ &= \begin{pmatrix} (\triangle + I_3)^{-1} & 0 \\ 0 & I_{2N} \end{pmatrix}\\ &= I_{3+2N} + \begin{pmatrix} (\triangle + I_3)^{-1} - I_3 & 0 \\ 0 & 0 \end{pmatrix}\\ &= I+ F_x^T[(I+\triangle)^{-1} - I]F_x\\ &= I + \Psi_t \end{aligned}\]

위의 정의를 이용하여 $\Phi_t$를 정리해 보면 \[\begin{aligned} \Phi_t &= [G_t^T]^{-1} \Omega_{t-1} G_t^{-1}\\ &= (I+\Psi_t^T) \Omega_{t-1} (I+\Psi_t)\\ &= \Omega_{t-1} +\Psi_t^T \Omega_{t-1} +\Omega_{t-1}\Psi_t + \Psi_t^T \Omega_{t-1} \Psi_t\\ &= \Omega_{t-1} + \lambda_t \end{aligned}\]

여기서 $\lambda_t$는 일정개수의 요소들만 non-zero의 값을 갖는 matrix이다.

따라서 $G_t^{-1}$의 계산속도가 constant하고 information matrix가 sparse하므로 $\Phi_t$의 계산속도는 constant하며, $\Phi_t$의 계산속도가 constant하므로 최종적으로 prediction step의 information matrix 계산량도 constant하다.

위에서 설명한 대로 일정한 계산속도를 유지하며 information matrix를 계산하는 과정을 정리하면 다음과 같다.

1. $\Psi_t$ 계산

2. $\Psi_t$를 이용하여 $\lambda_t$ 계산

3. $\lambda_t$를 이용하여 $\Phi_t$ 계산

4. $\Phi_t$를 이용하여 $\kappa_t$ 계산

5. 마지막으로 $\kappa_t$와 $\Phi_t$를 이용하여 $\bar{\Omega}_t$ 계산

위의 계산 방법을 통해 matrix크기에 상관없이 constant한 계산속도로 information matrix를 계산할 수 있다.

이제 information matrix를 구했으니 information vector를 구해야 한다. information vector는 아래의 유도과정을 통해 계산할 수 있다.

Information vector를 계산하기 위해 사용되는 값들의 대부분이 Information matrix를 구할 때 계산된 값들이며, inverse의 계산이 없기 때문에 계산량이 크지 않다.

따라서 SEIF의 prediction step에서 information matrix가 sparse 할 때, constant computation time으로 information vector와 information matrix를 계산할 수 있음을 증명하였다.

SEIF Measurement update(correction step)

SEIF의 correction step은 EIF와 마찬가지로 계산량이 크지않다. SEIF의 measurement update과정은 다음과 같다.

SEIF의 measurement update과정은 EKF와 대부분 동일하다. SEIF에서 바뀐 부분은 information vector와 matrix를 계산하는 12, 13번 과정이다. 큰 matrix의 inverse와 같은 많은 계산량을 요구하는 부분이 없기 때문에 constant computation time을 보장한다($Q_t^{-1}$는 observation의 noise로 matrix의 크기가 작으며 미리 계산할 수 있는 matrix이다). 위의 계산과정이 이해가 되지 않는다면 이전 글인 EKF SLAM을 참고하기 바란다.

SEIF Updated State Estimate

SEIF의 세번째 단계는 state의 평균값(mean, $\mu_{t}$)을 계산하는 과정이다. 계산된 평균값은 다음의 과정에서 사용된다.

이전 두 단계에서 information vector와 matrix를 계산하였기 때문에 mean값을 쉽게 계산할 수 있다. \[\mu = \Omega^{-1} \xi\]

하지만 이와 같이 단순한 정의에 의해서 계산하게 되면 한가지 문제가 발생한다. $\Omega$는 매우 큰 matrix이므로 이 matrix의 inverse과정은 매우 많은 계산량을 요구한다.

따라서 mean값을 계산하기 위해 optimization 방법으로 접근한다. \[\begin{aligned} \hat{\mu} &= \text{argmax} \ \ p(\mu)\\ &= \text{argmax}_{\mu} \ \ exp(-\frac{1}{2} \mu^T\Omega\mu + \xi^T\mu) \end{aligned}\]

SEIF의 이전 두 단계에서 information matrix와 vector를 계산하였기 때문에 이를 이용하여 Gaussian 분포를 표현할 수 있다. 이 Gaussian의 분포에서 mean값은 함수의 최대값을 갖는 위치 이므로 위와 같이 optimization 방법으로 mean값을 계산하는데, 함수를 미분하여 미분값이 0이 되는 $\mu$를 구하면 된다.

Optimization 방법으로 mean값을 구하는 과정에서, 로봇의 위치와 active landmark의 mean값만을 구함으로써 더 효율적으로 계산 가능하고, constant computation을 보장할 수 있다.

SEIF Sparsification

마지막은 Information matrix의 sparsification 과정이다. sparsification은 matrix의 non-zero off-diagonal의 수가 일정하게 유지되도록, 즉 sparse matrix가 유지되도록 만드는 과정이다. SEIF의 이전 step에서 constant computation을 보장하기 위한 조건이 information matrix의 sparseness이었으므로 이 과정이 꼭 필요하다.

Sparsification은 SEIF 에서 언급했던것 처럼 로봇과 landmark간의 link을 제거함으로써(conditional independence하다고 가정함으로써) non-zero off-diagonal의 수를 유지하는 것이다.

landmark는 2가지 종류로 분류할 수 있다. 현재 관측하거나 관측하고 있다고 생각하는 landmark(즉, direct link가 있는)는 active landmark이며, direct link가 없는 landmark를 passive landmark라고 한다.

sparsification 과정을 위해서 landmark를 다시 3가지로 분류한다. $m^{+}$는 현재 active landmark이며

계속 active landmark이다. $m^0$는 현재는 active landmark이지만 passive landmark로 만들 대상이다. $m^-$는 현재도 passive landmark이며 계속 passive landmark이다. \[\begin{aligned} p(x_t,m \mid z^t, u^t) &= p(x_t, m^+, m^0, m^- \mid z^t, u^t)\\ &= p(x_t \mid m^+, m^0, m^-, z^t, u^t)p(m^+, m^0, m^- \mid z^t, u^t)\\ &= p(x_t \mid m^+, m^0, m^- = 0, z^t, u^t)p(m^+, m^0, m^- \mid z^t, u^t) \end{aligned}\]

위 식에서 마지막 단계에서 우리가 만약에 $m^+$과 $m^0$에 대해서 알고있다면 $x_t$는 $m^-$에 independent하다는 사실을 이용하였다. 따라서 $m^-$의 값을 임의의 값으로 설정할 수 있으며, 여기서는 단순하게 0으로 설정하였다. 이제 위 식으로 부터 $m^0$를 제거함으로써 근사화 시킬 수 있다. passive landmark가 되는 $x^0$는 이제 로봇의 위치인 $x_t$와 independent하기 때문에 제거 가능하다. \[\begin{aligned} \tilde{p}(x_t,m \mid z^t, u^t) &= p(x_t \mid m^+, m^- = 0, z^t, u^t)p(m^0, m^+, m^- \mid z^t, u^t) \\ &= \frac{p(x_t, m^+ \mid m^- = 0, z^t, u^t)}{p(m^+ \mid m^- = 0, z^t, u^t)}p(m^0, m^+, m^- \mid z^t, u^t) \end{aligned}\]

$\tilde{}$는 sparsification을 통해 근사화를 했음을 의미한다. 이제 $\tilde{p}(x_t,m \mid z^t, u^t)$의 information matrix를 계산하는 과정이 sparsification 과정을 의미한다. \[\begin{aligned} \Omega_t &= \text{information matrix of } p(x_t, m^+, m^0, m^- \mid z^t, u^t) \\ \Omega_t' &= \text{information matrix of } p(x_t, m^+, m^0, \mid m^- = 0, z^t, u^t) \end{aligned}\]

맨 먼저 $\Omega_t’$를 계산하는 것으로 시작한다. bayes rule을 이용하여 전개하면 다음과 같다. \[p(x_t, m^+, m^0, \mid m^- = 0, z^t, u^t) = \frac{p(x_t, m^+, m^0, m^- \mid z^t, u^t)}{p(m^- = 0 \mid z^t, u^t)}\]

따라서 $\Omega_t’$는 $p(x_t, m^+, m^0, m^- \mid z^t, u^t)$의 information matrix에서 $m^-$에 해당하는 information 항을 0으로 만든 information matrix이다(information form을 이용한 Gaussian 분포 식을 생각하자). 이 matrix를 projection matrix $S$를 이용하여 표현하면 다음과 같다. \[\Omega_t' = S_{x,m^+,m^0}S_{x,m^+,m^0}^T \Omega_t S_{x,m^+,m^0} S_{x,m^+,m^0}^T\]

여기서 Projection marix는 남기고자 하는 부분이 identity로 구성된 matrix이다. 예를들어 로봇의 포즈와 첫번째 landmark만 남기고 모두 0으로 만드는 projection matrix는 다음과 같다. \[S_{x,m1} = \begin{bmatrix} I_3 & 0 & 0 & \cdots & 0\\0 & I_2 & 0 & \cdots & 0 \end{bmatrix}^T\]

따라서 $\Omega_t’$을 계산함으로써 $m^-$에 해당하는 row와 column은 0이 된다. 이 부분이 잘 이해가 되지 않는다면 matlab을 이용해서 계산해보면 쉽게 이해할 수 있다.

이제 $\Omega_t$와 $\Omega_t’$을 이용하여 \[\begin{aligned} \tilde{p}(x_t,m \mid z^t, u^t) &= \frac{p(x_t, m^+ \mid m^- = 0, z^t, u^t)}{p(m^+ \mid m^- = 0, z^t, u^t)}p(m^0, m^+, m^- \mid z^t, u^t) \end{aligned}\]

을 구성하는 각 항의 information matrix를 구해본다. \[\begin{aligned} \Omega_t^1 &= \text{information matrix of } p(x_t, m^+ \mid m^- = 0, z^t, u^t) \\ \Omega_t^2 &= \text{information matrix of } p(m^+ \mid m^- = 0, z^t, u^t) \\ \Omega_t^3 &= \text{information matrix of } p(m^0, m^+, m^- \mid z^t, u^t) \end{aligned}\]

각 information matrix는 information form의 marginalization을 이용하여 계산한다. information form의 marginalization은 EIF 글에 정리되어 있다. 이를 통해 각각의 information matrix를 계산하면 다음과 같다. \[\begin{aligned} \Omega_t^1 & = \Omega_t' - \Omega_t' S_{m^0}(S_{m^0}^T \Omega_t' S_{m^0})^{-1}S_{m^0}^T \Omega_t'\\ \Omega_t^2 & = \Omega_t' - \Omega_t' S_{x_t, m^0}(S_{x_t, m^0}^T \Omega_t' S_{x_t, m^0})^{-1}S_{x_t, m^0}^T \Omega_t'\\ \Omega_t^3 & = \Omega_t - \Omega_t S_{x_t}(S_{x_t}^T \Omega_t S_{x_t})^{-1}S_{x_t}^T \Omega_t\\ \end{aligned}\]

이제 계산된 information matrix를 이용하여 sparsification 된 information matrix를 계산할 수 있다. \[\begin{aligned} \tilde{\Omega}_t &= \Omega_t^1 - \Omega_t^2 + \Omega_t^3\\ &=\Omega_t - \Omega_t' S_{m^0}(S_{m^0}^T \Omega_t' S_{m^0})^{-1}S_{m^0}^T \Omega_t' \\ & \ \ + \Omega_t' S_{x_t, m^0}(S_{x_t, m^0}^T \Omega_t' S_{x_t, m^0})^{-1}S_{x_t, m^0}^T \Omega_t' \\ & \ \ - \Omega_t S_{x_t}(S_{x_t}^T \Omega_t S_{x_t})^{-1}S_{x_t}^T \Omega_t \end{aligned}\]

이제 sparsification을 통해 근사화된 information matrix가 계산되었다. 이제 information vector를 계산해야 되는데 다음과 같이 간단하게 계산할 수 있다. \[\begin{aligned} \tilde{\xi}_t &= \mu_t^T \tilde{\Omega}_t\\ &= \mu_t^T(\Omega_t - \Omega_t + \tilde{\Omega}_t)\\ &= \mu_t^T\Omega_t + \mu_t^T(\tilde{\Omega}_t - \Omega_t )\\ &= \xi_t + \mu_t^T(\tilde{\Omega}_t - \Omega_t ) \end{aligned}\]

SEIF SLAM Vs EKF SLAM

landmark의 갯수가 증가(matrix 크기 증가)함에 따른 계산시간의 변화를 보여준다. SEIF는 크게 변하지 않지만 EKF는 계산시간이 크게 증가한다.

landmark의 갯수 증가에 따른 메모리 크기변화를 보여준다. SEIF는 선형적으로 증가하지만 EKF는 quadratic하게 증가한다.

landmark의 갯수 증가에 따른 error를 보여준다. 근사화를 하였기 때문에 SEIF가 EKF에 비해 error는 높다.

Active landmark의 갯수에 따른 update 시간을 비교하였다. Active landmark의 갯수가 적을수록 계산시간은 빨라진다.

Active landmark의 갯수에 따른 error의 변화를 보여준다. Active landmark의 갯수가 적을수록 error는 증가한다.

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


[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를 비교해보자.

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


Pagination