두 가우시안의 곱

두 가우시안의 곱 $N(\mu_1,\sigma_1^2) \times N(\mu_2, \sigma_2^2) =N(\frac{\sigma_2^2*\mu_1+\sigma_1^2*\mu_2}{\sigma_1^2+\sigma_2^2}, \frac{1}{\frac{1}{\sigma_1^2}+\frac{1}{\sigma_2^2}})$ 을 만족하고,
새로운 가우시안의 variance는 기존의 두 가우시안보다 작다. 이 결과는 칼만필터의 measurement update 과정과 연관이 있다.
칼만필터는 Gaussian 가정을 통해 state estimation과 uncertainty를 함께 고려한다.

칼만필터

용어 정의

$\hat{x}$ : state estimation vector
$P$ : estimation error covariance matrix

$F$ : state transition matrix
$Q$ : process noise covariance matrix

$z$ : measurement vector
$H$ : measurement matrix
$R$ : measurement noise covariance matrix
$S$ : measurement covariance matrix
칼만필터는 Prediction과 Measurement Update를 반복하면서 $\hat{x}$과 $P$를 갱신하면서 동작한다.
이 글에서 prime(`)는 measurement update 이후의 값을 의미한다.

칼만필터 알고리즘

Prediction

$\hat{x}_{k+1}=F\hat{x}_{k}$
$P_{k+1}=FP_kF^T+Q$

Update

$y_k = z_k - H\hat{x}_k$
$S_k = HP_kH^T+R$
$K_k=P_kH^TS_k^{-1}$
$\hat{x}^\prime_k=\hat{x}_k+K_ky_k$
$P_k^\prime=(I-K_kH)P_k$

---

KF 유도

추정값이 아닌 실제 값 $x_{k}$의 경우 아래 식을 만족한다고 가정하자.
$x_{k+1} = Fx_{k}+w_{k}$
$z_{k}=Hx_{k}+v_{k}$

$w_{k}$는 process noise vector이다. state transition matrix $F$의 한계에 의해 발생한다. 예를 들어 state vector가 object의 위치와 속도만을 고려하는 경우 가속운동하는 object의 state를 정확히 계산할 수 없다. 이 경우 가속에 의한 오차가 $w_k$에 포함될 것이다.

$v_{k}$는 sensor noise를 의미하고, $e_{k}$는 estimation error를 의미한다.
$w_k, v_k, e_k$ 모두 $\mu=0$인 gaussian 분포를 따른다고 가정한다.

여기서 P, Q, R은 각각 e, w, v의 공분산행렬이다.
$P_k = E[e_ke^T_k]=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T]$
$P_k^\prime = E[e_k^{\prime}e^{{\prime}{T}}_k]=E[(x_k-\hat{x}_k^\prime)(x_k-\hat{x}_k^\prime)^T]$
$Q=E[w_kw_k^T],R=E[v_kv_k^T]$

Prediction

$\hat{x}_{k+1}=F\hat{x}_k$
$e_{k+1}=x_{k+1}-\hat{x}_{k+1}=(Fx_k+w_k)-F\hat{x}_k=Fe_k+w_k$
위에서 구한 $e_{k+1}$을 $P_{k+1}$에 대입하면 아래와 같은 결과를 얻는다.
$P_{k+1}=E[(Fe_k+w_k)(Fe_k+w_k)^T]=FP_kF^T+Q$

Update

state를 update할 때 새로운 measure값과 기존의 state $\hat{x}_k$값의 차이인 residual을 이용한다. residual은 아래와 같다.
$y_k=z_k-H\hat{x}_k$
따라서 새로운 state $\hat{x}_k^\prime$은 기존의 state에 residual에 비례하는 값을 더해서 구해준다.

$\hat{x}_k^\prime=\hat{x}_k+K_ky_k=\hat{x}_k+K_k(z_k - H\hat{x}_k)=\hat{x}_k+K_k(Hx_k+v_k - H\hat{x}_k)$
이제 Minimum MSE을 만족하는 Kalman gain $K_k$를 구해보자.
위에서 구한 $\hat{x}_k^\prime-\hat{x}_k$값을 이용해 $P_k^\prime$를 정리하면 아래와 같다.
$P_k^\prime=E[e_k^{\prime}e^{{\prime}{T}}_k]=E[(x_k-\hat{x}_k^\prime)(x_k-\hat{x}_k^\prime)^T]=E[((I-K_kH)(x_k-\hat{x}_k)-K_kv_k)(I-K_kH)(x_k-\hat{x}_k)-K_kv_k)^T]$

$=(I-K_kH)P_k(I-K_kH)^T+K_kRK_k^T$

위 식을 전개한 뒤 trace를 취하면 아래와 같다.
$tr[P^{\prime}_k]=tr[P_k]-2tr[K_kHP_k]+tr[K_k(HP_kH^T+R)K_k^T]$

이제 $K_k$에 대해서 미분하고 결과를 zero로 setting(minimum mse 조건)하면 아래와 같이 kalman gain을 얻는다.
$\frac{dT[P_k^\prime]}{dK_k}=-2(HP_k)^T + 2K_k(HP_kH^T+R)=0$
$K_k=P_kH^T(HP_kH^T+R)^{-1}=P_kH^TS^{-1}$

위에서 구한 $P^\prime_k$식에 $K_k$를 대입해서 정리하면
$P_k^\prime=(I-K_kH)P_k$을 얻는다.

Discussion

sensor noise covariance $R\rarr\infin$이면 kalman gain $K_k\rarr0$이고
$R\rarr0$이면 $K_k\rarr\infin$이 되는 것을 알 수 있다. 즉 measurement의 noise가 클수록(안좋은 센서) 적게 update가 되고 noise가 작은 센서(정밀한 센서)일수록 update에 더 많이 반영된다.
다시 맨 처음에 언급한 두 가우시안의 곱을 다시 살펴보자.
결과값을 정리하면 아래와 같다.
$\mu^\prime=\mu_1+\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}\times(\mu_2-\mu_1)$
$\sigma^{\prime2}=(1-\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2})\times{\sigma_1^2}$
kalman filter의 measurement update부분과 유사성을 확인할 수 있다.

확장칼만필터 EKF

칼만필터에서는 state transition $x_{k+1}=f(x_k)+w_k$와 measurement function $z_k=h(x_k)+v_k$의 f와 h를 선형변환으로 가정하기 때문에 변환을 행렬 F, H로 나타내 사용할 수 있었다.
하지만 대체로 f, h는 비선형변환이기 때문에 행렬로 나타낼 수 없고, 가우시안을 비선형변환할 경우 더이상 가우시안이 아니라는 문제가 있다.

EKF는 테일러전개를 이용해 f와 h를 선형근사해서 F, H를 얻는다.
$F_k=f^{\prime}(x_k), H_k=h^{\prime}(\hat{x}_k)$

EKF 알고리즘

Prediction

$\hat{x}_{k+1}=f(\hat{x}_{k})$
$P_{k+1}=F_kP_kF_k^T+Q$

Update

$y_k = z_k - h(\hat{x}_k)$
$S_k = H_kP_kH_k^T+R$
$K_k=P_kH_k^TS_k^{-1}$
$\hat{x}^\prime_k=\hat{x}_k+K_ky_k$
$P_k^\prime=(I-K_kH_k)P_k$