Kalman Filter에서 Factor Graph까지
이 글은 Kalman Filter, Factor Graph, SLAM을 공부하면서 내가 질문을 통해 확인한 이해를 정리한 기록이다. 아직 논문 세부 구현이나 특정 회사 내부 기술처럼 확인이 필요한 내용은 마지막의 다음 공부 질문으로 남긴다.
1. Kalman Filter에서 상태와 오차 공분산
상태 x는 우리가 알고 싶은 물리량이다. 예를 들어 객체 추적에서는 위치와 속도,
로봇에서는 위치와 자세가 상태가 된다. 하지만 우리는 진짜 상태를 정확히 알 수 없기 때문에
추정값 \(\hat{\mathbf{x}}\)을 평균으로 하는 가우시안 분포로 상태의 불확실성을 표현한다.
오차 공분산 P는 상태 자체가 아니라, 추정값이 실제 상태에서 얼마나 틀릴 수 있는지를
나타내는 불확실성이다. 1차원으로 보면 평균은 \(\hat{x}\), 분산은 \(P\)인
가우시안 분포를 생각하면 된다.
2. Q와 R은 무엇인가
프로세스 노이즈 Q는 예측 모델이 틀릴 가능성을 나타낸다. 예를 들어 도로 슬립,
바람, 모델링하지 못한 가속도 같은 외부 요인이 여기에 들어간다. 보통 평균은 0이고,
공분산이 Q인 노이즈로 가정한다.
측정 노이즈 R는 센서가 틀릴 가능성을 나타낸다. 센서 측정값 z를
평균으로 두고, 그 주변에 R만큼 퍼진 측정 분포를 생각할 수 있다. 결국 Kalman Filter의
update는 예측 분포와 측정 분포를 어떻게 섞을지 결정하는 과정이다.
3. Kalman Gain은 동적으로 정해지는 alpha처럼 볼 수 있다
단순 low-pass filter에서는 alpha를 사람이 정한다. 새 측정값을 얼마나 믿을지
고정된 비율로 결정하는 것이다. Kalman Filter에서는 이 비율이 고정값이 아니라,
예측 불확실성 P와 측정 불확실성 R에 따라 매 순간 계산된다.
그래서 Kalman Gain K는 공학적으로 보면 자동으로 계산되는 동적 alpha다.
수학적으로는 예측 분포와 측정 분포를 결합하거나, 평균제곱오차를 최소화하는 식을 전개할 때
자연스럽게 등장하는 가중치다. 두 관점은 서로 모순되지 않는다.
4. MSE 최소화와 가우시안 곱은 같은 문제로 연결된다
Kalman Filter는 평균제곱오차를 최소화하는 필터라고 설명되기도 하고, 베이즈 관점에서 예측 분포와 측정 분포를 곱한다고 설명되기도 한다. 이 둘은 서로 다른 이야기가 아니다.
가우시안 확률은 에러 제곱이 작을수록 커진다. 확률을 최대화하는 문제에 로그를 취하고 부호를 바꾸면, 결국 가중된 에러 제곱합을 최소화하는 문제가 된다. 즉, 확률 관점의 MAP 추정과 최적화 관점의 least squares는 같은 뿌리를 가진다.
5. Factor Graph에서 변수와 팩터
Factor Graph에서 변수 노드는 우리가 추정해야 하는 미지수다. SLAM에서는 로봇의 pose
x1, x2, x3와 landmark 위치 l1, l2가 변수 노드가 될 수 있다.
여기서 x는 시간에 따라 변하는 로봇 pose이고, l은 지도 안의 고정된
특징점 또는 물체 위치를 의미한다.
팩터 노드는 변수들 사이의 제약 조건이다. 오도메트리 팩터는 x1과 x2
사이의 이동량을 제약한다. 카메라 팩터는 어떤 pose에서 landmark를 보면 이미지의 특정 픽셀에
보여야 한다는 reprojection error를 만든다. IMU 팩터는 두 pose 사이의 회전, 속도, 위치 변화가
IMU 적분값과 일치해야 한다는 제약을 만든다.
6. SLAM에서 Factor Graph가 하는 일
SLAM에서는 로봇 pose와 landmark를 동시에 추정해야 한다. 로봇이 움직일수록 pose 변수와 센서 팩터가 계속 추가된다. 나중에 같은 장소를 다시 보면 loop closure 팩터가 생기고, 이 팩터는 과거 pose들의 누적 오차를 다시 조정할 수 있는 강한 제약이 된다.
Factor Graph 최적화는 모든 팩터의 에러를 동시에 작게 만드는 변수 값을 찾는다. 칼만 필터가 현재 상태 중심으로 순차 업데이트하는 방식이라면, graph optimization은 여러 시간의 상태를 펼쳐놓고 함께 조정할 수 있다.
7. 실시간성: 전체 batch와 sliding window
모든 과거 데이터를 계속 들고 전체 최적화를 하면 계산량이 커진다. 그래서 실제 시스템에서는 프론트엔드와 백엔드를 나누거나, 최근 몇 초 또는 몇 프레임만 유지하는 fixed-lag smoothing, sliding window optimization을 사용한다.
프론트엔드는 빠른 추정을 담당하고, 백엔드는 더 넓은 구간의 일관성을 맞춘다. 실시간 제어에는 지연이 작은 추정값이 필요하지만, 장기적인 지도와 누적 오차 보정에는 그래프 최적화가 중요하다.
8. Bayesian Network, Markov Network, Factor Graph
Bayesian Network는 방향이 있는 그래프다. 조건부 확률과 인과적 또는 시간적 흐름을 표현하기 좋다. Kalman Filter의 상태 전이 구조는 dynamic Bayesian network로 볼 수 있다.
Markov Network는 방향이 없는 그래프다. 누가 원인이고 결과인지보다, 변수들이 서로 어떤 제약이나 상호 의존성을 갖는지를 표현한다.
Factor Graph는 변수 노드와 팩터 노드를 분리해 표현하는 이분 그래프다. Bayesian Network나 Markov Network로 표현되는 문제도 factor 형태로 쪼개서 나타낼 수 있다. 그래서 Factor Graph는 특정 확률 모델 하나라기보다, 복잡한 확률/최적화 문제를 계산 가능한 구조로 표현하는 방식에 가깝다.
다음 공부 질문
- GTSAM에서 PriorFactor, BetweenFactor, ProjectionFactor, ImuFactor가 실제 코드에서 어떻게 연결될까?
- Factor Graph 기반 MOT 논문에서 GMM과 max-mixture를 정확히 어떻게 쓰는지 논문 수식으로 확인해야 한다.
- Ceres Solver는 residual block과 parameter block을 어떻게 구성해 nonlinear least squares를 푸는가?
- Kalman Filter의 covariance collapse 또는 inconsistency 문제를 MOT에서는 어떤 실전 기법으로 완화하는가?
- SpaceX 같은 특정 회사가 어떤 내부 알고리즘을 쓰는지는 공개 자료 없이는 단정하지 않는다.