2주차 목표

• 칼만 필터에 대한 조사 및 공부

회고

칼만필터란?

• A filter that predict about what the system will do next when information is unclear
• Reduce noise
• Don’t need to keep records except for previous state
• Fast

전체적인 절차

1. Select initial values
2. Predict estimation and covariance
3. Calculate Kalman gain
4. Calculate estimation and covariance

1. Select initial values

• $\hat{x}_0$
• $P_0$

2. Predict estimation and covariance

• $\hat{x}k = F\hat{x}{k-1}$
• $P_k = FP_{k-1}F^T+Q$

When the car status data is $\vec{x} = [p \space v]$ , If the car is doing constant speed exercise,

$p_k = p_{k-1}+v_{k-1}t$ (t is time)

$v_k = v_{k-1}$

→ $\hat{x}{\bar{k}} = \begin{bmatrix}1&t\0&1\end{bmatrix}\hat{x}{k-1}$

→ $\hat{x}{\bar{k}} = F_k \hat{x}{k-1}$

If covariance is multiplied by A, it’s $Cov(Ax) = APA^T$

So, $P_k = F_kP_{k-1}F^T_{k}$

However, the covariance is added with an external uncertain value Q, because the system doesn’t always move as modeled.

→ $P_k = FP_{k-1}F^T+Q$

3. Calculate Kalman gain

• $K_k = P_kH^T(HP_kH^T+R)^{-1}$

Using a matrix $H_k$,

match the range of predicted state with a range of state by the actual sensor ($z_k, R_k$)

$\hat{x}_k = H_k\hat{x}_k$

$P_k = H_kP_kH^T_k$

→ $(\mu_0, \sum_0) = (H_k\hat{x}_k, H_kP_kH^T_k)$

 $(\mu_1, \sum_1) = (z_k, R_k)$

The value of sensor appears as a Gaussian distribution because of noise, which combines the value with $\hat{x}_k$.

Calculate the mean and covariance through 2D Gaussian product.

$\mu = \mu_0 + K(\mu_1 - \mu_0)$ ———— (1)

$\sum = \sum_0-K\sum_0$ ———— (2)

$K=\sum_0(\sum_0+\sum_1)^{-1}$

K is Kalman gain and

$\sum_0 = H_kP_kH^T_k$ , $\sum_1 = R$

so, $K_k = P_kH^T(HP_kH^T+R)^{-1}$

4. Calculate estimation and covariance

• $\hat{x}_k^{‘} = \hat{x}_k + K_k(z_k-H_k\hat{x}_k)$
• $\hat{P}_k = P_k - K_kH_kP_k$

Using formula (1)

$\mu = \hat{x}_k + K_k(z_k-H_k\hat{x}_k)$

→ $\hat{x}_k^{‘} = \hat{x}_k + K_k(z_k-H_k\hat{x}_k)$

Using formula (2)

$\sum = P_k - K_kH_kP_k$

→ $\hat{P}_k = P_k - K_kH_kP_k$