[SLAM] Unscented Kalman Filter(UKF)

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들을 계산한다.

• 각 sigma point들의 weight를 계산한다.

• 비선형 함수를 통해 sigma point들의 출력을 계산한다.

• 출력된 point들의 새로운 Gaussian 분포를 계산한다.

• 이 과정은 EKF가 사용하는 mean값 근처에서의 선형화를 피하기 위한 방법이다.

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에 비해 다소 단계가 많아진 것 같지만 하나씩 보면 많은 부분이 유사함을 알 수 있다.

• 4번: sigma point들을 이용하여 prediction step에서 계산한 mean과 covariance값을 이용하여 새로운 sigma point들을 생성한다.

• 5번: 비선형 observation 모델의 출력(예상되는 sensor observation)을 계산한다.

• 6번: Sigma point들의 출력과 weight를 이용하여 평균(mean)을 계산한다.

• 7번: Sigma point들의 출력과 평균값, 그리고 weight를 이용하여 covariance matrix를 계산한다. 여기서 $Q_t$는 observation noise이다.

• 8번: prediction과 obseration의 cross covariance를 계산한다. 이 matrix는 Kalman gain을 계산할 때 사용된다.

• 9번: Kalman gain을 계산한다.

원래의 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$가 된다. 조금 햇갈릴 수도 있으니 서로의 관계를 잘 생각해보자.

• 10번: Kalman gain을 이용하여 mean을 수정한다.

• 11번: Kalman gain을 이용하여 covariance를 수정한다. 원래 EKF의 식에서 유도되는 과정은 다음과 같다.

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

UKF Vs EKF

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

• 선형 모델에서는 EKF와 UKF의 결과는 같다.

• 비선형 모델에서는 UKF가 EKF보다 더 나은 근사화 방법을 사용한다.

• 하지만 결과의 차이는 많은 경우에 그다지 크지 않다.

• UKF는 Jacobian matrix를 구할 필요가 없다.

• 계산 복잡도는 비슷하며, UKF가 EKF보다 계산속도는 약간 더 느리다.

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