무향 칼만 필터 정리, Unscented Kalman Filter - 작성중

일반적인 이산-시간 비선형 모델의 상태 공간에서의 표현은 다음과 같이 쓸 수 있다.

$$\array{ x_{k+1} &=& f \left(x_{k}, u_{k}, w_k \right) \\ y_{k} &=& h \left( x_{k} , u_{k} , v_k \right)} $$

시스템 모델은 state vector $x_{k} \in \mathbb{R}^n$, input $u_{k}$, process noise vector $v_k \sim \mathcal{N}\left( 0 , Q_k \right) $ 으로부터 시스템 모델 함수 $f (\cdot)$를 통해 다음 상태 $x_{k+1}$으로 전파된다.

출력 모델은 state $x_{k}$, input $u_{k}$, measurement noise vector $w_{k} \in \mathcal{N}\left( 0, R_k \right)$으로부터 출력 모델 함수 $h(\cdot)$을 통해 출력 $y_{k}$이 나온다.

 

Linear Kalman Filter

상태 벡터 $x$를 추정하기 위해 선형 칼만 필터는 다음의 예측과 추정 과정을 거친다.

 

Prediction

$$\array{ \hat{x}_k^- &=& F_{k-1} \hat{x}_{k-1} + G_{k-1} u_{k-1}& F_{k} = \left. \frac{\partial f}{\partial x} \right|_{\hat{x}_{k}} & G_k = \left. \frac{\partial f}{\partial u} \right|_{u_k} \\ P_k^- &=& F_{k-1} P_{k-1} F_{k-1}^T + Q_{k-1} & H_k = \left. \frac{\partial h}{\partial x} \right|_{\hat{x}_{k}^- } &  } $$

 

위 과정을 통해서 미리 알고 있는 시스템 모델을 통해 Priori state vector $\hat{x}_k^-$와 Priori error covariance matrix $P_k^-$ 을 얻는다. 모델의 상태를 예측하고 그 예측한 상태가 실제 모델과 얼마나 차이가 나는지 나타내는 공분산 행렬을 계산하는 것이다.

 

Measurement Update (Estimation)

$$\array{ K_k &=& P_k^- H_k^T \left( H_k P_k^- H_k^T + R_k \right)^{-1} \\ \hat{x}_k &=& \hat{x}_k^- + K_k \left( y_k - H_k \hat{x}_k^- \right) \\ P_k &=& \left( I-K_k H_k \right) P_k^- }$$

 

계측 값 $y_k$가 필터에 입력되면, 이를 이용하여 필터가 추정하는 상태를 출력한다. 위 과정을 통해 Posteriori state vector $\hat{x}_k$와 Posteriori error covariance matrix $P_{k}$를 얻는다. 이는 Prediction 과정에서 예측한 상태 벡터로 부터 모델 출력 $y_k^- = H_k \hat{x}_k^-$를 계산하고 계측 값 $y_k$과의 오차를 계산했을 때, 칼만 이득은 얼마나 이를 추정과정에 반영할 지를 결정한다.

 

Extended Kalman Filter

비선형 칼만 필터는 예측 과정에서 비선형 시스템 모델을 사용함으로써 시스템의 상태를 예측한다. 그러나 모델 선형화를 해야하는 단점이 존재한다. 

 

Prediction

$$\array{ \hat{x}_k^- &=& f \left( \hat{x}_{k-1},  u_{k-1}, 0 \right)& & \\ P_k^- &=& F_{k-1} P_{k-1} F_{k-1}^T + L_{k-1}Q_{k-1} L_{k-1}^{T} & F_{k} = \left. \frac{\partial f}{\partial x} \right|_{\hat{x}_{k}} & L_k = \left. \frac{\partial f}{\partial w_k} \right|_{\hat{x}_k } } $$

 

Measurement Update (Estimation)

$$\array{ K_k &=& P_k^- H_k^T \left( H_k P_k^- H_k^T + M_k R_k M_k^T \right)^{-1} & M_k = \left. \frac{\partial f}{\partial v} \right|_{\hat{x}_k^-} & H_k = \left. \frac{\partial h}{\partial x} \right|_{\hat{x}_{k}^- } \\ \hat{x}_k &=& \hat{x}_k^- + K_k \left[ y_k - h \left( \hat{x}_k^-, u_k , 0\right)  \right] & & \\ P_k &=& \left( I-K_k H_k \right) P_k^- & & }$$

 

비선형 칼만 필터는 

 

Unscented Transformation

무향 변환은 Unscented Kalman Filter, 무향 칼만 필터의 핵심적인 개념으로써 상태 변수들의 통계를 sigma point set 으로 변환하여 나타내는 것이 특징이다.

 

무향 변환의 특징을 요약하면,

• 가우시안 무작위 변수인 $x$와 $y$를 가우시안 분포로 해석한다.
• 상태 변수의 평균과 공분산 정보를 담은 sigma point으로 나타내어 사용한다.
• 비선형 사상(mapping)을 그대로 사용하기 때문에 기존의 선형화 과정이 필요없다.

간단히 방법을 요약하면

 상태 $x$에 대해 평균 $\bar{x}$, 공분산 행렬 $P_x$의 1 $\sigma$에 해당하는 sigma point set $\chi$을 생성하고, 비선형 시스템 함수를 통해 비선형 변환을 거친 후의 sigma point set $\Upsilon = f(\chi)$에 대해서 평균 $\bar{y}$와 공분산 행렬 $P_y$을 구한다.

 

Sigma point set $\chi$의 생성

열 벡터 $x\in \mathbb{R}^{L \times 1}$ 일 때, scaling paramter $\lambda$, 상태 $x$에 대한 prior mean $\bar{x}$, prior covariance $P_x$으로부터 $2L+1$개의 sigma point를 다음과 같이 생성한다.

$$\chi = \left[ \matrix{\bar{x} & \bar{x} + \sqrt{ L+\lambda } \sqrt{ P_x } & \bar{x} - \sqrt{ L+\lambda } \sqrt{ P_x } } \right] $$

$\chi \in \mathcal{R}^{L\times \left( 2L +1\right) }$이며, 각 열은 sigma point를 나타낸다.

위 식은 행렬의 제곱근을 포함하는데, 이를 계산하는 다양한 알고리즘이 있다. 성능과 연산 효율성을 고려했을 때, 추천하는 알고리즘은 Cholesky method 이며, 본 식은 다음과 같이 표현되는 lower Cholesky decomposition을 수행한다. 

$$P_x = \left(\sqrt{P_x} \right) \left( \sqrt{P_x} \right)^T$$

 

사족을 달면, 한 차원당 평균과 1 $\sigma$ 위치의 값 2개 표본을 뽑는 것이다.

 

The scaling of the Unscented Transformation

무향 변환 과정에서 상태 $x$에 대한 확률 분포를 나타낸 Sigma point들을 생성한다는 것을 알았다. 그러면 생성한 sigma point는 다시 정규 분포로 표현할 때, 상태 $x$에 대한 prior mean $\bar{x}$, prior covariance $P_x$를 가지도록 계산될 수 있어야 한다. 따라서 각 sigma point들에 mean weight vector $\eta^m$, covariance weight vector $\eta^c$를 적용하여 계산한다.

Weight vector는 다음과 같이 계산한다.

$$\array{ \lambda &=& \alpha^2 \left( L+\kappa \right) -L     \\    \eta_0^m &=& \lambda /\left( L + \lambda \right)    \\    \eta_0^c &=& \lambda / \left( L+\lambda \right) +1 - \alpha^2 + \beta      \\     \eta_i^m &=& {\eta_i^c} = 1 / \left[2 \left( L+\lambda \right) \right] } $$

$\alpha$는 $\bar{x}$ 근방의 시그마포인트의 확산을 결정하고, 일반적으로는 작은 양의 값으로 한다.

$\kappa$는 두 번째 스케일 변수이며, 일반적으로 0으로 한다.

$\beta$는 $x$ 분포에 대해 선험 지식 (prior knowledge)이며, 가우시안 분포라면 2가 최적 값이다.

$L$은 상태 $x$의 차원 크기라고 보면 된다.

Visualization of each processes, Actual Sampling(Left), Linearized/EKF(Center), UT/UKF(Right)

정리를 하면 선형/비선형 칼만 필터는

Gaussian Random Variable들인 $x$와 $y$에 대해서, $x$에 대한 확률 분포를 평균 $\hat{x}_k^-$과 공분산 행렬 $P_x^-$ 으로 나타내고 이를 통해서 $y$에 대한 확률 분포 또한 평균 $\hat{x}_k$ 과 공분산 행렬 $P_x$ 으로 나타낸다.

그러나 시스템 모델 함수 $f(\cdot)$이 불연속성을 가져 미분 불가능하거나 급격한 비선형성을 가질 경우, 선형화 가정이 잘 들어맞지 않게 된다.

무향 칼만 필터는 

Gaussian Random Variable들인 $x$와 $y$에 대해서, $x$에 대한 확률 분포를 평균 $\hat{x}_k^-$과 공분산 행렬 $P_x^-$ 정보를 담고 있는 Sigma Points를 생성하고, 이를 사상(Mapping)한 후에 다시 사상된 변수 $y$에 대해서 Sigma point 들의 확률 분포를 계산한다. 이를 통해서 사상된 Sigma points $\Upsilon$이 가지는 확률 분포 정보인 UT 평균 $\bar{\Upsilon}$과 UT 공분산 행렬 $P_x^-$을 얻는다.

 

Unscented Kalman Filter

 

Prediction

Generate sigma points

$$\chi_{k-1}^a = \left[ \array{ \hat{x}_{k-1}^a & \hat{x}_{k-1}^a \pm \sqrt{\left(L+\lambda \right) P_{k-1}^a } } \right] $$

$$\array{ \hat{\chi}_{k}^- &=& f \left[ \chi_{k-1}^x, \chi_{k-1}^v \right]       \\      \hat{x}_k^- &=& \sum_{i=0}^{2L} \mu_i^m \chi_{i}^{x-}     \\      P_k^- &=& \sum_{i=0}^{2L} \mu_i^c \left[ \chi_i^{x-} - \hat{x}_k^- \right] \left[ \chi_i^{x-} - \hat{x}_k^- \right]^T         \\       \Upsilon^- &=& h\left( \chi_k^{x-} , \chi_{k-1}^n  \right)       \\     \hat{y}_k^- &=& \sum_{i=0}^{2L}\mu_i^m \Upsilon_i^- } $$

 

 

Measurement Update (Estimation)

$$ \array{ P_{k,yy} &=& \sum_{i=0}^{2L} \mu_i^c \left[ \Upsilon_i^- - \hat{y}_k^- \right]  \left[ \Upsilon_i^- - \hat{y}_k^- \right]^T       \\      P_{k,xy} &=& \sum_{i=0}^{2L} \mu_i^c \left[ \chi_i^{x-} - \hat{x}_k^- \right] \left[ \Upsilon_i^- - \hat{y}_k^- \right]^T      \\       K_k &=& P_{k,xy} P_{k,yy}^{-1}      \\      \hat{x}_k &=& \hat{x}_k^- + K_k \left( y_k - \hat{y}_k^- \right)     \\  P_k &=& P_k^-  - K_k P_{k,yy}, K_k^T } $$

 

계산하기 위해서 Sigma Points를 생성하고 이를 통해서 Priori estimate $\hat{x}_k^-$와 Priori error covariance matrix $\hat{P}_k^-$을 계산한다.

왜 Sigma Points라는 이름을 붙였을까를 생각해보면 확률과 분포에서 주로 정규 분포(Gausian Distribution)에 대해서 다루는데, 어떤 변수 $x$가 평균 $\mu$, 표준 편차 $\sigma$인 정규 분포를 따른다면 다음과 같이 표기한다.

$$x \sim \mathcal{N} \left( \mu, \sigma \right)$$

그리고 무향 변환(Unscented Transformation) 과정에서 1 $

 

 

Reference

1. Julier, S., J. and Uhlmann, J., K. (1997) New extenstion of the Kalman filter to nonlinear systems, Signal processing, Sensor Fusion, and Target Recognition VI, Available at www.cs.unc.edu/~welch/kalman/media/pdf/Julier1997_SPIE_KF.pdf

2. Julier, S., J. and Uhlmann, J., K., (2004) Unscented Filtering and Nonlinear Estimation, Proceedings of the IEEE, Vol.92, No.3, pp.401-422. Available at www.cs.ubc.ca/~murphyk/Papers/Julier_Uhlmann_mar04.pdf

3. Wan, E., A., and Merwe, R., Van Der (2000) The Unscented Kalman filter for nonlinear estimation, Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, pp.153-158. Available at www.seas.harvard.edu/courses/cs281/papers/unscented.pdf