[칼만필터] Chap 8. 초간단 칼만필터예제

 

" 칼만필터는 어렵지않아" 파이썬으로 된 내용이 있을까... 검색해보았더니 뙇...

역시 세상은 넓고, 감사한 분들은 많다. 

https://github.com/tbmoon/kalman_filter

오랜만에 jupyter notebook을 켜서 확인해보았는데, 이미 아름답게 다 구현이 되어있다. 

앞으로 글정리에서도 위의 코드들을 쏙쏙 이해만해보도록한다..

 

 

---

Chap 8. 초간단 칼만필터예제

https://github.com/tbmoon/kalman_filter/tree/master/Ch08.SimpleKalmanFilter

 

배터리의 전압을 측정하는데, 잡음이 심하다. 칼만필터로 측정데이터의 잡음을 제거한다.

전압은 0.2초 간격으로 측정한다.

 

시스템모델

\(x_{k+1} = x_{k}\)

\(z_k = x_k + v_k \)

앞서 칼만필터의 식에서 A,H,Q,R을 대입해서 보면 

\(x_{k+1} = A x_{k} + w_{k}\)

\(z_{k} = H x_{k} + v_{k} \)

A = 1, H = 1, Q = 0, R = 4 이다. (1:1로 대입해서 보면 똑같이 나온다)

 

\(x_0 = 14 \) -> 초기값은 14V

\(v_k = N(0, 2^{2})\) -> 측정잡음의 평균은 0, 표준편차는 2인 정규분포를 따름

\(w_k = 0\) 임은 시스템 잡음은 없다고 가정하는 것 

배터리 전압이 일정하게 유지되고있다는 상황에서 잡음이 많이 낀 것이라 시스템 모델은 이해가된다. 

 

초기 예측값을 먼저 정해준다. (초기정보가 없으면 오차공분산을 크게 잡자)

\(\hat{x}_{0}^{-} = 14\)

\(P_{0}^{-} = 6\)

 

 

8.4 오차 공분산과 칼만이득

칼만필터 계산시 각 루프마다 측정값, 추정값 및 칼만이득(\(K_k\), 오차공분산\(P_k\)를 저장하여 같이 살펴본다.

---

코드 예제..

함수 kalman_filter

def kalman_filter(z_meas, x_esti, P):
    """Kalman Filter Algorithm for One Variable."""
    # (1) Prediction.
    x_pred = A * x_esti
    P_pred = A * P * A + Q

    # (2) Kalman Gain.
    K = P_pred * H / (H * P_pred * H + R)

    # (3) Estimation.
    x_esti = x_pred + K * (z_meas - H * x_pred)

    # (4) Error Covariance.
    P = P_pred - K * H * P_pred

    return x_esti, P

코드의 경우는 해보면 되기때문에 어렵지않다. 

결과적으로만 분석해보면..

 

 

 

 

  v = np.random.normal(0, 2)   # v: measurement noise.
  volt_true = 14.4             # volt_true: True voltage [V].
  z_volt_meas = volt_true + v

numpy의 random.normal로 하면 평균0, 분산2의 랜덤한 변수들이 생성된다. 

kalman_filter 함수를 수행할때마다, 추정값, P(Error covariance), K(kalman gain) 값을 저장하고 그래프로 출력한 것이다.

시간이 지날수록 P,K값이 줄어들고, 칼만필터가 측정잡음을 제거하여 추정값이 참값(14.4V)에 근접하게 그래프를 보여주고있다. 

 

오차공분산이 줄어든다는 말은 추정값의 오차가 작아진다는말과같다.  

추정값의 오차가 충분히 작아서 더이상 줄어들 여지가 없는 상태로 수렴한 것. 

 

칼만 게인이 작아진다는 것의 의미는?

 \(\hat{x}_{k}= \hat{x}_{k}^{-}+ K_{k}(z_{k} - H\hat{x}_{k}^{-})\)

예측값: \( \hat{x}_{k}^{-}\)

추정값: \(K_{k}(z_{k} - H\hat{x}_{k}^{-})\)

칼만게인이 작으면 추정값은 작아지고, 예측값의 비중이 커지게된다. 

추정값은 추정오차가 충분히 작아지면 더이상 추정값이 변하지않을정도로 수렴해버려서 결국 예측값이 큰비중을 차지하게된다.