Lecture 5. 칼만 필터 (2D)

평면 위 로봇의 위치를 추정해봅시다. 1D의 모든 식이 행렬 버전으로 자연스럽게 확장됩니다.

1. 왜 2D인가? — 측위 문제

모바일 로봇과 자동차는 평면 위를 움직입니다. 위치 $(x, y)$를 알아야 지도와 비교하고 장애물을 피할 수 있습니다. 하지만 한 종류의 센서만으로는 부족합니다.

센서	장점	단점
휠 오도메트리 (Wheel Odometry)	고주파, 단기 정확	드리프트 누적
관성 센서 (IMU)	외부 신호 불필요	적분 시 오차 누적
GPS	절대 위치, 드리프트 없음	저주파, 잡음, 실내·운하 약함
LiDAR / 비전	지도와 매칭 가능	처리 부담

"드리프트는 적은데 절대 위치가 없는" 센서와 "절대 위치는 있지만 잡음이 큰" 센서를 합치는 것이 칼만 필터의 전형적인 임무입니다.

2. 2D 상태 벡터와 모델

가장 흔한 모델은 등속도 가정의 4차원 상태입니다.

$$ \mathbf x = \begin{bmatrix} x \\ y \\ \dot x \\ \dot y \end{bmatrix} $$

이산 시간 전이 행렬:

$$ A_d = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

가속도가 입력일 때:

$$ B_d = \begin{bmatrix} \tfrac12 \Delta t^2 & 0 \\ 0 & \tfrac12 \Delta t^2 \\ \Delta t & 0 \\ 0 & \Delta t \end{bmatrix} $$

1D 칼만 필터의 모든 식이 그대로 행렬 버전이 됩니다. 차이는 단지 스칼라가 행렬이 된 것뿐입니다.

3. 공분산 행렬과 타원

1D에서 불확실성은 한 숫자 $P$로 충분했습니다. 2D 위치 $(x,y)$에서는 두 가지가 더 필요합니다.

$$ P = \begin{bmatrix} \sigma_x^2 & \sigma_{xy} \\ \sigma_{xy} & \sigma_y^2 \end{bmatrix} $$

$\sigma_x^2, \sigma_y^2$ — 각 축의 분산
$\sigma_{xy}$ — 두 축 사이의 공분산. 양수면 한 쪽이 커질 때 다른 쪽도 커지는 경향.

이 2×2 행렬의 고유벡터·고유값이 곧 타원의 주축과 반지름입니다. 그래서 2D 추정의 불확실성은 타원으로 시각화할 수 있습니다.

1σ 타원: 진짜 위치가 그 안에 들어 있을 확률 약 39%
2σ 타원: 약 86%
3σ 타원: 약 99%

🎮 인터랙티브: 공분산 타원 만들어보기

공분산 행렬의 세 원소를 슬라이더로 바꾸면 그에 대응하는 가우시안 타원이 즉시 그려집니다. 비대각 항(공분산)을 키우면 타원이 기울어지는 것을 보세요.

σ²ₓ 2 σ²ᵧ 1 σₓᵧ 0.5

4. 다변량 가우시안 합치기

두 다변량 가우시안 $\mathcal N(\boldsymbol\mu_1, \Sigma_1)$과 $\mathcal N(\boldsymbol\mu_2, \Sigma_2)$를 합치면:

$$ \Sigma_\text{new} = (\Sigma_1^{-1} + \Sigma_2^{-1})^{-1} $$ $$ \boldsymbol\mu_\text{new} = \Sigma_\text{new}(\Sigma_1^{-1}\boldsymbol\mu_1 + \Sigma_2^{-1}\boldsymbol\mu_2) $$

1D의 식 $1/\sigma^2 = 1/\sigma_1^2 + 1/\sigma_2^2$가 행렬 역함수로 일반화된 모습입니다. 결과 공분산은 두 입력 공분산보다 항상 "작다"(고유값이 모두 작거나 같다)는 사실은 그대로입니다.

5. 예제 — GPS 위치 융합

등속도 모델 + GPS 위치 측정의 가장 흔한 조합입니다.

상태 $\mathbf x = [x, y, \dot x, \dot y]^\top$ (4차원)
측정 $\mathbf z = [x_\text{GPS}, y_\text{GPS}]^\top$ (2차원)
$H = [I_2,\;0_{2\times 2}]$ — 측정은 위치만 본다
$R = \sigma_\text{GPS}^2 I_2$ — GPS 잡음 (등방성 가정)

🎮 인터랙티브: 2D 칼만 필터 측위

로봇(녹색)이 원을 그리며 움직이고, GPS(빨강)는 노이즈가 큽니다. 칼만 필터(파랑)와 공분산 타원이 시간에 따라 어떻게 갱신되는지 보세요.

GPS 잡음 R 10 모델 잡음 Q 0.05

6. 거리 기반 측정 — 비선형 문제

지금까지의 측정 모델 $\mathbf z = H\mathbf x$는 선형이었습니다. 그런데 LiDAR나 UWB 비콘처럼 "랜드마크까지의 거리"를 재는 센서는 비선형입니다.

$$ z = h(\mathbf x) = \sqrt{(x - x_L)^2 + (y - y_L)^2} + v $$

제곱근이 들어가니 칼만 필터의 가정($\mathbf z = H\mathbf x$)이 깨집니다. 해결책은 매 스텝 측정 함수를 1차 테일러 전개로 선형화하는 것 — EKF(Extended Kalman Filter)입니다.

7. EKF — 확장 칼만 필터

측정 행렬 $H$ 자리에 야코비안을 넣습니다.

$$ H_k = \left.\frac{\partial h}{\partial \mathbf x}\right|_{\hat{\mathbf x}_{k|k-1}} $$

거리 측정의 경우:

$$ H = \frac{1}{d}[\,(x - x_L),\;(y - y_L),\;0,\;0\,], \quad d = \sqrt{(x-x_L)^2+(y-y_L)^2} $$

나머지 식(예측·갱신)은 표준 칼만 필터와 거의 같습니다. EKF는 1차 근사이므로 비선형성이 강하면 발산할 수 있지만, 자율 주행과 SLAM에서 가장 널리 쓰이는 추정기입니다.

🎮 인터랙티브: 거리 측정으로 위치 추정 (삼각측량 직감)

3개의 랜드마크(빨강) 각각에서 잡음 섞인 거리 측정값이 옵니다. 각 측정은 원 모양 분포가 되고, 세 원이 겹치는 곳에 진짜 위치가 있습니다. 마우스를 움직여 진짜 로봇 위치를 옮겨보세요.

측정 잡음 σ 5

8. UKF와 파티클 필터

EKF의 1차 근사가 부족할 때 두 대안이 있습니다.

UKF (Unscented KF)

야코비안 대신 평균 주위에 정해진 규칙으로 시그마 포인트들을 흩뿌립니다. 각 점을 비선형 함수에 통과시킨 뒤 다시 가중 평균/공분산을 계산합니다.

미분이 필요 없음 → 함수가 미분 불가능하거나 코드로만 주어져도 OK
EKF보다 비선형성에 더 강함
계산량은 EKF의 약 2배

파티클 필터 (Particle Filter)

분포 자체를 수많은 점(파티클)의 집합으로 표현합니다.

각 파티클을 모델로 한 스텝 진행 (예측)
측정과 일치하는 정도에 따라 파티클에 가중치 부여
가중치에 비례해 다시 샘플링 (resampling)

장점: 분포가 가우시안이 아니어도 OK, 다중 가설(multi-modal)도 표현 가능. 단점: 파티클 수가 많아야 정확하므로 계산량이 큼.

🎮 인터랙티브: 파티클 필터로 1D 위치 추정

1차원 직선 위에서 진짜 위치(녹색)를 200개 파티클(파랑 점들)이 추정합니다. 측정이 들어올 때마다 진짜 위치 근처의 파티클이 살아남고, 멀리 있는 파티클은 제거됩니다.

측정 잡음 σ 3

코드 예제 (Python · NumPy)

import numpy as np

dt = 0.1
A = np.array([[1,0,dt,0],[0,1,0,dt],[0,0,1,0],[0,0,0,1]])
H = np.array([[1,0,0,0],[0,1,0,0]])    # GPS는 (x,y)만 측정
Q = 0.01*np.eye(4)
R = 4.0*np.eye(2)

x = np.zeros(4)
P = np.eye(4)*10

def kf_step(z):
    global x, P
    # 1) 예측
    x = A @ x
    P = A @ P @ A.T + Q
    # 2) 갱신
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    x = x + K @ (z - H @ x)
    P = (np.eye(4) - K @ H) @ P
    return x

# EKF 버전: 측정 함수가 비선형이면 야코비안 사용
def h(x, landmark):
    return np.array([np.linalg.norm(x[:2] - landmark)])

def H_jacobian(x, landmark):
    dx = x[:2] - landmark
    d  = np.linalg.norm(dx)
    return np.array([[dx[0]/d, dx[1]/d, 0, 0]])

📖 더 깊이 공부하기

Probabilistic Robotics (Thrun, Burgard, Fox) — 2~4장이 칼만/EKF/파티클 필터의 정석.
PythonRobotics Localization — EKF, UKF, Particle Filter, Histogram Filter가 모두 짧은 파이썬 구현으로 있습니다.
Kalman Filter from the Ground Up — Multi-dimensional
Underactuated Robotics 12장 — State Estimation

Study Page