- 칼만 필터는 잡음이 섞인 센서 측정과 불완전한 동적 모델을 함께 사용해, 현재 상태와 다음 상태의 불확실성까지 추정하는 알고리듬임
- 튜토리얼은 항공기 레이더 추적 예제로 거리 (r)와 속도 (v)를 상태 벡터로 두고, 예측값과 측정값을 결합하는 과정을 수치로 따라감
- 초기 측정값 (10,000m), (200m/s)와 샘플링 간격 (5s)를 쓰면 상수 속도 모델에서 다음 위치는 (11,000m)로 예측되며, 측정 잡음 (R)과 공정 잡음 (Q)가 공분산에 반영됨
- 두 번째 측정값 (11,020m), (202m/s)는 더 불확실하지만, 칼만 이득 (K)가 예측과 측정을 가중 결합해 갱신 상태를 (11,009.37m), (201.43m/s)로 계산함
- 초기화 뒤에는 예측-갱신 루프가 반복되며, 실제 구현에서는 Joseph form 같은 안정적인 공분산 갱신식과 이상 측정값 처리까지 고려해야 함
칼만 필터가 푸는 추정 문제
- 칼만 필터는 불확실성이 있는 환경에서 시스템의 상태를 추정하고 예측하는 알고리듬임
- 측정 잡음이 있는 센서 데이터
- 알려지지 않은 외부 요인
- 동적 모델과 실제 움직임 사이의 차이
- 객체 추적, 내비게이션, 로보틱스, 제어, 금융 시장 분석, 기상 예측 등에 쓰임
- 컴퓨터 마우스 궤적 추정에 적용하면 잡음을 줄이고 손 떨림을 보정해 더 안정적인 이동 경로를 만들 수 있음
- 튜토리얼은 복잡한 수학 설명보다 수치 예제와 직관적 설명으로 칼만 필터를 이해하도록 구성됨
- 잘못 설계된 상황에서 칼만 필터가 물체를 제대로 추적하지 못하는 예와 이를 보정하는 방법도 포함함
학습 경로
- 이 프로젝트는 칼만 필터를 세 가지 깊이로 학습할 수 있게 구성됨
- 단일 페이지 개요: 핵심 아이디어와 필수 방정식을 유도 없이 설명하며, 기본적인 통계와 선형대수 지식을 가정함
- 무료 예제 기반 웹 튜토리얼: 수치 예제로 직관을 만들고 칼만 필터 방정식 유도까지 단계적으로 다루며, 사전 지식이 필요 없다고 안내함
- Kalman Filter from the Ground Up: 14개의 완전 풀이 수치 예제, 성능 플롯과 표, Extended Kalman Filter, Unscented Kalman Filter, 센서 융합, 구현 가이드라인을 포함함
레이더 추적 예제로 보는 예측 요구
- 항공기를 추적하는 레이더에서는 항공기가 시스템이고, 추정해야 할 위치가 시스템 상태임
- 레이더는 좁은 빔을 항공기 방향으로 조향하므로, 다음 빔을 어디로 보낼지 정하려면 미래 위치를 예측해야 함
- 예측에 실패하면 빔이 잘못된 방향을 향해 추적을 잃을 수 있음
- 시간에 따른 시스템 움직임을 나타내는 동적 모델이 필요함
- 단순화된 1차원 예제에서는 항공기가 레이더를 향하거나 멀어지는 직선 방향으로 움직인다고 가정함
- 레이더는 펄스 송수신 시간으로 거리 (r)를 계산함
- 도플러 효과로 속도 (v)도 측정할 수 있음
- (t_0)에서 거리 (10,000m), 속도 (200m/s)가 매우 정확하게 측정되고, 샘플링 간격 (\Delta t=5s)이며 속도가 일정하다고 가정하면 다음 위치는 (11,000m)임
- (\Delta r = v \cdot \Delta t)
- (r_{t_1}=10,000+200\cdot5=11,000m)
측정 잡음과 공정 잡음
- 실제 레이더 측정은 완벽히 정밀하지 않아, 같은 순간에 여러 레이더가 측정해도 서로 조금씩 다른 값을 낼 수 있음
- 이 변동은 측정 잡음으로 표현됨
- 상태 추정값뿐 아니라 그 추정이 얼마나 신뢰할 만한지도 계산해야 함
- 동적 모델도 완벽하지 않음
- 항공기가 일정 속도로 움직인다고 가정해도 바람 같은 외부 요인 때문에 실제 움직임이 달라질 수 있음
- 이런 예측 불가능한 영향은 공정 잡음임
- 칼만 필터는 현재 상태 추정, 미래 상태 예측, 그리고 각각의 불확실성을 함께 제공함
- 시스템과 잡음이 모델의 가정을 따른다는 조건에서 상태 추정 불확실성을 최소화하는 최적 알고리듬임
상태 벡터와 초기화
- 예제의 시스템 상태는 항공기의 거리 (r)와 속도 (v)로 구성됨
[ \boldsymbol{x}= \begin{bmatrix} r\ v \end{bmatrix} ]
- 첫 번째 측정값은 (t_0)에서 다음과 같음
[ \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- 측정값에는 불확실성이 있으므로 각 측정에는 분산 형태의 측정 불확실성이 붙음
- 거리 측정 표준편차: (4m)
- 속도 측정 표준편차: (0.5m/s)
- 분산은 표준편차의 제곱임
[ \boldsymbol{R}_0= \begin{bmatrix} 16 & 0\ 0 & 0.25 \end{bmatrix} ]
- 이 예제에서는 거리와 속도 측정 오류가 서로 관련 없다고 가정해 공분산 행렬의 비대각 원소를 0으로 둠
- 초기화 단계에서는 측정값과 시스템 상태가 같은 물리량 (r), (v)를 나타내므로 첫 측정값을 초기 상태 추정값으로 사용할 수 있음
[ \hat{\boldsymbol{x}}_{0,0}= \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- 이 방식은 초기화 단계에서만 사용할 수 있음
예측 단계: 상태와 공분산 전파
- 예측은 현재 상태와 상태 전이 행렬 (\boldsymbol{F})를 사용해 다음 시점의 상태를 계산함
- 상수 속도 모델에서는 다음 식을 사용함
[ v_1=v_0=v ]
[ r_1=r_0+v_0\Delta t ]
- 행렬 형태의 상태 예측식은 다음과 같음
[ \hat{\boldsymbol{x}}_{n+1,n}
\boldsymbol{F} \hat{\boldsymbol{x}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n ]
- (\boldsymbol{u}_n): 입력 변수
- (\boldsymbol{G}): 입력 전이 행렬
- 이 단순 예제에서는 입력이 없어서 (\boldsymbol{u}_n=0)
- (\Delta t=5s)일 때 상태 전이 행렬은 다음과 같고, 예측 결과는 (11,000m), (200m/s)임
[ \boldsymbol{F}= \begin{bmatrix} 1 & 5\ 0 & 1 \end{bmatrix} ]
[ \hat{\boldsymbol{x}}_{1,0}
\begin{bmatrix} 11{,}000\ 200 \end{bmatrix} ]
- 공분산 예측은 단순히 (\boldsymbol{F}\boldsymbol{P})가 아니라 (\boldsymbol{F}\boldsymbol{P}\boldsymbol{F}^T)를 사용함
[ \boldsymbol{P}_{n+1,n}
\boldsymbol{F} \boldsymbol{P}_{n,n} \boldsymbol{F}^T + \boldsymbol{Q} ]
- 공정 잡음을 제외하면 예측 공분산은 다음과 같음
[ \boldsymbol{P}_{1,0}
\begin{bmatrix} 22.25 & 1.25\ 1.25 & 0.25 \end{bmatrix} ]
- 속도 분산은 상수 속도 모델 때문에 (0.25)로 유지됨
- 거리 분산은 속도 불확실성이 시간에 따라 거리 불확실성을 키우므로 (16)에서 (22.25)로 증가함
공정 잡음 반영
- 실제 항공기 속도는 바람 같은 예측 불가능한 외부 요인 영향을 받을 수 있으므로 공정 잡음 (\boldsymbol{Q})를 공분산 예측에 더함
- 예제에서는 랜덤 가속도의 표준편차를 (\sigma_a=0.2m/s^2)로 가정함
- 분산은 (\sigma_a^2=0.04m^2/s^4)
- (\Delta t=5s)일 때 공정 잡음 행렬은 다음과 같음
[ \boldsymbol{Q}
\begin{bmatrix} 6.25 & 2.5\ 2.5 & 1 \end{bmatrix} ]
- 공정 잡음을 더한 예측 공분산은 다음과 같음
[ \boldsymbol{P}_{1,0}
\begin{bmatrix} 28.5 & 3.75\ 3.75 & 1.25 \end{bmatrix} ]
갱신 단계: 예측과 측정의 가중 결합
- (t_1)에서 두 번째 측정값은 다음과 같음
[ \boldsymbol{z}_1= \begin{bmatrix} 11{,}020\ 202 \end{bmatrix} ]
- 이 측정은 강한 잡음 스파이크로 신호대잡음비가 낮아져 첫 번째 측정보다 불확실성이 크다고 가정함
- 거리 표준편차: (6m)
- 속도 표준편차: (1.5m/s)
[ \boldsymbol{R}_1= \begin{bmatrix} 36 & 0\ 0 & 2.25 \end{bmatrix} ]
- 예측 공분산 (\boldsymbol{P}_{1,0})의 대각 원소는 측정 공분산 (\boldsymbol{R}_1)보다 작으므로 예측 쪽 불확실성이 더 낮음
- 칼만 필터는 예측만 쓰거나 측정만 쓰지 않고, 불확실성이 낮은 쪽에 더 큰 가중치를 주어 결합함
- 1차원 형태의 가중 평균은 다음과 같음
[ \hat{x}_{1,1}
K_1 z_1 + (1-K_1)\hat{x}_{1,0} ]
- (\boldsymbol{K})는 칼만 이득이며, 갱신된 추정값의 불확실성을 최소화하도록 측정과 예측의 가중치를 정함
혁신, 관측 행렬, 칼만 이득
- 상태 갱신식은 예측값에 보정항을 더하는 형태로 쓸 수 있음
[ \hat{\boldsymbol{x}}_{1,1}
\hat{\boldsymbol{x}}_{1,0} + \boldsymbol{K}_1 ( \boldsymbol{z}_1
\boldsymbol{H}\hat{\boldsymbol{x}}_{1,0} ) ]
- (\boldsymbol{z}1-\boldsymbol{H}\hat{\boldsymbol{x}}{1,0})는 혁신(innovation) 또는 잔차(residual)이며, 새 측정이 제공하는 정보를 나타냄
- (\boldsymbol{H})는 관측 행렬 또는 측정 행렬로, 상태 변수를 실제 측정되는 물리량으로 매핑함
- 이 예제에서는 상태와 측정이 모두 거리와 속도이므로 (\boldsymbol{H}=\boldsymbol{I})
- 일반적으로는 디지털 온도계처럼 측정값과 상태가 서로 다른 물리 영역에 있을 수 있음
- 다변량 칼만 이득은 다음과 같음
[ \boldsymbol{K}_n
\boldsymbol{P}{n,n-1} \boldsymbol{H}^T ( \boldsymbol{H} \boldsymbol{P}{n,n-1} \boldsymbol{H}^T + \boldsymbol{R}_n )^{-1} ]
- 예제에서 계산된 칼만 이득은 다음과 같음
[ \boldsymbol{K}_1= \begin{bmatrix} 0.4048 & 0.6377\ 0.0399 & 0.3144 \end{bmatrix} ]
- 행렬 역행렬 계산은 MATLAB의
inv(A)또는 Python의numpy.linalg.inv(A)로 가능하지만, 실제 구현에서는 명시적 역행렬보다A\b또는numpy.linalg.solve(A, b)처럼 선형 시스템을 직접 푸는 방식이 일반적으로 더 나음
갱신 결과와 공분산 감소
- 이 예제의 혁신은 다음과 같음
[ \boldsymbol{z}1-\hat{\boldsymbol{x}}{1,0}
\begin{bmatrix} 20\ 2 \end{bmatrix} ]
- 칼만 이득으로 보정항을 계산하면 다음과 같음
[ \boldsymbol{K}_1 \begin{bmatrix} 20\ 2 \end{bmatrix}
\begin{bmatrix} 9.37\ 1.43 \end{bmatrix} ]
- 갱신된 상태 추정값은 다음과 같음
[ \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 11{,}009.37\ 201.43 \end{bmatrix} ]
- 다변량 공분산 갱신에는 수치적으로 안정적인 Joseph form이 흔히 사용됨
[ \boldsymbol{P}_{n,n}
(\boldsymbol{I}-\boldsymbol{K}n\boldsymbol{H}) \boldsymbol{P}{n,n-1} (\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H})^T + \boldsymbol{K}_n \boldsymbol{R}_n \boldsymbol{K}_n^T ]
- 단순화된 공분산 갱신식도 문헌에서 자주 보임
[ \boldsymbol{P}_{n,n}
(\boldsymbol{I}-\boldsymbol{K}n\boldsymbol{H}) \boldsymbol{P}{n,n-1} ]
- 정확한 산술에서는 두 형태가 같은 결과를 주지만, 컴퓨터 구현에서는 Joseph form이 일반적으로 더 수치적으로 안정적임
- 예제에서 단순화된 식으로 계산한 갱신 공분산은 다음과 같음
[ \boldsymbol{P}_{1,1}
\begin{bmatrix} 14.57 & 1.43\ 1.43 & 0.71 \end{bmatrix} ]
- 갱신 공분산의 대각 원소는 예측 공분산 ((28.5, 1.25))과 측정 공분산 ((36, 2.25))보다 낮음
- 새 정보는 불확실성이 높더라도 추정 불확실성을 줄이며, 이론적으로 새 측정은 무시하지 않아야 함
- 실제 구현에서는 신뢰할 수 없는 측정값을 거부해야 하는 경우가 있으며, 이상값 처리 방법은 책의 Outlier Treatment 장에서 다룸
다음 예측과 반복 루프
- Iteration 1의 예측 단계는 Iteration 0과 같지만, 시작점이 갱신된 (\hat{\boldsymbol{x}}{1,1})과 (\boldsymbol{P}{1,1})로 바뀜
- 상태 예측 결과는 다음과 같음
[ \hat{\boldsymbol{x}}_{2,1}
\boldsymbol{F} \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 12{,}016.5\ 201.43 \end{bmatrix} ]
- 공분산 예측 결과는 다음과 같음
[ \boldsymbol{P}_{2,1}
\begin{bmatrix} 52.86 & 7.47\ 7.47 & 1.71 \end{bmatrix} ]
- 새 측정 없이 시간이 지나면 불확실성이 자연스럽게 커지므로 예측 단계에서 분산이 다시 증가함
- 속도 불확실성은 거리 불확실성을 추가로 키움
- 그래서 거리 분산이 속도 분산보다 더 빠르게 증가함
- 예제는 칼만 필터의 세 단계를 보여줌
- 초기화: 시작 시 한 번 수행
- 예측: 동적 모델로 다음 상태와 불확실성을 전파
- 갱신: 새 측정과 예측을 칼만 이득으로 결합
- 초기화 이후 칼만 필터는 계속해서 예측-갱신 루프로 동작함