이 문서는 칼만 필터의 복잡한 수학 이론을 잠시 접어두고, 오직 파이썬 코드를 통한 ‘구현’에 집중하기 위해 작성되었다. 칼만 필터를 실제로 사용하려면 이론적 배경만큼이나 코드로 옮기는 능력이 중요하다. 여기서는 가장 단순한 1차원 필터부터 복잡한 비선형 시스템을 다루는 확장 칼만 필터(EKF)까지, 난이도 단계별 예제를 통해 실전 구현 능력을 체계적으로 쌓아나가는 것을 목표로 한다.
모든 칼만 필터, 그리고 그 변종들은 본질적으로 두 가지 단계의 무한 반복으로 이루어진다. 이 구조를 먼저 이해하면 앞으로 나올 모든 코드를 일관된 관점으로 파악할 수 있다.
이 두 단계의 반복이 칼만 필터 알고리즘의 전부다. 하지만 이 단순한 사이클이 ‘최적 추정기’로 불리는 데에는 근본적인 가정이 숨어있다. 바로 시스템의 상태와 모든 잡음(noise)이 가우시안 분포(정규 분포)를 따른다는 가정이다. 예측 단계는 현재의 가우시안 분포를 시스템 모델에 따라 다음 시간의 가우시안 분포로 변환하는 과정이며, 갱신 단계는 예측된 가우시안 분포와 측정값의 가우시안 분포라는 두 정보를 결합하여 더 정확한(분산이 작은) 새로운 가우시안 분포를 만들어내는 과정이다. 이 가우시안 가정이 깨질 때 왜 EKF와 같은 변종이 필요한지가 명확해진다.
보고서 전체에서 일관되게 사용할 핵심 변수와 행렬을 미리 정의하고 넘어가자. 이 표는 각 예제 단계별로 변수들의 차원이 어떻게 ‘성장’하는지를 보여주는 로드맵 역할을 할 것이다.
| 변수 | 기호 | 설명 | 난이도 하 (1D) | 난이도 중 (2D) | 난이도 상 (EKF) |
|---|---|---|---|---|---|
| 상태 추정값 | $x$ | 시스템의 현재 상태에 대한 최적의 추정치. | (1, 1) |
(4, 1) |
(4, 1) |
| 오차 공분산 | $P$ | 상태 추정값의 불확실성 및 변수 간 상관관계. | (1, 1) |
(4, 4) |
(4, 4) |
| 상태 전이 모델 | $F$ | 현재 상태를 다음 상태로 변환하는 선형 모델. 비선형 시 야코비안 $F_j$ 사용. | (1, 1) |
(4, 4) |
(4, 4) |
| 프로세스 잡음 공분산 | $Q$ | 상태 전이 모델 자체의 불확실성 (모델링되지 않은 동역학). | (1, 1) |
(4, 4) |
(4, 4) |
| 관측 모델 | $H$ | 상태 공간을 측정 공간으로 매핑하는 선형 모델. 비선형 시 야코비안 $H_j$ 사용. | (1, 1) |
(2, 4) |
(2, 4) - 야코비안 |
| 측정 잡음 공분산 | $R$ | 센서 측정값에 포함된 불확실성. | (1, 1) |
(2, 2) |
(2, 2) |
| 측정값 | $z$ | 센서로부터 받은 실제 데이터. | (1, 1) |
(2, 1) |
(2, 1) |
이 표를 보면, 문제의 차원이 높아질수록 상태 벡터 $x$가 더 많은 정보를 담게 되고, 이에 따라 다른 행렬들의 차원도 함께 커지는 것을 알 수 있다. 예를 들어, 오차 공분산 $P$는 1차원에서는 단순한 분산 값(스칼라)이지만, 2차원 추적 문제에서는 위치와 속도의 불확실성 및 그들 간의 상관관계를 모두 담는 4x4 행렬로 확장된다.
이제 이 기본 프레임워크와 변수 정의를 머릿속에 넣고, 가장 간단한 예제부터 시작해보자.
가장 먼저 다룰 예제는 칼만 필터의 핵심 로직을 군더더기 없이 이해하기 위한 가장 단순한 시나리오다. 복잡한 물리 모델 대신, 변하지 않는 참값을 가진 시스템을 노이즈가 낀 센서로 측정하여 원래의 참값을 추정하는 문제를 풀어본다.
1.25V의 전압을 내는 안정적인 배터리가 있다고 가정하자. 이 배터리의 전압을 멀티미터로 반복해서 측정한다. 멀티미터는 완벽하지 않아서 측정할 때마다 약간의 노이즈가 낀 값을 보여준다. 우리의 목표는 이 노이즈 낀 측정값들을 사용해 배터리의 실제 전압(1.25V)을 가장 정확하게 추정하는 것이다.
이 시나리오를 칼만 필터의 용어로 모델링해보자.
이제 위 설명을 칼만 필터의 행렬로 변환해야 한다. NumPy를 사용하여 각 파라미터를 정의한다.
[[1.]] 이다.[[1.]] 이다.[[0.01]] 로 설정한다.[[1e-5]] 로 설정한다.[[0.]][[1.]]. $P$를 크게 잡아야 필터가 초기에 새로운 측정값을 더 적극적으로 받아들인다.이제 전체 코드를 통해 예측과 갱신 사이클을 구현해보자.
import numpy as np
import matplotlib.pyplot as plt
# 0. 시스템 파라미터 정의
dt = 1.0 # 시간 간격 (이 예제에서는 사용되지 않음)
n_iter = 50 # 시뮬레이션 반복 횟수
true_voltage = 1.25 # 배터리의 실제 전압 (참값)
# 1. 칼만 필터 변수 초기화
# 모든 변수는 행렬 형태(2D array)로 다룬다.
# 이는 나중에 다차원 시스템으로 확장할 때 코드의 일관성을 유지하기 위함이다.
# 상태 변수 x: [전압]
x = np.array([[0.]]) # 초기 상태 추정치 (0V로 가정)
# 오차 공분산 P: 상태 추정값의 불확실성
P = np.array([[1.]]) # 초기 불확실성은 크게 설정
# 상태 전이 행렬 F
# x_k = F * x_{k-1}
# 전압은 변하지 않는다고 가정하므로 F = 1
F = np.array([[1.]])
# 관측 행렬 H
# z_k = H * x_k
# 전압을 직접 측정하므로 H = 1
H = np.array([[1.]])
# 측정 잡음 공분산 R
# 측정 노이즈의 분산. 표준편차가 0.1이라고 가정 (R = sigma^2)
R = np.array([[0.01]])
# 프로세스 잡음 공분산 Q
# 모델의 불확실성. 전압이 미세하게 변할 수 있음을 반영
Q = np.array([[1e-5]])
# 시뮬레이션을 위한 데이터 저장용 리스트
x_estimates =
measurements =
p_history =
# 2. 메인 루프: 예측과 갱신 사이클
np.random.seed(42) # 재현성을 위한 시드 설정
for _ in range(n_iter):
# --- (1) 예측 (Predict) 단계 ---
# 상태 예측: x_pred = F * x
x = F @ x
# 오차 공분산 예측: P_pred = F * P * F^T + Q
P = F @ P @ F.T + Q
# --- (2) 갱신 (Update) 단계 ---
# 칼만 이득(K) 계산
# K = P_pred * H^T * (H * P_pred * H^T + R)^-1
K_numerator = P @ H.T
K_denominator = H @ P @ H.T + R
# 1D에서는 행렬의 역행렬이 스칼라의 역수와 같으므로 단순 나눗셈 가능
K = K_numerator / K_denominator
# 측정값 생성 (시뮬레이션)
# 실제 값에 노이즈를 추가하여 측정값을 모방
measurement_noise = np.random.randn() * np.sqrt(R)
z = np.array([[true_voltage + measurement_noise]])
# 상태 추정치 갱신
# x_new = x_pred + K * (z - H * x_pred)
residual = z - H @ x
x = x + K @ residual
# 오차 공분산 갱신
# P_new = (I - K * H) * P_pred
I = np.eye(1)
P = (I - K @ H) @ P
# 결과 저장
x_estimates.append(x.item())
measurements.append(z.item())
p_history.append(P.item())
# 3. 결과 시각화
plt.figure(figsize=(12, 8))
# 전압 추정 결과
plt.subplot(2, 1, 1)
plt.plot(range(n_iter), measurements, 'r.', label='Measurements (z)')
plt.plot(range(n_iter), x_estimates, 'b-', label='Kalman Filter Estimate (x)')
plt.axhline(y=true_voltage, color='g', linestyle='--', label='True Voltage')
plt.title('1D Kalman Filter: Voltage Estimation')
plt.xlabel('Time Step')
plt.ylabel('Voltage (V)')
plt.legend()
plt.grid(True)
# 오차 공분산 P의 변화
plt.subplot(2, 1, 2)
plt.plot(range(n_iter), p_history, 'k-')
plt.title('Error Covariance (P)')
plt.xlabel('Time Step')
plt.ylabel('Variance')
plt.grid(True)
plt.tight_layout()
plt.show()
# 상태 예측: x_pred = F * x
x = F @ x
# 오차 공분산 예측: P_pred = F * P * F^T + Q
P = F @ P @ F.T + Q
[[1.]]이므로, 이 코드는 $x$를 그대로 유지한다. 이는 “다음 전압도 현재 전압과 같을 것이다”라는 우리의 모델을 그대로 반영한다.# 칼만 이득(K) 계산
K_numerator = P @ H.T
K_denominator = H @ P @ H.T + R
K = K_numerator / K_denominator
# 상태 추정치 갱신
residual = z - H @ x
x = x + K @ residual
# 오차 공분산 갱신
I = np.eye(1)
P = (I - K @ H) @ P
residual = z - H @ x는 실제 측정값($z$)과 예측된 측정값($H @ x$) 사이의 차이, 즉 ‘혁신(innovation)’이다. 이 오차에 칼만 이득 $K$를 곱한 만큼 현재 상태 추정치 $x$를 보정한다.위 코드를 실행하면 두 개의 그래프가 나타난다.
이 단순한 1차원 예제는 사실 동적 가중 이동 평균(dynamic weighted moving average)과 같다. 상태 갱신 수식 $x_{new} = x_{old} + K \cdot (z - x_{old})$는 $x_{new} = (1 - K) \cdot x_{old} + K \cdot z$로 다시 쓸 수 있다. 이는 이전 추정치($x_{old}$)와 새로운 측정치($z$) 사이의 가중 평균이다. 일반적인 이동 평균과 다른 점은 가중치 $K$가 고정되어 있지 않고, 필터의 불확실성($P$)에 따라 매 순간 최적으로 계산된다는 것이다. 이것이 칼만 필터가 단순한 필터링 기법을 넘어 ‘최적 추정기(optimal estimator)’라 불리는 이유다.
이제 한 단계 나아가, 여러 개의 상태 변수를 동시에 다루는 다차원 시스템으로 확장해보자. 1차원 예제에서 다진 예측-갱신 구조는 그대로 유지된다. 달라지는 것은 상태 벡터와 시스템 행렬의 정의, 그리고 모든 연산이 스칼라가 아닌 행렬 연산으로 수행된다는 점뿐이다.
2차원 평면 위를 거의 일정한 속도로 움직이는 물체(예: 자동차, 드론)가 있다고 가정하자. 우리는 노이즈가 낀 GPS 센서를 통해 주기적으로 이 물체의 위치(x, y 좌표)를 측정할 수 있다. 우리의 목표는 이 불규칙한 위치 측정값을 바탕으로 물체의 실제 경로를 부드럽게 추정하고, 더 나아가 측정되지 않는 속도($v_x, v_y$)까지 추정하는 것이다.
이 문제를 칼만 필터로 풀기 위한 가장 중요한 첫 단계는 상태 벡터 $x$를 어떻게 정의할 것인가이다. 우리는 위치뿐만 아니라 속도도 추정하고 싶으므로, 상태 벡터에 위치와 속도를 모두 포함해야 한다.
이제 이 4차원 상태 벡터를 기준으로 나머지 행렬들을 정의해야 한다.
상태 전이 행렬 ($F$): 이 행렬은 현재 상태(k-1)가 다음 상태(k)로 어떻게 변하는지를 기술하는 물리 법칙(운동 모델)을 담고 있다. 등속도 운동 모델에서 다음 위치와 속도는 다음과 같이 계산된다.
이 관계를 행렬 형태로 표현하면 $x_k = F x_{k-1}$ 이며, 4x4 상태 전이 행렬 $F$는 다음과 같다. \(F = \begin{bmatrix} 1 & 0 & dt & 0 \\ 0 & 1 & 0 & dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}\) 관측 행렬 ($H$): 이 행렬은 4차원의 상태 공간에서 우리가 실제로 측정할 수 있는 2차원의 측정 공간으로 어떻게 매핑되는지를 정의한다. GPS 센서는 위치($p_x, p_y$)만 측정하고 속도는 측정하지 못한다.
따라서 2x4 관측 행렬 $H$는 4차원 상태 벡터에서 우리가 관심 있는 부분(위치)만 “추출”하는 역할을 한다. \(H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}\) 측정 잡음 공분산 ($R$): GPS 센서의 측정 노이즈에 대한 2x2 공분산 행렬이다. x축과 y축의 측정 노이즈가 서로 독립적이라고 가정하면, 대각선에 각 축의 노이즈 분산을, 비대각선에는 0을 넣는다. 예를 들어, 위치 측정의 표준편차가 5미터라고 가정하면, 분산은 25가 된다.
R = np.array([[25., 0.],
[0., 25.]])
프로세스 잡음 공분산 ($Q$): 우리의 ‘등속도’ 모델이 얼마나 정확한지에 대한 불확실성을 나타낸다. 실제 세계에서 물체는 완벽한 등속도로 움직이지 않는다. 바람, 마찰, 운전자의 가속/감속 등 예측하지 못한 미세한 가속도가 항상 존재한다. $Q$는 이러한 모델링되지 않은 작은 변화(가속도)가 시스템의 불확실성에 어떤 영향을 미치는지를 표현하는 4x4 행렬이다. $Q$를 구성하는 것은 칼만 필터 튜닝에서 가장 어려운 부분 중 하나이지만, 일반적으로 시간 간격 $dt$와 가속도 노이즈의 분산을 이용하여 구성한다. $Q$를 크게 설정할수록 필터는 모델 예측보다 측정값을 더 신뢰하게 된다.
초기값:
P = np.diag([10., 10., 1000., 1000.]) # 위치 분산은 작게, 속도 분산은 매우 크게
이제 2차원 추적 필터를 코드로 구현해보자. 1차원 예제와 구조는 동일하지만, 모든 변수가 행렬이라는 점에 유의해야 한다.
import numpy as np
import matplotlib.pyplot as plt
# 0. 시뮬레이션 환경 설정
dt = 1.0
n_iter = 100
# 실제 물체의 초기 상태 [px, py, vx, vy]
true_x = np.array([0.0, 0.0, 5.0, 5.0]) # x=0, y=0에서 시작, vx=5, vy=5
# 1. 칼만 필터 파라미터 초기화
# 초기 상태 추정치. 첫 위치는 알지만(첫 측정값 사용 가정), 속도는 모른다고 가정
x = np.array([[0.0], [0.0], [0.0], [0.0]]) # 4x1 벡터
# 초기 오차 공분산. 위치 불확실성은 작게, 속도 불확실성은 매우 크게
P = np.diag([10.0, 10.0, 1000.0, 1000.0]) # 4x4 행렬
# 상태 전이 행렬 F (등속도 모델)
F = np.array([[1, 0, dt, 0],
[0, 1, 0, dt],
,
]) # 4x4 행렬
# 관측 행렬 H (위치만 측정)
H = np.array([,
]) # 2x4 행렬
# 측정 잡음 공분산 R (GPS 노이즈)
measurement_noise_std = 10.0 # 표준편차 10m
R = np.diag([measurement_noise_std**2, measurement_noise_std**2]) # 2x2 행렬
# 프로세스 잡음 공분산 Q (모델의 불확실성)
# 가속도에 의한 불확실성을 모델링
q_std = 0.1
Q = np.diag([q_std**2, q_std**2, q_std**2, q_std**2]) # 간단한 형태의 Q
# 데이터 저장을 위한 리스트
true_path =
measurements =
kf_estimates =
# 2. 메인 루프: 예측과 갱신
for _ in range(n_iter):
# 실제 물체 이동 (시뮬레이션용)
true_x = F @ true_x
true_path.append(true_x.flatten()[:2])
# 측정값 생성 (시뮬레이션용)
measurement_noise = np.random.randn(2, 1) * measurement_noise_std
z = H @ true_x.reshape(4,1) + measurement_noise
measurements.append(z.flatten())
# --- (1) 예측 (Predict) 단계 ---
x_pred = F @ x
P_pred = F @ P @ F.T + Q
# --- (2) 갱신 (Update) 단계 ---
# 잔차(Innovation) 계산
y = z - H @ x_pred
# 잔차 공분산(Innovation Covariance) 계산
S = H @ P_pred @ H.T + R
# 칼만 이득(K) 계산
K = P_pred @ H.T @ np.linalg.inv(S)
# 상태 추정치 갱신
x = x_pred + K @ y
# 오차 공분산 갱신
I = np.eye(len(x))
P = (I - K @ H) @ P_pred
# 결과 저장
kf_estimates.append(x.flatten()[:2])
# NumPy 배열로 변환
true_path = np.array(true_path)
measurements = np.array(measurements)
kf_estimates = np.array(kf_estimates)
# 3. 결과 시각화
plt.figure(figsize=(12, 12))
plt.plot(true_path[:, 0], true_path[:, 1], 'g-', label='True Path')
plt.plot(measurements[:, 0], measurements[:, 1], 'r.', markersize=5, label='Measurements (z)')
plt.plot(kf_estimates[:, 0], kf_estimates[:, 1], 'b-', linewidth=2, label='Kalman Filter Estimate')
plt.title('2D Object Tracking with Kalman Filter')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.legend()
plt.grid(True)
plt.axis('equal')
plt.show()
코드를 보면 1차원 예제와 알고리즘 흐름이 완전히 동일함을 알 수 있다. $F @ x$, $F @ P @ F.T + Q$ 등 모든 수식이 그대로 사용된다. 단지 이제는 NumPy의 @ 연산자가 스칼라 곱셈이 아닌 행렬 곱셈을 수행할 뿐이다.
가장 큰 차이점과 그 의미는 오차 공분산 행렬 $P$에서 나타난다. 1차원 예제에서 $P$는 단일 값, 즉 전압 추정치의 분산이었다. 하지만 이 2차원 예제에서 4x4 행렬 $P$는 훨씬 더 풍부한 정보를 담고 있는 지식의 지도와 같다.
이 상관관계는 어떻게 생성되고 활용되는가?
이처럼 칼만 필터는 단순히 각 변수를 독립적으로 추정하는 것이 아니라, 변수들 간의 불확실성이 어떻게 서로 영향을 주고받는지를 $P$ 행렬에 동적으로 기록하고 관리한다. 이 덕분에 우리는 위치만 측정함에도 불구하고 속도를 추정할 수 있다. 필터는 시간에 따른 위치 변화 패턴을 관찰하고, 그 변화를 가장 잘 설명하는 속도를 ‘추론’해내는 것이다.
지금까지 다룬 표준 칼만 필터는 강력하지만 치명적인 한계가 있다. 바로 시스템이 선형(linear)이라고 가정한다는 점이다. 상태 전이($x_k = F \cdot x_{k-1}$)와 관측($z_k = H \cdot x_k$)이 모두 행렬 곱셈으로 표현될 수 있어야만 한다. 하지만 현실 세계의 많은 시스템은 비선형적이다.
확장 칼만 필터(Extended Kalman Filter, EKF)는 이러한 비선형 문제를 다루기 위한 가장 대표적인 해법이다. EKF의 핵심 아이디어는 간단하다. “비선형 함수를 다룰 수 없다면, 매 순간 현재 추정치 주변에서 함수를 선형으로 근사하여 표준 칼만 필터를 적용하자.” 이 ‘선형 근사’를 위해 사용되는 수학적 도구가 바로 야코비안 행렬(Jacobian Matrix)이다.
이번에는 레이더(Radar)를 이용해 2차원 평면 위의 물체를 추적하는 시나리오를 다룬다. 이전 예제와 다른 점은 센서의 측정 방식이다.
시스템 상태 ($x$)와 운동 모델 ($f(x)$): 물체의 상태와 움직임은 이전 예제와 동일하게 등속도 모델을 따른다고 가정한다. 따라서 상태 벡터 $x$는 $[p_x, p_y, v_x, v_y]^T` 이고, 상태 전이 함수 $f(x)$는 선형이므로 $f(x) = Fx$ 로 표현할 수 있다.
비선형 관측 모델 ($h(x)$): 레이더는 물체의 데카르트 좌표($p_x, p_y$)를 직접 측정하지 않는다. 대신, 레이더 자신으로부터 물체까지의 거리(range)와 방위각(bearing/azimuth)을 측정한다. 상태 벡터 $x$에서 이 측정값으로 변환하는 관계는 다음과 같은 비선형 함수 $h(x)$로 표현된다.
따라서 관측 함수 $h(x)$는 다음과 같다. \(h(x) = h\left(\begin{bmatrix} p_x \\ p_y \\ v_x \\ v_y \end{bmatrix}\right) = \begin{bmatrix} \sqrt{p_x^2 + p_y^2} \\ \text{atan2}(p_y, p_x) \end{bmatrix}\) 이 함수는 더 이상 $z = Hx$ 형태의 간단한 행렬 곱으로 표현할 수 없다. 이것이 바로 EKF가 필요한 이유다.
EKF는 비선형 함수 $h(x)$를 현재 상태 추정치 $x_{est}$ 주변에서 테일러 급수 1차 전개를 통해 선형으로 근사한다. 이 근사치의 기울기 역할을 하는 것이 바로 야코비안 행렬이다. 야코비안 행렬 $H_j$는 함수 $h(x)$를 각 상태 변수($p_x, p_y, v_x, v_y$)로 편미분한 행렬이다. \(H_j = \frac{\partial h}{\partial x} \bigg|_{x=x_{est}} = \begin{bmatrix} \frac{\partial \rho}{\partial p_x} & \frac{\partial \rho}{\partial p_y} & \frac{\partial \rho}{\partial v_x} & \frac{\partial \rho}{\partial v_y} \\ \frac{\partial \phi}{\partial p_x} & \frac{\partial \phi}{\partial p_y} & \frac{\partial \phi}{\partial v_x} & \frac{\partial \phi}{\partial v_y} \end{bmatrix}\) 각 편미분을 계산하면 다음과 같다.
거리와 방위각은 속도 $v_x, v_y$에 직접적으로 의존하지 않으므로, 속도에 대한 편미분은 모두 0이다. 따라서 야코비안 행렬 $H_j$는 다음과 같다. \(H_j = \begin{bmatrix} \frac{p_x}{\rho} & \frac{p_y}{\rho} & 0 & 0 \\ -\frac{p_y}{\rho^2} & \frac{p_x}{\rho^2} & 0 & 0 \end{bmatrix}\) EKF의 핵심은, 표준 칼만 필터의 갱신 단계에서 고정된 $H$ 행렬을 사용하는 대신, 매 시간 스텝마다 현재의 상태 추정치 $x$를 이용해 이 야코비안 행렬 $H_j$를 새로 계산하여 사용한다는 점이다.
EKF와 표준 KF의 차이점을 요약하면 다음과 같다.
import numpy as np
import matplotlib.pyplot as plt
# 0. 시뮬레이션 환경 설정
dt = 0.1
n_iter = 200
# 실제 물체의 초기 상태 [px, py, vx, vy]
true_x_init = np.array([20.0, 20.0, -3.0, 0.0])
# 1. EKF 파라미터 초기화
# 초기 상태 추정치. 약간의 오차를 주어 시작
x = np.array([[21.0], [21.0], [-2.8], [0.1]]) # 4x1 벡터
# 초기 오차 공분산
P = np.diag([1.0, 1.0, 1.0, 1.0])
# 상태 전이 행렬 F (선형 등속도 모델)
F = np.array([[1, 0, dt, 0],
[0, 1, 0, dt],
,
])
# 측정 잡음 공분산 R (레이더 노이즈)
r_std = 0.3 # 거리(range) 표준편차
phi_std = 0.02 # 방위각(bearing) 표준편차 (radians)
R = np.diag([r_std**2, phi_std**2])
# 프로세스 잡음 공분산 Q
q_std = 0.1
Q = np.diag([q_std**2, q_std**2, q_std**2, q_std**2])
# 비선형 관측 함수 h(x)
def h(state_vector):
px = state_vector
py = state_vector
rho = np.sqrt(px**2 + py**2)
phi = np.arctan2(py, px)
return np.array([[rho], [phi]])
# 관측 함수의 야코비안 H_j(x)
def calculate_jacobian_H(state_vector):
px = state_vector
py = state_vector
rho_sq = px**2 + py**2
rho = np.sqrt(rho_sq)
# 0으로 나누는 것을 방지
if rho < 1e-6:
rho = 1e-6
rho_sq = rho**2
H_j = np.array([[px/rho, py/rho, 0, 0],
[-py/rho_sq, px/rho_sq, 0, 0]])
return H_j
# 데이터 저장을 위한 리스트
true_path =
measurements =
ekf_estimates =
true_x = true_x_init.reshape(4, 1)
# 2. 메인 루프: EKF 예측과 갱신
for _ in range(n_iter):
# 실제 물체 이동 (시뮬레이션용)
true_x = F @ true_x
true_path.append(true_x.flatten())
# 측정값 생성 (시뮬레이션용)
true_measurement = h(true_x)
measurement_noise = np.array([[np.random.randn() * r_std],
[np.random.randn() * phi_std]])
z = true_measurement + measurement_noise
measurements.append(z.flatten())
# --- (1) 예측 (Predict) 단계 ---
# 상태 전이 모델이 선형이므로 표준 KF와 동일
x_pred = F @ x
P_pred = F @ P @ F.T + Q
# --- (2) 갱신 (Update) 단계 ---
# 야코비안 H_j 계산
H_j = calculate_jacobian_H(x_pred)
# 잔차(Innovation) 계산 (비선형 함수 h 사용)
y = z - h(x_pred)
# 잔차의 각도(phi)를 -pi ~ pi 범위로 정규화 (중요!)
y = (y + np.pi) % (2 * np.pi) - np.pi
# 잔차 공분산(Innovation Covariance) 계산
S = H_j @ P_pred @ H_j.T + R
# 칼만 이득(K) 계산
K = P_pred @ H_j.T @ np.linalg.inv(S)
# 상태 추정치 갱신
x = x_pred + K @ y
# 오차 공분산 갱신
I = np.eye(len(x))
P = (I - K @ H_j) @ P_pred
# 결과 저장
ekf_estimates.append(x.flatten())
# NumPy 배열로 변환
true_path = np.array(true_path)
measurements = np.array(measurements)
rho_meas = measurements[:, 0]
phi_meas = measurements[:, 1]
measurements_cartesian = np.vstack((rho_meas * np.cos(phi_meas),
rho_meas * np.sin(phi_meas))).T
ekf_estimates = np.array(ekf_estimates)
# 3. 결과 시각화
plt.figure(figsize=(12, 12))
plt.plot(true_path[:, 0], true_path[:, 1], 'g-', linewidth=3, label='True Path')
plt.plot(measurements_cartesian[:, 0], measurements_cartesian[:, 1], 'r.', markersize=5, label='Measurements (Cartesian)')
plt.plot(ekf_estimates[:, 0], ekf_estimates[:, 1], 'b-', linewidth=2, label='EKF Estimate')
plt.title('Object Tracking with EKF (Radar Measurements)')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.legend()
plt.grid(True)
plt.axis('equal')
plt.show()
결과 그래프를 보면, EKF 추정치(파란 선)가 비선형적인 측정값(거리, 방위각)을 사용했음에도 불구하고 실제 경로(녹색 선)를 매우 잘 따라가는 것을 볼 수 있다. 이는 야코비안을 통한 선형 근사가 이 문제에서는 충분히 잘 작동했음을 의미한다.
하지만 EKF의 작동 방식을 더 깊이 들여다보면 그 본질적인 한계가 드러난다. 서론에서 언급했듯이, 표준 칼만 필터는 시스템의 상태가 항상 가우시안 분포를 따른다고 가정한다. 선형 변환은 가우시안 분포를 또 다른 가우시안 분포로 바꾸기 때문에 이 가정이 유지된다.
문제는 비선형 함수 $h(x)$를 통과한 가우시안 분포는 더 이상 가우시안 분포가 아니라는 점이다. 예측된 상태 $x_{pred}$의 가우시안 분포를 비선형 관측 함수 $h(x)$에 통과시키면, 결과로 나오는 측정값의 확률 분포는 바나나 모양처럼 휘거나 찌그러진 비-가우시안 형태가 된다. 칼만 필터의 갱신 단계는 두 가우시안 분포를 결합해야 하는데, 하나가 가우시안이 아니므로 계산을 진행할 수 없다.
EKF는 이 문제를 “억지로” 해결한다. 왜곡된 비-가우시안 분포를 무시하고, 그 분포와 가장 비슷한 가우시안 분포를 찾아 근사하는 것이다. 야코비안 $H_j$는 바로 이 새로운 근사 가우시안의 평균과 공분산을 계산하기 위한 도구(국소적 선형 모델)일 뿐이다.
이러한 근사 때문에 EKF는 두 가지 주요한 약점을 가진다.
EKF는 수십 년간 로켓 유도부터 로봇 공학에 이르기까지 수많은 분야에서 성공적으로 사용된 매우 실용적인 기법이다. 하지만 이러한 근본적인 한계 때문에, 비선형성이 매우 강한 시스템을 다룰 때는 무향 칼만 필터(Unscented Kalman Filter, UKF)나 파티클 필터(Particle Filter)와 같은 더 발전된 기법들이 대안으로 제시된다. EKF는 비선형 필터링 세계로 들어가는 첫 번째 관문이자 가장 중요한 디딤돌이라고 할 수 있다.
지금까지 칼만 필터의 수학적 구조와 코드 구현에 집중했다. 하지만 실제 현장에서 필터를 성공적으로 적용하기 위해서는 이론과 코드를 넘어서는 ‘튜닝(tuning)’이라는 예술의 영역을 이해해야 한다. 칼만 필터의 성능은 어떤 알고리즘을 사용했는지 만큼이나, 잡음 공분산 행렬 $Q$와 $R$을 어떻게 설정했는지에 따라 극적으로 달라진다.
$Q$와 $R$은 단순한 상수가 아니다. 이들은 필터의 동적 행동을 결정하는 가장 중요한 ‘조절 손잡이(knob)’다.
$Q$와 $R$의 수학적 정의를 넘어 그 본질적인 의미를 다시 생각해보자.
결국, 필터의 행동은 $Q$와 $R$의 상대적인 크기에 의해 결정된다. 이 둘의 비율이 칼만 이득 $K$를 조절하고, $K$가 예측과 측정 사이의 가중치를 결정하는 ‘중재자’ 역할을 한다.
백문이 불여일견이다. ‘난이도 중’에서 다뤘던 2차원 등속도 추적 예제를 가지고 $Q$와 $R$ 값을 극단적으로 바꿔보며 필터의 출력이 어떻게 변하는지 직접 확인해보자. 아래 코드는 $Q$와 $R$의 스케일을 조절하는 파라미터를 추가하여, 세 가지 다른 튜닝 시나리오를 한 번에 시뮬레이션하고 비교한다.
import numpy as np
import matplotlib.pyplot as plt
# 2D 추적 예제 코드를 기반으로 튜닝 실험 함수를 만듦
def run_kf_simulation(q_scale, r_scale):
dt = 1.0
n_iter = 100
true_x_init = np.array([0.0, 0.0, 5.0, 5.0])
# 파라미터 초기화
x = np.array([[0.0], [0.0], [0.0], [0.0]])
P = np.diag([10.0, 10.0, 1000.0, 1000.0])
F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], , ])
H = np.array([, ])
# R과 Q를 스케일링
base_r_std = 10.0
R = np.diag([(base_r_std * r_scale)**2, (base_r_std * r_scale)**2])
base_q_std = 0.1
Q = np.diag([(base_q_std * q_scale)**2] * 4)
# 시뮬레이션
true_path =
measurements =
kf_estimates =
true_x = true_x_init.reshape(4, 1)
np.random.seed(42) # 동일한 노이즈 사용
for _ in range(n_iter):
true_x = F @ true_x
true_path.append(true_x.flatten()[:2])
measurement_noise = np.random.randn(2, 1) * base_r_std
z = H @ true_x + measurement_noise
measurements.append(z.flatten())
x_pred = F @ x
P_pred = F @ P @ F.T + Q
y = z - H @ x_pred
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
x = x_pred + K @ y
I = np.eye(len(x))
P = (I - K @ H) @ P_pred
kf_estimates.append(x.flatten()[:2])
return np.array(true_path), np.array(measurements), np.array(kf_estimates)
# 세 가지 시나리오 실행
true_path, measurements, est_balanced = run_kf_simulation(q_scale=1.0, r_scale=1.0)
_, _, est_high_r = run_kf_simulation(q_scale=1.0, r_scale=10.0) # 센서 불신
_, _, est_high_q = run_kf_simulation(q_scale=10.0, r_scale=1.0) # 모델 불신
# 결과 시각화
plt.figure(figsize=(15, 10))
plt.plot(true_path[:, 0], true_path[:, 1], 'g-', linewidth=4, label='True Path')
plt.plot(measurements[:, 0], measurements[:, 1], 'kx', markersize=4, label='Measurements')
plt.plot(est_balanced[:, 0], est_balanced[:, 1], 'b-', linewidth=2, label='Balanced Filter (Q_scale=1, R_scale=1)')
plt.plot(est_high_r[:, 0], est_high_r[:, 1], 'm--', linewidth=2, label='Trust Model (High R_scale)')
plt.plot(est_high_q[:, 0], est_high_q[:, 1], 'c:', linewidth=2, label='Trust Measurement (High Q_scale)')
plt.title('Kalman Filter Tuning: The Impact of Q and R')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.legend()
plt.grid(True)
plt.axis('equal')
plt.show()
위 코드를 실행하면 나타나는 그래프는 $Q$와 $R$ 튜닝의 효과를 명확하게 보여준다.
이 실험은 $Q$와 $R$ 튜닝이 단순한 숫자놀음이 아니라, 필터의 근본적인 ‘성격’을 결정하는 과정임을 보여준다. 그렇다면 실전에서 튜닝이 잘 되었는지는 어떻게 정량적으로 판단할 수 있을까? 그 해답은 잔차(residual) 분석에 있다.
잔차 $y = z - Hx_{pred}$는 측정값에서 모델 예측을 뺀 값, 즉 모델이 예측하지 못한 ‘놀라움(surprise)’의 정도를 나타낸다. 이론적으로, 완벽하게 모델링되고 튜닝된 칼만 필터의 잔차는 그 원인이 순수한 측정 잡음 $R$ 뿐이어야 한다. 측정 잡음은 평균이 0이고 자기상관이 없는 백색 잡음(white noise)으로 가정되므로, 잔차 또한 백색 잡음의 특성을 보여야 한다.
잔차 분석은 두 가지 핵심 속성을 확인하는 과정이다.
아래 코드는 균형 잡힌 필터의 잔차를 수집하여 그 분포와 자기상관 함수를 시각화하는 방법을 보여준다.
# 잔차 분석을 위한 코드
def run_and_analyze_residuals(q_scale, r_scale):
#... 이전 시뮬레이션 코드와 동일...
residuals = # 잔차 저장용 리스트
# 시뮬레이션 루프 내
for _ in range(n_iter):
#... 예측 및 갱신...
y = z - H @ x_pred
#...
residuals.append(y.flatten())
return np.array(residuals)
# 균형 잡힌 필터의 잔차 수집
residuals = run_and_analyze_residuals(q_scale=1.0, r_scale=1.0)
# 잔차 분석 시각화
plt.figure(figsize=(12, 6))
# 잔차 시계열 플롯
plt.subplot(1, 2, 1)
plt.plot(residuals[:, 0], label='Residual X')
plt.plot(residuals[:, 1], label='Residual Y')
plt.title('Residuals over Time')
plt.xlabel('Time Step')
plt.ylabel('Error')
plt.grid(True)
plt.legend()
# 자기상관 함수 플롯 (X축 잔차)
plt.subplot(1, 2, 2)
plt.acorr(residuals[:, 0], maxlags=20, usevlines=True, normed=True, lw=2)
plt.title('Autocorrelation of X-Residual')
plt.xlabel('Lag')
plt.ylabel('Correlation')
plt.grid(True)
plt.tight_layout()
plt.show()
잘 튜닝된 필터의 경우, 왼쪽 그래프의 잔차는 0을 중심으로 무작위로 흩어져 있어야 하며, 오른쪽 자기상관 그래프는 지연(Lag) 0에서만 1의 값을 갖고 나머지 지연에서는 0에 가까운 값을 보여야 한다. 만약 다른 지연에서도 높은 상관관계가 나타난다면, $Q$ 값을 높여 모델의 불확실성을 더 크게 반영해야 한다는 신호일 수 있다.
이처럼 잔차 분석은 튜닝을 ‘감’의 영역에서 ‘공학’의 영역으로 끌어오는 강력한 진단 도구다.
이 문서는 파이썬 구현을 통해 칼만 필터의 핵심을 단계적으로 탐험했다.
이 모든 과정의 기저에는 ‘가우시안 가정’이라는 하나의 원칙이 있었다. 이 가정이 성립할 때 표준 칼만 필터는 최적의 성능을 보이며, 이 가정이 깨질 때 EKF와 같은 근사 기법이 필요해진다.
마지막으로, $Q$와 $R$ 튜닝이 필터의 ‘성격’을 결정하는 핵심 과정이며, 잔차 분석이 그 튜닝의 적절성을 판단하는 객관적인 기준임을 확인했다. 단순히 코드를 복사-붙여넣기 하는 수준을 넘어, 자신의 문제에 맞게 필터를 설계하고, 튜닝하고, 그 한계를 명확히 인지하는 것이 진정한 전문가로 가는 길이다.
여기서 더 나아가고 싶다면, EKF의 선형화 오차를 개선한 무향 칼만 필터(Unscented Kalman Filter, UKF)나 가우시안 가정 자체에서 벗어나 더욱 복잡한 분포를 다루는 파티클 필터(Particle Filter)를 탐구해볼 수 있다. EKF는 그 여정을 시작하는 가장 중요하고 실용적인 첫걸음이 될 것이다.