728x90
반응형
tbmoon님의 Github:
https://github.com/tbmoon/kalman_filter
본인의 Github:
https://github.com/JeongMinHyeok/Kalman_Filter
위치로 속도 추정하기
- 잡음제거가 아닌 다른 목적으로 칼만필터를 사용하는 예제
예제 9-1
- 손칼만 연구원은 요즘 신형 열차의 성능 시험으로 바쁘다. 시험 내용은 직선 선로에서 열차가 80m/s의 속도를 유지하는지 확인하는 것이다. 위치와 속도 정보는 0.1초 간격으로 측정해서 저장하도록 되어 있다. 그런데 시험결과를 살펴보던 손칼만 연구원의 얼굴이 갑자기 사색으로 변했다. 속도 데이터가 모두 0으로 찍혀 있는 게 아닌가! 실험에 드는 비용과 시간이 만만치 않았는데, 이 사실을 위에서 알면 난리가 날 게 뻔하다. 그나마 다행인 것은 위치 정보는 이상이 없어 보인다는 점이다. 우리의 손칼만 연구원을 이 위기에서 구해 낼 방법은 없을까?
시스템 모델
- (속도) = (이동거리) / (시간) 의 공식대로 값을 구하면, 오차가 큼
- 이동평균, 다항식 근사 뒤 미분하는 것은 복잡할 뿐더러 오차가 큼 => 칼만필터로 해결가능
칼만필터 설계 (시스템 모델 설계)
- 열차의 위치와 속도를 상태변수로 정의
- 시스템 모델 설정 (xk, zk, A, H)
시스템 모델 확인 (예제 내용 반영하는지)
- 행렬 A와 상태변수의 정의 시스템 모델식에 대입
- 위치의 관계식 : 위치k+1 = 위치k + 속도k * t(시간간격)
- (현재위치) = (직전위치) + (이동거리)의 물리법칙을 수식으로 표현
- 잡음이 관계식에 포함되지 않음
- 속도 관계식 : 속도k+1 = 속도k + wk(시스템잡음)
- 열차속도는 시스템 잡음의 영향만 받고, 외부영향은 받지 않음 (즉, 속도가 일정)
- 시스템잡음은 마찰, 엔진 제어기의 오차 등 열차속도에 영향을 주는 모든 요인의 합
- 측정값 관계식(행렬 H를 측정값 관계식에 대입) : zk(측정값) = 위치k + vk(측정잡음)
- 측정값은 위치 뿐이며, 측정값에 잡음이 섞여있음 (=예제와 일치함)
- 행렬 A와 H는 임의로 선정하는 것이 아닌, 시스템의 물리적관계 모델링한 결과물
- 즉, 시스템 모델은 속도와 거리 사이의 물리적 관계, 속도에 영향을 주는 오차 요인 등을 수학식으로 표현한 것
잡음의 공분산 행렬 (Q, R)
- 측정잡음(vk)의 오차특성은 보통 센서제작사에서 제공, 정보 없다면 직접 실험 또는 경험을 통해
- 시스템잡음(wk)의 모델링은 시스템에 대한 지식과 경험에 의존해야 함
- 논리적으로 Q와 R을 구하기 어렵다면, 두 행렬을 칼만필터의 설계인자로 보고 시행착오를 거쳐 선정하는 것도 한 방법
칼만필터 함수
- 매개변수 : z(위치 측정값)
- 반환값 : 위치 추정값, 속도 추정값
import numpy as np
import matplotlib.pyplot as plt
from numpy.linalg import inv
np.random.seed(0)
def get_pos_vel(itime, pos_true):
"""측정된 위치와 실제 속도 및 실제 위치 반환하는 함수"""
w = np.random.normal(0, np.sqrt(10)) # w: system noise (시스템 소음)
v = np.random.normal(0, np.sqrt(10)) # v: measurement noise (측정 소음)
vel_true = 80 + w # nominal velocity = 80 [m/s].
if itime == 0:
pos_true = pos_true
else:
pos_true = pos_true + vel_true * dt
z_pos_meas = pos_true + v # z_pos_meas: measured position (관찰가능한)
return z_pos_meas, vel_true, pos_true
def kalman_filter(z_meas, x_esti, P):
"""Kalman Filter Algorithm."""
# (1) Prediction.
x_pred = A @ x_esti
P_pred = A @ P @ A.T + Q
# (2) Kalman Gain.
K = P_pred @ H.T @ inv(H @ P_pred @ H.T + 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
# Input parameters. (0.1초마다 / 총 4초간)
time_end = 4
dt= 0.1
# Initialization for system model. (시스템모델 초기화)
# Matrix: A, H, Q, R, P_0 (행렬값)
# Vector: x_0 (벡터값)
A = np.array([[1, dt],
[0, 1]])
H = np.array([[1, 0]])
Q = np.array([[1, 0],
[0, 3]])
R = np.array([[10]])
# Initialization for estimation.
x_0 = np.array([0, 20]) # position and velocity
P_0 = 5 * np.eye(2) # 2 * 2 행렬 생성
time = np.arange(0, time_end, dt)
n_samples = len(time)
pos_meas_save = np.zeros(n_samples)
vel_true_save = np.zeros(n_samples)
pos_esti_save = np.zeros(n_samples)
vel_esti_save = np.zeros(n_samples)
fig, axes = plt.subplots(nrows=1, ncols=2, figsize=(10, 5))
plt.subplot(1, 2, 1)
plt.plot(time, pos_meas_save, 'r*--', label='Measurements', markersize=10)
plt.plot(time, pos_esti_save, 'bo-', label='Estimation (KF)')
plt.legend(loc='upper left')
plt.title('Position: Meas. v.s. Esti. (KF)')
plt.xlabel('Time [sec]')
plt.ylabel('Position [m]')
plt.subplot(1, 2, 2)
plt.plot(time, vel_true_save, 'g*--', label='True', markersize=10)
plt.plot(time, vel_esti_save, 'bo-', label='Estimation (KF)')
plt.legend(loc='lower right')
plt.title('Velocity: True v.s. Esti. (KF)')
plt.xlabel('Time [sec]')
plt.ylabel('Velocity [m/s]')
plt.show()
- 왼쪽 그림의 파란선은 칼만필터의 추정위치, 빨간선은 측정값
- 측정잡음을 효과적으로 제거하여 추정위치의 궤적이 더 매끄러움
- 오른쪽 그림의 파란선은 칼만필터의 추정값, 초록색은 실제 속도값
- 초반에는 오차가 크지만, 2초 이후부터는 꽤 정확한 값으로 속도 추정
속도로 위치 추정하기
- 측정값이 위치에서 속도로 변경되었으므로 행렬 H가 달라져야 함
- H = [1 0 ] --> H = [0 1]
- 행렬 H를 초기화하는 코드 빼고는 위와 같음
def get_pos_vel(itime):
"""Return Measured Velocity and True Position."""
v = np.random.normal(0, np.sqrt(10)) # v: measurement noise.
vel_true = 80 # nominal velocity = 80 [m/s]. no system noise here.
pos_true = vel_true * (itime * dt) # pos_true: true position.
z_vel_meas = vel_true + v # z_vel_meas: measured velocity (observable)
return z_vel_meas, pos_true
Initialization for system model.
# Matrix: A, H, Q, R, P_0
# Vector: x_0
A = np.array([[1, dt],
[0, 1]])
H = np.array([[0, 1]])
Q = np.array([[1, 0],
[0, 3]])
R = np.array([[10]])
# Initialization for estimation.
x_0 = np.array([0, 20]) # position and velocity
P_0 = 5 * np.eye(2)
time = np.arange(0, time_end, dt)
n_samples = len(time)
vel_meas_save = np.zeros(n_samples)
pos_true_save = np.zeros(n_samples)
pos_esti_save = np.zeros(n_samples)
vel_esti_save = np.zeros(n_samples)
x_esti, P = None, None
for i in range(n_samples):
z_meas, pos_true = get_pos_vel(i)
if i == 0:
x_esti, P = x_0, P_0
else:
x_esti, P = kalman_filter(z_meas, x_esti, P)
vel_meas_save[i] = z_meas
pos_true_save[i] = pos_true
pos_esti_save[i] = x_esti[0]
vel_esti_save[i] = x_esti[1]
fig, axes = plt.subplots(nrows=1, ncols=2, figsize=(10,5))
plt.subplot(1, 2, 1)
plt.plot(time, vel_esti_save, 'bo-', label='Estimation (KF)')
plt.plot(time, vel_meas_save, 'r*--', label='Measurements', markersize=10)
plt.legend(loc='lower right')
plt.title('Velocity: Meas. v.s. Esti. (KF)')
plt.xlabel('Time [sec]')
plt.ylabel('Velocity [m/s]')
plt.subplot(1, 2, 2)
plt.plot(time, pos_esti_save, 'bo-', label='Estimation (KF)')
plt.plot(time, pos_true_save, 'g*--', label='True', markersize=10)
plt.legend(loc='upper left')
plt.title('Position: True v.s. Esti. (KF)')
plt.xlabel('Time [sec]')
plt.ylabel('Position [m]')
- 추정속도 그래프 : 측정잡음이 제거된모습, 기준속도(80m/s) 근처에 추정속도 모여있는 모습 볼 수 있음
- 추정위치 그래프 : 평균속도(80m/s)에 따라 추정위치 움직이는 모습 볼 수 있음 (1초에 약 80m)
초음파 거리계로 속도 추정하기
- 속도가 일정하지 않은 경우에 위치로 속도 추정하기
- 거리와 속도의 물리적인 관계는 변하지 않기 때문에 같은 시스템모델 사용
from scipy import io
input_mat = io.loadmat('./data/9.DvKalman/SonarAlt.mat')
def get_sonar(i):
"""초음파 거리계 측정 데이터 가져오기"""
z = input_mat['sonarAlt'][0][i] # input_mat['sonaralt']: (1, 1501)
return z
def kalman_filter(z_meas, x_esti, P):
# (1) Prediction.
x_pred = A @ x_esti
P_pred = A @ P @ A.T + Q
# (2) Kalman Gain.
K = P_pred @ H.T @ inv(H @ P_pred @ H.T + 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
# Input parameters.
n_samples = 500
time_end = 10
# 시스템 모델 설정
# Matrix: A, H, Q, R, P_0
# Vector: x_0
dt = time_end / n_samples
A = np.array([[1, dt],
[0, 1]])
H = np.array([[1, 0]])
Q = np.array([[1, 0],
[0, 3]])
R = np.array([[10]])
# 추정값 최초 초기화
x_0 = np.array([0, 20]) # 위치 / 속도
P_0 = 5 * np.eye(2)
time = np.arange(0, time_end, dt)
z_pos_meas_save = np.zeros(n_samples)
x_pos_esti_save = np.zeros(n_samples)
x_vel_esti_save = np.zeros(n_samples)
x_esti, P = None, None
for i in range(n_samples):
z_meas = get_sonar(i)
if i == 0:
x_esti, P = x_0, P_0
else:
x_esti, P = kalman_filter(z_meas, x_esti, P)
z_pos_meas_save[i] = z_meas
x_pos_esti_save[i] = x_esti[0]
x_vel_esti_save[i] = x_esti[1]
fig, ax1 = plt.subplots(figsize=(10, 5))
plt.plot(time, z_pos_meas_save, 'r*--', label='Position: Measurements')
plt.plot(time, x_pos_esti_save, 'b-', label='Position: Estimation (KF)')
plt.legend(loc='upper left')
plt.title('Position and Velocity')
plt.xlabel('Time [sec]')
plt.ylabel('Position [m]')
ax2 = ax1.twinx()
plt.plot(time, x_vel_esti_save, 'go-', label='Velocity: Estimation (KF)')
plt.legend(loc='upper right')
plt.ylabel('Velocity [m/s]')
plt.grid(True)
- 잡음제거 및 시간지연 영향 거의 보이지 않음 (잡음제거 및 변화민감성 문제 해결)
- 추정거리(파란색 실선)의 변화율과 추정속도 그래프의 궤적이 비슷한 형태를 띔 ==> 타당한 형태
효율적인 칼만 필터 함수
- 칼만이득(Kk)을 계산할 때 역행렬 계산해내야 함
- 역행렬을 수치해석 기법으로 도출하는데, 이는 계산시간과 안정성에 악영향을 줌
- 위의 예제와 같이 시스템모델의 행렬이 작으면 (2*2) 행렬 계산식을 풀어쓰는 것이 계산시간과 오차를 줄이는 데 도움
- 간단한 행렬을 사용하는 행렬식을 풀어쓰면, 행렬수식이 스칼라 수식으로 변하고 수식이 간단해짐
- 행렬식과 풀어쓴 계산식 동일값 도출 (물리법칙이 같기 때문)
- 상태변수가 많아져 시스템모델의 행렬이 커지면 이 방법 사용하기 어려움
- 효율적인 수치해석 라이브러리 사용할 수 밖에 없음
시스템 모델의 힘
칼만필터가 측정하지 않은 값도 추정할 수 있는 이유
- 저주파 통과필터
- 측정신호에 대해 어떠한 가정도 하지 않음
- 새 측정값과 이전추정값에 가중치를 주고 더할 뿐이므로 측정하지 않은 물리량은 추정할 수 없음
- 칼만 필터
- 시스템의 수학적 모델 알고 있다고 가정 (= 측정값을 출력하는 법칙을 알고 있음)
- 상태변수 사이의 연관관계를 나타내는 시스템 모델을 통해 측정하지 않는 상태변수 간접 추정
- (예제) 추정속도는 시스템의 물리법칙(변수 사이의 관계)을 따라야하므로, 자연히 위치변화의 추이가 속도추정값에 반영됨
추정에 시스템 모델을 사용하는 것은 '양날의 검'
- 시스템 모델이 실제 시스템과 많이 다를 경우, 추정결과와 이를 넘어 전체시스템에 악영향을 줄 수 있음
728x90
'Minding's Reading > 칼만필터는 어렵지 않아' 카테고리의 다른 글
[칼만필터는 어렵지않아/Python] CH.12 확장 칼만 필터 (0) | 2021.07.05 |
---|---|
[칼만필터는어렵지않아/Python] CH.10 영상 속의 물체 추적하기 (0) | 2021.07.01 |
[칼만필터는어렵지않아/Python] CH.07 시스템모델 (0) | 2021.06.24 |
[칼만필터는 어렵지않아/Python] CH.06 예측과정 (0) | 2021.06.24 |
[칼만필터는 어렵지않아 / Python] CH.05 추정과정 (0) | 2021.06.23 |