Kalman filter - Wikipedia
en.wikipedia.org
영상을 보고 내가 이해한 것은 다음과 같다.
- estimate 값과 관찰된 measure 값을 가지고 둘 중 noise의 분산이 더 적은 쪽으로 weight를 더 크게 줘 estimate 값과 관찰된 measure 값 사이의 최종 값을 낸다.
애초에 system은 어떻게 구성되는지, noise distribution은 어떻게 형성이 되는지는 깊숙히 봐야할 것 같다.
디테일하게까지 공부해야하나... 어려울 것 같은데,,
생각했던 것처럼 system이 어떻게 돌아가는지에 대한 이해로부터 식을 짤 수 있어보인다.
내가 kalman filter를 사용하여 filtering하고 싶은 데이터는 위치 데이터이다.
당연하겠지만 현재 위치에서 다음 위치는 (한 축만 생각했을 떄) 현재의 속력 * dt 로 계산할 수 있다.
x와 v를 통해 kalman filter의 system을 구축해보자.
1) Time evolution
\( x_{k + 1} = x_k + v_k \Delta t + \frac{1}{2} a \Delta t^2 \)
\( v_{k + 1} = v_k + a \Delta t \)
$$ \overline{x}_{k + 1} = \begin{bmatrix} x_{k + 1} \\ v_{k + 1} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \overline{x}_k + \begin{bmatrix} \frac{1}{2} \Delta t^2 \\ \Delta t \end{bmatrix} a $$
$$ = F \overline{x}_k + G \overline{u}_k \text{ ( } a \sim N(0, \sigma_a^2) \text{ )} $$
2) \( z_k = x_k + \epsilon_k \)
$$ \overline{z}_k = \begin{bmatrix} 1 & 0 \end{bmatrix} \overline{x}_k + \overline{\epsilon}_k $$
$$ = H \overline{x}_k + \overline{\epsilon}_k \text{ ( } \overline{\epsilon}_k \sim N(0, R_k) \text{ ) } $$
- Prediction step -> Propagate state \( \overline{x}_k \) -> \( \overline{x}_{k + 1} \)
Kalman Predict Equation
If \( \overline{x}_k \sim N(\hat{\overline{x}}_k, P_k) \),
$$ \hat{\overline{x}}_{k + 1} = F \hat{\overline{x}}_k $$
$$ P_{k + 1} = F P_k F^T + G \sigma_a^2 G^T $$
- Measurement step -> incorporate knowledge of \( z_k \) into \( \overline{x}_k \)
Kalman Update Equation
$$ \overline{y} = \overline{z}_k - H \hat{\overline{x}}_k $$
$$ S_k = H P_k H^T + R_k $$
$$ K = P_k H^T S_k^{-1} $$
$$ \hat{\overline{x}}_{|z} = \hat{\overline{x}}_k + K \overline{y} $$
$$ P_{|z} = ( I - K H ) P_k $$
대충 구현해서 parameter tuning하는데 그냥 system자체가 prediction을 너무 못해서 가면 갈수록 measurment에 가중치를 다 줘서 그런지 measurment 값 그대로 나온다.
K 값을 확인하였을 때 거의 1에 인접하게 나오는 것으로 보아 converge한 상태에서 swing하고 있는 것 같다.
이런 이유가 맞는지 모르겠다.
댓글