[SLAM] 3일차: EKF-SLAM (Extended Kalman Filter SLAM) 이론과 실습

EKF-SLAM (Extended Kalman Filter SLAM) 이론과 실습

이번 단계에서는 EKF-SLAM의 기본 이론을 배우고, 이를 적용한 간단한 실습을 진행하겠습니다.

1. EKF-SLAM 이론

Extended Kalman Filter (EKF)는 비선형 시스템의 상태 추정을 위해 사용되는 필터입니다. EKF-SLAM은 로봇의 위치와 환경의 특징을 동시에 추정합니다.

EKF-SLAM의 주요 단계:

1. 예측 단계 (Prediction Step): 로봇의 새로운 상태를 예측
2. 갱신 단계 (Update Step): 센서 데이터를 이용해 상태 추정 값을 갱신

예측 단계 수식:

• 상태 예측:여기서 x^k\hat{x}_kx^k​는 예측된 상태, fff는 로봇의 운동 모델, uku_kuk​는 제어 입력, wkw_kwk​는 프로세스 노이즈입니다.
• x^k=f(x^k−1,uk)+wk\hat{x}_k = f(\hat{x}_{k-1}, u_k) + w_kx^k​=f(x^k−1​,uk​)+wk​
• 공분산 예측:여기서 PkP_kPk​는 상태 공분산 행렬, Fk−1F_{k-1}Fk−1​은 상태 전이 행렬, QkQ_kQk​는 프로세스 노이즈 공분산 행렬입니다.
• Pk=Fk−1Pk−1Fk−1T+QkP_k = F_{k-1} P_{k-1} F_{k-1}^T + Q_kPk​=Fk−1​Pk−1​Fk−1T​+Qk​

갱신 단계 수식:

• 칼만 이득 계산:여기서 KkK_kKk​는 칼만 이득, HkH_kHk​는 측정 모델 행렬, RkR_kRk​는 측정 노이즈 공분산 행렬입니다.
• Kk=PkHkT(HkPkHkT+Rk)−1K_k = P_k H_k^T (H_k P_k H_k^T + R_k)^{-1}Kk​=Pk​HkT​(Hk​Pk​HkT​+Rk​)−1
• 상태 업데이트:여기서 zkz_kzk​는 실제 측정 값, hhh는 측정 모델입니다.
• x^k=x^k+Kk(zk−h(x^k))\hat{x}_k = \hat{x}_k + K_k (z_k - h(\hat{x}_k))x^k​=x^k​+Kk​(zk​−h(x^k​))
• 공분산 업데이트:여기서 III는 단위 행렬입니다.
• Pk=(I−KkHk)PkP_k = (I - K_k H_k) P_kPk​=(I−Kk​Hk​)Pk​

 

2. 실습: 간단한 EKF-SLAM 구현

Python을 이용하여 EKF-SLAM을 간단하게 구현해보겠습니다.

 

1. 필요한 라이브러리 설치

pip install numpy scipy matplotlib

 

2. EKF-SLAM 구현 코드

import numpy as np
import matplotlib.pyplot as plt

def prediction_step(mu, sigma, u, G, R):
    mu = np.dot(G, mu) + u
    sigma = np.dot(G, np.dot(sigma, G.T)) + R
    return mu, sigma

def update_step(mu, sigma, z, H, Q):
    S = np.dot(H, np.dot(sigma, H.T)) + Q
    K = np.dot(sigma, np.dot(H.T, np.linalg.inv(S)))
    mu = mu + np.dot(K, (z - np.dot(H, mu)))
    sigma = sigma - np.dot(K, np.dot(H, sigma))
    return mu, sigma

# 초기 상태
mu = np.array([0, 0, 0])  # x, y, theta
sigma = np.eye(3) * 0.1

# 예측 단계
u = np.array([1, 0, 0.1])  # x, y, theta
G = np.eye(3)
R = np.eye(3) * 0.1
mu, sigma = prediction_step(mu, sigma, u, G, R)

# 갱신 단계
z = np.array([1.1, 0.1])  # 실제 측정 값
H = np.array([[1, 0, 0], [0, 1, 0]])
Q = np.eye(2) * 0.1
mu, sigma = update_step(mu, sigma, z, H, Q)

print("추정된 상태:", mu)
print("추정된 공분산 행렬:", sigma)

 

3. 결과 확인

위 코드를 실행하면 로봇의 추정된 상태와 공분산 행렬이 출력됩니다. 이 과정을 통해 EKF-SLAM의 예측 및 갱신 단계를 이해할 수 있습니다.

 

EKF-SLAM의 기본 이론과 간단한 구현을 통해 SLAM의 기초 개념을 이해할 수 있습니다. 이제 더 복잡한 예제를 통해 SLAM을 심화 학습할 준비가 되었습니다