칼만필터 파이썬 - 5. 칼만 필터를 이용한 위치로 속도 계산

아깐 볼트 측정 값을 받아와서 추정하다보니

시스템 행렬이고 뭐고 할게 없었음

이번엔 위치로 속도 구하려고하는데

다루는 상태가 전압값 이 아닌

상태는 위치, 속도 두가지

이젠 행렬을 다뤄야 한다.

 

 

다루는 상태 X는 pos, vel

측정하는 값은 pos만 한다고 하자

시스템 모델 정리하면 다음과 같다,

 

 

pos는 이전 pos + 이전 속도 * dt로 구할수 있으므로

시스템 행렬 A는 다음과 같으며

관측 행렬 H의 경우 pos와 vel 중에서 pos만 뽑으면 되니 (1 0)이 된다. 

 

시스템 노이즈 Q와 관측 노이즈 R은 다음과 같이 설정하자

 

 

 

위치 값 가져오기 함수 구현

 

아직 사용하는 위치는 x, y같은 좌표값은 아니고 1차원 값임

초기 위치 0, 속도 80

import numpy as np


pos_true = 0
vel_true = 80
dt = 0.1

def get_pos():
    global pos_true
    global vel_true
    w = 0 + 10 * np.random.normal() # vel noise
    v = 0 + 10 * np.random.normal() # pos noise
    z = pos_true + vel_true * dt + v
    pos_true = z - v
    vel_true = 80 + w
    return z

for _ in range(20):
    print(pos_true, "-", get_pos())

 

 

 

위치 -> 속도, 위치 추정

일단 사용할 변수들 부터  정의하자

시스템 노이즈 Q 같은 경우 포즈 공분산 1, 속도 공분산 3놓고

X는 pos 0 vel 20으로 설정하자

오차 공분산 P는 5 * eye해서 정하고

관측 노이즈 R은 10으로 설정

 

import matplotlib.pyplot as plt

pos_true = 0
vel_true = 80
dt = 0.1
time_cur, time_stop = 0, 5

t_list = []
pos_true_list = []
pos_est_list = []
vel_true_list = []
vel_est_list = []

A = np.array([
    [1, dt],
    [0, 1]
])

H = np.array([
    [1, 0]
])
Q = np.array([
    [1, 0],
    [0, 3]
])
X = np.array([
    [0, 20]
]).T
P = 5 * np.eye(2)
R = 10

 

 

위치 -> 위치, 속도 계산하는 kf 로직은 거의 비슷한데

행렬 연산하다보니 연산자 @을 사용

def dv_kf(z):
    x_pred = A @ X
    P_pred = A @ P @ A.T + Q

    K_numerator = P_pred @ H.T
    K_denominator = H @ P_pred @ H.T + R
    K_denominator = np.linalg.inv(K_denominator)
    K = K_numerator @ K_denominator
    
    measurement_err = z - H @ x_pred
    x_est = x_pred + K @ measurement_err
    P_est = P_pred - K @ H @ P_pred
    return x_est, P_est

 

 

동작 로직은 다음과 같이 구현

while time_cur < time_stop:
    z = get_pos()
    X, P = dv_kf(z)

    t_list.append(time_cur)
    pos_true_list.append(pos_true)
    pos_est_list.append(X[0][0])
    vel_true_list.append(vel_true)
    vel_est_list.append(X[1][0])
    time_cur += dt

ax1 = plt.subplot(2, 1, 1)
ax2 = plt.subplot(2, 1, 2)
ax1.plot(t_list, pos_true_list, label="true pos")
ax1.plot(t_list, pos_est_list, label="kf pos")
ax1.legend()
ax2.plot(t_list, vel_true_list, label="true speed")
ax2.plot(t_list, vel_est_list, label="kf speed")
ax2.legend()

 

 

추정 결과를 보면

위치는 잘 잡긴한데

속도는 처음엔 잘 못잡다가 시간 가면서 실제 속도를 잘 추정해낸다.

 

 

 

 

 

속도 -> 속도, 위치 추정

먼저 데이터 가져오기 함수부터 고치자

아깐 위치 가져오기 함수였지만 

실제 거리에선 노이즈 빼고

z = vel_ true로 설정

def get_vel():
    global pos_true
    global vel_true
    w = 0 + 10 * np.random.normal() # vel noise
    vel_true = 80 + w
    pos_true = pos_true + vel_true * dt
    z = vel_true
    return z

 

칼만필터 로직은 변경할거 없고

X에서 위치를 뽑아내려고

H = [1, 0] 로 했던걸

속도를 뽑아낼수 있도록

H = [0, 1]로 고치가만 하면 끝

 

 

import matplotlib.pyplot as plt

def get_vel():
    global pos_true
    global vel_true
    w = 0 + 10 * np.random.normal() # vel noise
    vel_true = 80 + w
    pos_true = pos_true + vel_true * dt
    z = vel_true
    return z


pos_true = 0
vel_true = 80
dt = 0.1
time_cur, time_stop = 0, 5

t_list = []
pos_true_list = []
pos_est_list = []
vel_true_list = []
vel_est_list = []

A = np.array([
    [1, dt],
    [0, 1]
])

H = np.array([
    [0, 1]
])
Q = np.array([
    [1, 0],
    [0, 3]
])
X = np.array([
    [0, 20]
]).T
P = 5 * np.eye(2)
R = 10

def int_kf(z):
    x_pred = A @ X
    P_pred = A @ P @ A.T + Q

    K_numerator = P_pred @ H.T
    K_denominator = H @ P_pred @ H.T + R
    K_denominator = np.linalg.inv(K_denominator)
    K = K_numerator @ K_denominator

    measurement_err = z - H @ x_pred
    x_est = x_pred + K @ measurement_err
    P_est = P_pred - K @ H @ P_pred
    return x_est, P_est


while time_cur < time_stop:
    pos_true_list.append(pos_true)

    z = get_vel()
    X, P = int_kf(z)

    t_list.append(time_cur)
    pos_est_list.append(X[0][0])
    vel_true_list.append(vel_true)
    vel_est_list.append(X[1][0])
    time_cur += dt

ax1 = plt.subplot(2, 1, 1)
ax2 = plt.subplot(2, 1, 2)
ax1.plot(t_list, pos_true_list, label="true pos")
ax1.plot(t_list, pos_est_list, label="kf pos")
ax1.legend()
ax2.plot(t_list, vel_true_list, label="true speed")
ax2.plot(t_list, vel_est_list, label="kf speed")
ax2.legend()