[SLAM] Extended Information Filter(EIF) 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 정리

위에서 유도했던 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와 비교하여 정리하면 다음과 같다.

• EIF는 EKF의 information 형태로 변형한 것이다.

• EKF는 correction step의 계산량이 많고, EIF는 prediction step의 계산량이 많다.

• EIF와 EKF의 전체 계산량은 비슷하다.

• Information form에서도 Unscented transform이 사용될 수 있다.

• EIF가 EKF에 비해서 수학적으로 조금 더 안정하다고 알려져 있다.

• 실제로는 EIF보다 EKF가 더 많이 사용된다.

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

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