칼만필터 파이썬 - 6. 칼만 필터를 이용한 추적기 구현

이번에 작성할 내용은 칼만 필터를 이용한 추적기를 만들려고 했는데

opencv 영상 처리로 얻어낸 물체 추적을 하자니 너무 복잡해질거같고

파이 게임에다가 공 그려놓고 하자니 파이 게임 경험이 없어서 하질 못하겠다

 

가장 간단하게 생각 나는건

plt 애니메이션 인데

어쩔수 없이 plt 애니메이션 만듬

 

동작은 0,0 원점에서 10,10 지점까지 직선으로 이동하는 상황을 플로팅부터 구현함

 

 

vscode ipynb matplotlib funcanimation 쓰기

아무 plt 애니메이션 예제 돌리려고하니까 동작이 안된다

찾아보니 vscode 에서 ipynb 아나콘다 파이선 인터프리터 쓰려고보니

%matplotlib notebook은 주피터 노트북이 아니면 동작안되서 그렇다는거같다

대신 이 글대로 ipympl 설치해서 플로팅 전에

%matplotlib ipympl 해주면 된다

https://stackoverflow.com/questions/64613706/animate-update-a-matplotlib-plot-in-vs-code-notebook

 

 

대충 이렇게 하면 

y=x 따라 점 포인트가 0, 0 에서 10, 10으로 이동하는 애니메이션이 나온다

%matplotlib ipympl

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

fig, ax = plt.subplots()
def animate(x):
    ax.clear()
    ax.grid()
    ax.scatter(x, x)
    ax.set_xlim([0, 10])
    ax.set_ylim([0, 10])
    

ani = FuncAnimation(fig, animate, frames=np.arange(0, 10, 0.1))
plt.show()

 

 

 

공 위치 구현하기

일단 vscode ipynb에서 함수 애니메이션은 했는데

공 위치를 0 ~ 10은 너무 좁은거같아 0 ~ 100으로 늘림,

노이즈 고려해서 구간도 -50 ~ 150으로 늘리고 보기 좋게 축 라인도 그려줌

 

프레임 간격도 0.1에서 1로 변경

animate(x)로 넘어오는 x는 실제 위치니

여기다 노이즈도 첨가

 

이제 좀 이쁘게 보인다

 

%matplotlib ipympl

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

def get_ball_pos(pos_true):
    x = pos_true + 5 * np.random.normal()
    y = pos_true + 5 * np.random.normal()
    return x, y

fig, ax = plt.subplots()
def animate(pos_true):
    ax.clear()
    ax.grid()
    x, y = get_ball_pos(pos_true)
    ax.scatter(x, y, label="measured pos")
    ax.scatter(pos_true, pos_true, label="true pos")
    ax.set_xlim([-50, 150])
    ax.set_ylim([-50, 150])
    ax.axhline(linewidth=2, color='k')
    ax.axvline(linewidth=2, color='k')
    ax.legend()

ani = FuncAnimation(fig, animate, frames=np.arange(0, 100, 1))
plt.show()

 

 

시스템 행렬, 관측 행렬 등 정리하기

애니메이션, 관측값 노이즈 첨가도 했으니

kf 구현하기전에 시스템 모델을 어떻게 할지 정리하자

 

상태 x 는 pos x, vel x, pos y, vel y 4가지로 구성되고

관측할건 pos x, pos y가 되겠다.

그러면 시스템 행렬 A와 관측 모델 H는 다음과 같음

 

 

 

kf 공 추적기 구현하기

아까 실수로 get ball pos에서 z가 아닌 x, y 반환하도록 구현했다. 이거 수정하고

위에서 설명한대로 A, H 구현

나머지 Q, R, P 같은 경우는 어떻게 좋은 값 구하는 방법을 잘모르겟다 그냥 설정

 

전역, 지역변수 뒤죽박죽되서 코드가 좀 엉망이긴한데

대충 관측 x, y 값가지고 

실제 x, y에 가깝게 잘 추정해낸다.

 

출력해서 제대로 보진 않았지만 x, y vel도 잘 추정하고 있겟지

 

%matplotlib ipympl

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

def get_ball_pos(pos_true):
    x = pos_true + 5 * np.random.normal()
    y = pos_true + 5 * np.random.normal()
    z = np.array([[x, y]]).T
    return z

dt = 1
A = np.array([
    [1, dt, 0, 0],
    [0, 1, 0, 0],
    [0, 0, 1, dt],
    [0, 0, 0, 1]
])
H = np.array([
    [1, 0, 0, 0],
    [0, 0, 1, 0]
])
Q = 0.01 * np.eye(4)
R = np.array([
    [10, 0],
    [0, 10]
])
P = 20 * np.eye(4)
X = np.array([
    [0, 0, 0, 0]
]).T

def kf_tracker(z):
    global P

    X_pred = A @ X
    P_pred = A @ P @ A.T + Q
    K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
    X_est = X_pred + K @ (z - H @ X_pred)
    P = P_pred - K @ H @ P_pred
    return X_est


fig, ax = plt.subplots()

def animate(pos_true):
    global X
    print(pos_true)
    ax.clear()
    ax.grid()
    z = get_ball_pos(pos_true)

    ax.scatter(z[0][0], z[1][0], label="measured pos")
    ax.scatter(pos_true, pos_true, label="true pos")
    X_est = kf_tracker(z)
    X = X_est
    ax.scatter(X_est[0][0], X_est[2][0], label="estimated pos")
    ax.set_xlim([-50, 150])
    ax.set_ylim([-50, 150])
    ax.axhline(linewidth=2, color='k')
    ax.axvline(linewidth=2, color='k')
    ax.legend()

ani = FuncAnimation(fig, animate, frames=np.arange(0, 100, 1))
plt.show()