Day 33: Python으로 간단한 EKF-SLAM 구현

1. 서론

앞선 글에서 우리는 **EKF-SLAM(Extended Kalman Filter SLAM)**의 수학적 원리를 살펴보았습니다.
이번 글에서는 이 개념을 실제로 구현해보며, Python을 이용해 간단한 EKF-SLAM 시뮬레이션을 만들어 보겠습니다.

수학적 이론을 눈으로만 보는 것보다, 직접 실행 가능한 코드를 통해 확인하면 SLAM의 구조와 동작 원리를 훨씬 직관적으로 이해할 수 있습니다.

Python으로 간단한 EKF-SLAM 구현

---

2. EKF-SLAM 구현 개요

EKF-SLAM 구현은 크게 세 단계로 나눌 수 있습니다.

1. 환경 정의: 로봇과 랜드마크(벽, 기둥, 물체 등)를 설정
2. 예측 단계(Prediction): 로봇의 이동 모델을 기반으로 위치 추정
3. 갱신 단계(Update): 센서 관측값을 활용해 추정치 보정

➡ 핵심은 “예측 → 관측 → 보정”의 반복 구조입니다.

---

3. Python 코드 예제

아래 코드는 매우 단순화된 EKF-SLAM 구조입니다.
(실제 응용에서는 LiDAR, 카메라 등 다양한 센서 융합이 필요하지만, 학습용으로 기본 구조만 구현합니다.)

python

 

import numpy as np
import matplotlib.pyplot as plt

# 초기 상태 [x, y, 방향]
state = np.array([0.0, 0.0, 0.0])
P = np.eye(3) * 0.1  # 초기 공분산

# 제어 입력 (속도, 각속도)
u = np.array([1.0, 0.1])

# 이동 모델
def motion_model(state, u, dt=1.0):
    x, y, theta = state
    v, w = u
    x_new = x + v * np.cos(theta) * dt
    y_new = y + v * np.sin(theta) * dt
    theta_new = theta + w * dt
    return np.array([x_new, y_new, theta_new])

# 예측 단계
def predict(state, P, u):
    F = np.eye(3)
    Q = np.eye(3) * 0.01
    state_pred = motion_model(state, u)
    P_pred = F @ P @ F.T + Q
    return state_pred, P_pred

# 관측 (랜덤 센서값 예시)
def observe(state):
    x, y, _ = state
    z = np.array([x + np.random.randn()*0.5, y + np.random.randn()*0.5])
    return z

# 보정 단계
def update(state, P, z):
    H = np.array([[1, 0, 0],
                  [0, 1, 0]])
    R = np.eye(2) * 0.5
    z_pred = H @ state
    y = z - z_pred
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    state_upd = state + K @ y
    P_upd = (np.eye(3) - K @ H) @ P
    return state_upd, P_upd

# 시뮬레이션
trajectory = []
for _ in range(20):
    state, P = predict(state, P, u)
    z = observe(state)
    state, P = update(state, P, z)
    trajectory.append(state[:2])

# 시각화
trajectory = np.array(trajectory)
plt.plot(trajectory[:,0], trajectory[:,1], marker='o')
plt.title("EKF-SLAM 간단 시뮬레이션")
plt.xlabel("X")
plt.ylabel("Y")
plt.grid(True)
plt.show()

➡ 위 코드를 실행하면, 로봇이 이동하면서 관측값을 보정하고 점차 더 안정적인 위치 추정 경로를 만들어내는 과정을 확인할 수 있습니다.

---

4. 실습 포인트

• 코드 실행 시 마다 랜덤 노이즈가 달라, 매번 경로가 조금씩 다르게 나타납니다.
• Q(프로세스 잡음)와 R(관측 잡음)의 값을 바꾸면 정확도와 안정성이 달라집니다.
• EKF-SLAM은 예측 모델과 센서 모델의 균형이 중요합니다.

---

5. EKF-SLAM 구현의 의미

이 간단한 예제는 실제 자율주행 시스템에서의 복잡한 SLAM의 축소판입니다.

• 실제 차량에서는 IMU, GPS, LiDAR, 카메라 등 다양한 센서를 융합
• Python 구현은 학습용 프로토타입이지만, 핵심 알고리즘의 구조를 이해하는 좋은 시작점

반응형

---

6. 마무리

오늘은 Python을 활용해 EKF-SLAM을 간단히 구현해 보았습니다.
비록 단순화된 코드지만, 이 과정을 통해 EKF-SLAM의 “예측 → 관측 → 보정” 메커니즘을 직접 체험할 수 있었습니다.

---

🔜 다음 글 소개

다음 글에서는 **[Day 34: FastSLAM 개념 (입자 필터 기반 SLAM)]**을 다룹니다.
👉 FastSLAM은 EKF-SLAM의 한계를 극복하기 위해 제안된 알고리즘으로, 입자 필터(Particle Filter)를 기반으로 동작합니다. 이를 통해 더 넓은 환경에서도 빠르고 효율적인 SLAM이 가능합니다.