센서와 상태 추정: 칼만 필터 손계산과 SLAM 개요

센서와 상태 추정: 칼만 필터 손계산과 SLAM 개요

로봇은 자기 위치와 자세를 직접 "아는" 것이 아니라, 잡음 섞인 센서 값과 움직임 예측을 섞어 추정 합니다. 이 수업은 로봇에 붙는 센서의 종류, 두 측정을 섞는 원리(센서 융합), 1차원 칼만 필터의 예측 → 보정 두 단계를 손으로 계산하고, 확장 칼만 필터(EKF)와 SLAM(동시적 위치 추정 및 지도 작성)이 무엇을 푸는지까지 정리합니다.

들어가기 전에

• 확률과 통계 최소한 — 평균 · 분산 · 정규분포 · 베이즈 정리. 칼만 필터는 "정규분포 두 개를 베이즈 정리로 곱하는 계산"을 반복하는 것입니다.
• 로봇 기구학 첫 수업 (모던 로보틱스) — 관절 각도 · 손끝 자세 · 자코비안. EKF의 선형화에도 자코비안이 그대로 나옵니다.

한눈에 보기

• 로봇에 쓰는 주요 센서(엔코더 · IMU · 카메라 · 깊이 카메라 · 라이다 · 힘/토크 센서)가 각각 무엇을 재고 어떤 약점이 있는지 압니다.
• 분산이 다른 두 측정을 섞을 때 "더 믿을 만한 쪽에 가중치를 더 준다"는 원리를 식과 숫자로 확인합니다.
• 칼만 필터의 예측(time update) · 보정(measurement update) 다섯 줄을 1차원 예로 세 걸음 손계산합니다.
• 비선형 시스템에서 EKF가 자코비안으로 선형화한다는 점을 압니다.
• SLAM이 왜 "닭이 먼저냐 달걀이 먼저냐" 문제인지, 루프 닫기(loop closure)가 왜 중요한지 설명할 수 있습니다.

1. 로봇의 감각: 센서 종류와 약점

직관(해설용 비유). 눈을 감고 방을 걸을 때 사람은 "몇 걸음 걸었나"(자기 움직임 감각)와 가끔 손에 닿는 벽(바깥 측정)을 섞어 위치를 짐작합니다. 로봇도 똑같이 자기 감각 센서(proprioceptive) 와 외부 감각 센서(exteroceptive) 를 섞습니다.

왜 섞나. 위 표에서 보듯 모든 센서에는 약점이 있고, 약점의 종류가 서로 다릅니다. 엔코더 · IMU는 짧은 시간에는 정확하지만 오래 쓰면 오차가 쌓이고, 카메라 · 라이다는 순간순간은 시끄럽거나 가끔 실패하지만 오차가 쌓이지 않습니다. 둘을 섞으면 서로의 약점을 메울 수 있습니다. 이것이 센서 융합(sensor fusion) 입니다.

모방 학습 수업(LeRobot)의 데이터셋에서 observation.state(관절 각도) 와 observation.images.*(카메라) 가 따로 기록되는 것도, 정책이 바로 이 두 종류의 감각을 함께 입력으로 받기 때문입니다.

2. 두 측정 섞기: 분산의 역수로 가중 평균

직관. 같은 물건의 거리를 두 사람이 쟀습니다. 한 사람은 대충(오차 큼), 한 사람은 꼼꼼하게(오차 작음) 쟀다면, 꼼꼼한 쪽 값에 더 가까운 숫자를 믿는 것이 자연스럽습니다.

단계.

1. 측정 1: 평균 $\mu_1$, 분산 $\sigma_1^2$ 인 정규분포로 봅니다.
2. 측정 2: 평균 $\mu_2$, 분산 $\sigma_2^2$.
3. 두 분포를 곱하면(베이즈 정리, 사전 분포가 평평할 때) 다시 정규분포가 되고, 평균과 분산은 다음과 같습니다.

숫자로 해 보기(해설용). 초음파 센서는 벽까지 $\mu_1 = 10$ m($\sigma_1^2 = 4$), 라이다는 $\mu_2 = 12$ m($\sigma_2^2 = 1$) 라고 했습니다.

• $\mu = (1\times10 + 4\times12)/(4+1) = 58/5 = 11.6$ m
• $\sigma^2 = 4\times1/5 = 0.8$

분산이 작은 라이다 쪽(12)에 가깝게 끌려갔고, 섞은 결과의 분산 0.8은 라이다 하나(1)보다도 작습니다. 측정을 하나 더 얻으면 불확실성은 줄어들 뿐 늘지 않는다 는 점이 핵심입니다.

간단한 실전판: 상보 필터(complementary filter). IMU로 기울기를 잴 때 자주 쓰는 방법입니다. 자이로 적분값(짧게는 정확, 길게는 흘러감)에 큰 가중치 $\alpha$, 가속도계로 구한 기울기(시끄럽지만 흘러가지 않음)에 작은 가중치 $1-\alpha$ 를 줍니다.

숫자로 해 보기(해설용). $\theta_{t-1}=10^\circ$, 자이로 $\omega = 5^\circ/\text{s}$, $\Delta t = 0.01$ s, 가속도계 기울기 $12^\circ$, $\alpha = 0.98$ 이면 $0.98\times10.05 + 0.02\times12 = 10.089^\circ$ 입니다. 매 순간은 자이로를 따르되, 가속도계가 천천히 드리프트를 잡아 줍니다. $\alpha$ 는 고정값이라 간단하지만, 상황에 따라 가중치를 스스로 정하지는 못합니다. 그 가중치를 매 걸음 분산으로 계산해 주는 것이 칼만 필터입니다.

3. 칼만 필터: 예측하고, 보정하고

직관. 칼만 필터는 두 박자를 반복합니다. ① 예측: "아까 여기 있었고 1 m/s로 움직이라고 명령했으니 지금은 대략 여기겠지" — 움직임 모델로 위치를 옮기고, 그만큼 불확실성(분산)을 키웁니다. ② 보정: 센서 값이 들어오면 2절처럼 예측과 측정을 분산에 따라 섞고, 불확실성을 줄입니다. Welch · Bishop 보고서는 이 둘을 시간 갱신(time update, "predict")과 측정 갱신(measurement update, "correct")이라 부릅니다.

모델. 상태 $x_k$ 와 측정 $z_k$ 가 선형이고 잡음이 정규분포라고 가정합니다.

예측(시간 갱신).

보정(측정 갱신).

보고서가 짚는 두 극단이 직관을 줍니다. 센서 잡음 $R$ 이 0에 가까우면 이득이 커져 측정을 거의 그대로 믿고, 사전 불확실성 $P_k^-$ 가 0에 가까우면 이득이 0이 되어 측정을 무시합니다.

숫자로 해 보기(해설용, 1차원). 레일 위 로봇이 매초 1 m씩 가라는 명령을 받습니다. $A = 1$, $B u = 1$, $H = 1$, $Q = 0.1$, $R = 0.5$, 처음 추정 $\hat x_0 = 0$, $P_0 = 1$ 로 둡니다. 라이다 측정은 $z_1 = 1.2$, $z_2 = 2.0$, $z_3 = 3.3$ 이 들어왔습니다.

(손계산은 파이썬으로 다시 확인했습니다.) 처음에는 자기 추정이 불확실해($P_0=1$) 측정을 많이 믿고($K\approx0.69$), 걸음이 쌓일수록 $P$ 가 줄어 이득이 0.4 근처로 내려옵니다. $Q$ 와 $R$ 이 고정이면 $P$ 와 $K$ 는 곧 일정한 값으로 안정되는데(보고서도 이 점을 짚습니다), 이 예에서는 $P\approx0.179$, $K\approx0.358$ 로 수렴합니다.

왜 이렇게 하나. 매 걸음 지난 측정을 모두 다시 보지 않고, "평균 하나 + 분산 하나"(다차원이면 벡터 하나 + 공분산 행렬 하나)만 들고 다니며 갱신합니다. 선형 · 가우시안 가정 아래에서는 이것이 평균 제곱 오차를 최소로 하는 추정이고, 계산량이 일정해 로봇처럼 실시간으로 돌려야 하는 곳에 맞습니다.

$Q$ 와 $R$ 정하기. 보고서는 $R$ 은 센서를 미리 여러 번 재서 분산을 구할 수 있지만, $Q$ 는 직접 관찰하기 어려워 대개 조정(tuning)으로 정한다고 설명합니다. 보고서의 예제(잡음 섞인 전압으로 일정한 값을 추정)는 $Q = 10^{-5}$, $\hat x_0 = 0$, $P_0 = 1$ 에서 시작하고, 측정 잡음을 0.1 V RMS로 둡니다. $P_0 \neq 0$ 이기만 하면 처음 값 선택은 크게 중요하지 않다고 덧붙입니다.

4. 비선형일 때: 확장 칼만 필터(EKF)

로봇의 움직임은 대부분 비선형입니다. 바퀴 로봇이 방향 $\phi$ 로 $v$ 만큼 가면 $x \leftarrow x + v\cos\phi\,\Delta t$ 처럼 삼각함수가 들어갑니다. 측정도 "랜드마크까지의 거리 $\sqrt{\Delta x^2+\Delta y^2}$" 처럼 비선형입니다.

EKF는 현재 추정 근처에서 비선형 함수 $f$ · $h$ 를 편미분 행렬(자코비안) 로 선형화해, 3절 식의 $A$ · $H$ 자리에 넣습니다. 예측 상태 자체는 비선형 함수 $f$ 로 계산하고, 공분산 계산에만 자코비안을 씁니다. Welch · Bishop 보고서는 EKF가 선형화로 베이즈 규칙의 최적성을 근사할 뿐이라는 한계도 분명히 적습니다. 비선형 변환을 거치면 분포가 더는 정규분포가 아니기 때문입니다. 이 한계를 줄이려는 변형으로 무향 칼만 필터(UKF)가, 분포를 점(입자)들로 나타내는 방법으로 입자 필터가 쓰입니다.

5. SLAM: 지도도 없고 위치도 모를 때

직관(해설용 비유). 처음 가 본 건물에서 지도 없이 돌아다니며 동시에 지도를 그린다고 상상해 봅니다. 지도를 정확히 그리려면 내 위치를 알아야 하고, 내 위치를 알려면 지도가 있어야 합니다. 이 순환 문제가 SLAM(Simultaneous Localization and Mapping) 입니다. Durrant-Whyte · Bailey 튜토리얼은 SLAM을 "모르는 환경의 모르는 위치에서 출발한 로봇이 일관된 지도를 만들면서 동시에 그 지도 안에서 자기 위치를 정하는 문제"로 정의하고, 이를 확률 문제로 씁니다.

단계(개념).

1. 상태 벡터에 로봇 자세와 랜드마크 위치들 을 함께 넣습니다.
2. 움직이면 로봇 자세의 불확실성이 커집니다(3절의 예측).
3. 랜드마크를 보면 로봇 자세와 그 랜드마크 위치가 함께 보정됩니다. 둘의 오차가 서로 얽혀(상관) 있다는 점이 SLAM의 핵심입니다.
4. 오래 돌아다닌 뒤 예전에 본 곳을 다시 알아보면(루프 닫기), 그동안 쌓인 오차가 한꺼번에 줄어듭니다.

ORB-SLAM 논문(Mur-Artal · Montiel · Tardós, 2015)은 같은 특징점을 추적 · 지도 작성 · 재위치 추정 · 루프 닫기에 모두 쓰는 단안 카메라 SLAM으로, 실내외 · 크고 작은 환경에서 실시간으로 동작한다고 보고합니다.

12345678flowchart LR
  U[모터 명령 u] --> P[예측: x̂⁻ = A·x̂ + B·u<br/>P⁻ = A·P·Aᵀ + Q]
  P --> K[칼만 이득 K]
  Z[센서 측정 z<br/>라이다 · 카메라 · IMU] --> C[보정: x̂ = x̂⁻ + K·(z − H·x̂⁻)<br/>P = (I − K·H)·P⁻]
  K --> C
  C --> E[현재 상태 추정<br/>위치 · 자세]
  E --> CTRL[제어기 · 학습된 정책]
  E --> P

학습 기반 로봇과의 관계. RT-2 · OpenVLA 같은 VLA 모델은 카메라 영상을 바로 입력으로 받아 행동을 내므로, 정책 내부에서 상태 추정을 "암묵적으로" 배웁니다. 그래도 실제 시스템에는 관절 엔코더 필터링, 이동 로봇의 위치 추정(SLAM), 안전 정지 판단처럼 고전 추정이 맡는 층이 여전히 남아 있습니다. 둘은 대체 관계라기보다 층이 다른 부품입니다.

연습 문제

1. 두 측정 $\mu_1 = 5$($\sigma_1^2 = 1$), $\mu_2 = 7$($\sigma_2^2 = 1$) 을 섞으면?
   • 풀이: 분산이 같으므로 단순 평균 $6$, 분산은 $1\times1/2 = 0.5$ 입니다.
2. 3절 예에서 $R$ 을 0.5 대신 $0.05$ 로 바꾸면 첫 걸음의 $K$ 와 $\hat x_1$ 은?
   • 풀이: $K = 1.1/(1.1+0.05) \approx 0.9565$, $\hat x_1 = 1 + 0.9565\times0.2 \approx 1.1913$ 입니다. 센서를 거의 그대로 믿습니다.
3. 3절 예에서 측정이 한 번 빠져 보정을 건너뛰면 그 걸음의 $P$ 는 어떻게 되나요?
   • 풀이: 예측만 하므로 $P = P^- = P_{k-1} + Q$ 로 커지기만 합니다. 측정이 없는 동안 불확실성은 계속 늘어납니다.
4. 상보 필터에서 $\alpha = 1$ 로 두면 어떤 문제가 생기나요?
   • 풀이: 가속도계 정보를 전혀 쓰지 않아 자이로 적분의 드리프트가 그대로 쌓입니다.

스스로 점검

1. 칼만 이득 $K$ 가 커지는 경우는 언제인가요?
   • 답: 센서 잡음 $R$ 이 작거나, 지금 추정의 불확실성 $P^-$ 가 클 때입니다. 그럴수록 측정을 더 믿습니다.
2. EKF가 일반 칼만 필터와 다른 점은?
   • 답: 비선형 움직임 · 측정 함수를 현재 추정 근처에서 자코비안으로 선형화해 공분산을 계산합니다. 그래서 선형화가 크게 틀리는 곳에서는 추정이 나빠질 수 있습니다.
3. SLAM에서 루프 닫기가 중요한 이유는?
   • 답: 돌아다니며 쌓인 위치 오차가, 예전에 본 곳을 다시 알아보는 순간 큰 제약 하나로 묶여 한꺼번에 줄어들기 때문입니다.

용어 정리

더 깊이 보려면

• Welch · Bishop 보고서 3절 — 일정한 전압을 칼만 필터로 추정하는 예제와 $Q$ · $R$ 조정 실험
• Durrant-Whyte · Bailey 「SLAM Part I」(2006) · 「Part II」 — SLAM의 확률적 정의와 EKF-SLAM · FastSLAM, 이후의 계산 기법
• ORB-SLAM — 특징점 기반 단안 시각 SLAM의 대표 시스템
• 로봇 기구학 첫 수업 5장(자코비안) — EKF 선형화와 같은 도구

다음에 볼 것

• 로봇 제어 기초: 피드백 · PID · MPC — 추정한 상태를 목표로 데려가는 제어기
• Hugging Face LeRobot: 로봇 학습 오픈소스 — 센서 기록(관절 · 카메라)으로 정책을 학습하는 모방 학습

라이선스

본문은 KAITABASE 자체 해설입니다. Welch · Bishop 보고서는 UNC 기술 보고서로 공개되어 있으며 별도 자유 이용 허락 표기는 확인하지 못해, 식의 내용만 참고하고 문장은 옮기지 않았습니다. 레일 로봇 · 초음파/라이다 · 상보 필터 숫자는 이 수업을 위해 만든 해설용 예입니다.

출처

• Greg Welch · Gary Bishop, An Introduction to the Kalman Filter, UNC-Chapel Hill TR 95-041 (Updated 2004-04-05): techreports.cs.unc.edu/papers/95-041.pdf
• Hugh Durrant-Whyte · Tim Bailey, Simultaneous Localization and Mapping: Part I, IEEE Robotics & Automation Magazine 13(2), 2006, doi:10.1109/MRA.2006.1638022
• Raúl Mur-Artal · J. M. M. Montiel · Juan D. Tardós, ORB-SLAM: a Versatile and Accurate Monocular SLAM System, IEEE Transactions on Robotics, 2015: arxiv.org/abs/1502.00956