2026, Aug 30 · State Estimation / Tracking

Kalman Filter를 이해하는 과정

객체 추적을 공부하면서 가장 먼저 부딪힌 문제는 “현재 객체의 위치를 알았을 때, 다음 순간의 위치와 속도를 어떻게 안정적으로 추정할 것인가?”였다. 센서가 주는 위치는 매번 조금씩 흔들리고, 두 프레임의 위치 차이를 시간으로 나누는 단순한 방법은 작은 위치 오차를 큰 속도 오차로 바꿔버린다.

Kalman Filter는 이 문제를 다루는 재귀적 상태 추정기다. 시스템의 상태를 하나의 확정된 숫자로 단정하지 않고, “이 상태일 가능성이 어느 정도인가”라는 Gaussian 확률 분포로 표현한다. 그 뒤 물리 모델을 이용해 다음 상태를 예측하고, 새로 들어온 측정값을 이용해 예측을 보정한다.

아래 글은 이 흐름을 발표 자료로 공부한 내용을 바탕으로 다시 설명한 기록이다. 그림은 설명을 보조하기 위한 참고 자료로 배치했고, 그림을 읽는 데 필요한 내용은 본문에서 이어지는 논리로 풀어 썼다.

왜 위치만으로는 속도를 구하기 어려운가

LiDAR나 카메라로 객체를 검출하면 각 시점의 위치를 얻을 수 있다. 가장 단순한 속도 계산은 다음과 같다.

v_t = \frac{p_t - p_{t-1}}{\Delta t}

하지만 위치에 노이즈가 섞여 있으면 문제가 생긴다. 프레임 간격이 0.1초인데 위치 오차가 1m만 생겨도 속도에는 10m/s, 즉 36km/h에 해당하는 오차가 생긴다. 속도를 구하기 위해 위치를 미분하는 순간 센서 노이즈가 증폭되는 것이다.

게다가 두 시점의 검출 결과가 같은 객체인지 먼저 알아야 한다. 과거 프레임의 객체와 현재 프레임의 객체를 올바르게 연결하는 data association이 틀리면, 실제로는 다른 객체의 위치 차이를 속도로 계산하게 된다. 따라서 추적 시스템에서는 객체 매칭, 대표 tracking point 선택, 상태 추정을 하나의 흐름으로 봐야 한다.

Kalman Filter의 핵심은 재귀적 추정이다

Kalman Filter는 재귀 필터의 한 종류다. 재귀적이라는 것은 k번째 변수를 k-1번째 변수에 대한 함수로 표현할 수 있다는 뜻이다. Kalman Filter에서는 k-1번째 추정 상태와 k번째 측정 위치를 입력값으로 사용해 k번째 추정 상태를 출력한다. 따라서 k번째 상태를 계산하기 위해 모든 과거 센서 데이터를 매번 처음부터 다시 읽을 필요가 없다.

k-1번째까지의 정보는 추정 상태와 오차 공분산이라는 요약값으로 보존된다. 새로운 측정값이 들어오면 이 요약값과 측정값을 결합해 현재 상태를 계산하고, 그 결과를 다음 단계의 입력으로 넘긴다. 이것이 Kalman Filter가 시간의 흐름을 따라 상태를 추적하면서도 일정한 형태의 계산을 반복할 수 있는 이유다.

첫 번째 관측에서는 이전 상태가 없으므로 위치를 측정한 뒤 그 측정값을 Kalman Filter의 초기 상태로 사용한다. 첫 번째 관측에서는 비교할 과거 상태가 없기 때문에 아직 본격적인 추정을 수행하지 않는다. 추정은 두 번째 관측부터 시작된다.

두 번째 관측 시점에서는 첫 번째 관측에서 만든 초기 상태가 과거 상태로 바뀐다. 동시에 센서가 새로운 현재 위치를 측정한다. 이제 Kalman Filter는 과거 상태와 새 측정값을 함께 사용할 수 있다.

먼저 예측 시스템 A를 이용해 과거 상태를 현재 시점으로 이동시킨다. 이것이 prediction이다. 이후 prediction으로 얻은 예측 상태와 두 번째 관측에서 얻은 현재 측정 위치를 함께 활용해 현재 상태를 추정한다. 즉, prediction은 “모델에 따르면 지금 어디에 있을까?”를 계산하고, update는 “새로운 센서 측정까지 고려하면 지금 어디에 있다고 보는 것이 가장 합리적일까?”를 계산한다.

k번째 관측에서도 같은 과정이 반복된다. k-1번째 추정 상태를 과거 상태로 넘기고, 예측 시스템으로 k번째 상태를 예측한 다음, k번째 측정 위치로 현재 상태를 보정한다. 이렇게 추정 상태가 재귀적으로 갱신되며, 시스템 모델과 측정 노이즈가 적절히 설정되어 있다면 반복할수록 실제 위치와 추정 위치의 오차가 줄어든다.

과거와 현재 시점에서 측정 위치가 Kalman Filter를 거쳐 추정 상태가 되는 자동차 흐름 — 재귀 필터는 이전 추정 상태와 새 측정 위치를 다음 추정 상태로 전달한다.

첫 번째 관측에서 자동차 위치를 초기 상태로 정하는 그림 — 첫 번째 관측에서는 측정 위치를 초기 상태로 사용한다.

첫 번째 관측 상태가 두 번째 관측에서 과거 상태가 되는 자동차 그림 — 시간이 흐르면 초기 상태는 과거 상태가 된다.

두 번째 관측에서 새로운 현재 위치를 측정하는 자동차 그림 — 두 번째 관측에서는 새로운 현재 위치 측정값이 들어온다.

예측 시스템 A가 과거 상태에서 현재 위치를 예측하는 자동차 그림 — 예측 시스템은 과거 상태를 현재 시점의 예측 상태로 이동시킨다.

예측 상태와 측정 위치를 사용해 현재 상태를 추정하는 자동차 그림 — 예측 상태와 측정 위치를 결합해 현재 상태를 추정한다.

Prediction과 Update

Kalman Filter의 알고리즘은 초기화 이후 Prediction과 Update를 반복하는 구조로 정리할 수 있다. Prediction에서는 상태의 평균뿐 아니라 상태가 얼마나 불확실한지도 함께 예측한다. Update에서는 measurement를 입력하고 Kalman gain을 계산한 뒤, 상태와 오차 공분산을 갱신한다.

초기값 선정과 prediction, measurement, update로 구성된 Kalman Filter 알고리즘 — Kalman Filter 전체 알고리즘의 흐름.

Prediction과 Update의 세부 계산 순서를 보여주는 알고리즘 그림 — 상태와 불확실성을 함께 예측하고 측정으로 보정한다.

예측 시스템은 물리 모델에서 출발한다

Kalman Filter가 다음 상태를 예측하려면 상태가 시간에 따라 어떻게 변하는지 설명하는 시스템 모델이 필요하다. 이 모델은 필터가 알아서 발견하는 것이 아니라 문제를 이해한 사람이 설계한다. 차량의 위치와 속도를 추정한다면 위치와 속도의 관계, 시간 간격, 등속도 또는 가속도에 대한 가정을 모델에 넣는다.

과거 위치 5m와 속도 10m/s를 사용해 1초 뒤 현재 위치 15m를 예측하는 그림 — 과거 위치와 속도를 이용해 현재 위치를 예측하는 시스템 모델.

이미지의 예시에서는 과거 위치가 \(5\,\mathrm{m}\), 속도가 \(10\,\mathrm{m/s}\), 시간 간격이 \(1\,\mathrm{s}\)다. 등속도 가정에서는 속도에 시간 간격을 곱한 이동 거리를 과거 위치에 더해 현재 위치를 예측한다.

p_1 = p_0 + v_0\Delta t = 5 + 10\times 1 = 15\,\mathrm{m}

초기 상태에서 예측 위치와 예측 Gaussian 분포가 만들어지는 그림 — 물리 모델로 위치를 예측하면 예측 위치와 그 불확실성을 함께 얻는다.

그림의 예측 Gaussian은 단순히 자동차의 위치 하나만 표시한 것이 아니다. 중심은 예측 위치이고 폭은 예측의 불확실성이다. 따라서 시스템 행렬은 평균을 이동시키는 역할과 함께, 이전 공분산을 현재 시점으로 전달하는 역할도 한다.

발표 자료에서는 상태를 Gaussian 분포로 표현하는 것을 다음처럼 적었다. 여기서 \(\mu\)는 평균이고 \(\sigma\)는 표준편차다. 공분산을 사용하는 다변량 상태에서는 \(\sigma^2\) 대신 공분산 행렬 \(\mathbf{P}\)를 사용한다.

f(x) = \mathcal{N}(\mu,\sigma)

다만 모델이 현실을 완벽하게 설명할 수는 없다. 등속도 모델을 사용해도 실제 차량은 가속하거나 감속할 수 있고, 바람, 노면, 타이어 상태, 운전자의 조작처럼 미리 정확하게 알 수 없는 요인이 있다. 이 모델 오차와 외부 요인을 process noise로 다룬다.

시스템 모델 오차와 예측할 수 없는 외부 요인의 불확실성 — 예측 모델의 불확실성과 외부 요인의 불확실성.

상태를 Gaussian 확률 분포로 표현한다

실제 객체의 상태를 완벽하게 알 수 없을 때, 하나의 값만 저장하면 그 값이 얼마나 믿을 만한지 알 수 없다. Kalman Filter는 상태를 Gaussian 분포로 표현한다. 분포의 평균은 가장 가능성이 높은 상태를 뜻하고, 분산 또는 공분산은 그 상태의 불확실성 크기를 뜻한다.

확률 분포와 Gaussian 정규 분포의 개념 — 평균 주변에 값이 모이고 멀어질수록 가능성이 낮아지는 Gaussian 분포.

초기 상태의 평균과 공분산을 각각 \(\hat{\mathbf{x}}_0\), \(\mathbf{P}_0\)라고 하자. 예측 시스템 \(\mathbf{A}\)를 적용하면 평균은 예측 위치로 이동하고, 공분산에는 process noise covariance \(\mathbf{Q}\)가 더해진다.

\hat{\mathbf{x}}_1^- = \mathbf{A}\hat{\mathbf{x}}_0, \qquad \mathbf{P}_1^- = \mathbf{A}\mathbf{P}_0\mathbf{A}^{\mathsf{T}} + \mathbf{Q}

여기서 세 가지를 분리해서 이해해야 한다. 상태 x는 알고 싶은 실제 물리량이고, 추정 상태는 그 상태에 대한 현재의 믿음이다. 추정 상태의 오차 공분산은 P로 표현한다. 반면 Q는 process noise의 공분산이고, R은 measurement noise의 공분산이다. Q와 R은 P와 같은 대상이 아니라, P가 어떻게 변할지를 결정하는 노이즈 모델이다.

상태 불확실성과 process noise covariance Q를 Gaussian으로 표현한 그림 — 상태의 평균은 추정값이고 분산은 불확실성이다. Q는 process noise covariance다.

일변량 Gaussian 변수 두 개의 합을 생각하면 평균은 더해지고, 서로 독립인 경우 분산도 더해진다.

\mu' = \mu_0 + \mu_1, \qquad (\sigma')^2 = \sigma_0^2 + \sigma_1^2

등속도 모델로 위치와 속도를 함께 예측하기

가장 단순한 상태 벡터를 위치와 속도로 두자.

\mathbf{x}_k = \begin{bmatrix} p_k \\ v_k \end{bmatrix}

등속도 모델은 짧은 시간 동안 속도가 유지된다고 가정한다. 그러면 다음 위치는 이전 위치에 속도와 시간 간격의 곱을 더한 값이 된다. 이 관계를 행렬로 쓰면 상태 전이 행렬 A를 통해 이전 상태를 현재 예측 상태로 바꿀 수 있다.

속도는 위치의 변화량으로 정의할 수 있다. 이전 위치를 p_{k-1}, 현재 위치를 p_k, 시간 간격을 \(\Delta t\)라고 하면 다음과 같다.

v_k = \frac{p_k - p_{k-1}}{\Delta t}, \qquad p_k = p_{k-1} + v_k\Delta t

등속도 모델에서는 과거의 속도와 현재의 속도가 같다고 가정한다.

v_k = v_{k-1}

위치와 속도를 하나의 상태 벡터로 묶으면, 등속도 시스템은 다음과 같은 행렬식으로 표현할 수 있다. 이때 A가 예측 시스템이고, 오른쪽의 상태 벡터가 과거 상태다.

\begin{bmatrix} p_k \\ v_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k-1} \\ v_{k-1} \end{bmatrix}, \qquad \mathbf{A} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}

이제 상태를 추정값으로 쓰면, k-1 시점의 추정 상태는 \(\hat{\mathbf{x}}_{k-1} = [p_{k-1}, v_{k-1}]^T\)이고 예측된 현재 상태는 다음과 같다.

\hat{\mathbf{x}}_k^- = \mathbf{A}\hat{\mathbf{x}}_{k-1}

상태값은 하나의 확정된 점이 아니라 Gaussian 분포를 따른다고 가정한다. 따라서 상태의 평균은 추정 상태이고, 공분산 행렬은 위치와 속도의 불확실성 및 두 변수 사이의 상관관계를 담는다.

\mathbf{P} = \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \end{bmatrix}

\(\Sigma_{pp}\)는 위치의 분산, \(\Sigma_{vv}\)는 속도의 분산이며, \(\Sigma_{pv}\)와 \(\Sigma_{vp}\)는 위치와 속도 사이의 공분산이다. 예측 시스템을 상태에 적용하면 공분산은 선형 변환의 성질에 따라 A P A^T로 변환된다.

\begin{aligned} \mathbf{P} &= \operatorname{Cov}(\hat{\mathbf{x}}) \\ &= \mathbb{E}\left[(\hat{\mathbf{x}}-\mathbb{E}[\hat{\mathbf{x}}]) (\hat{\mathbf{x}}-\mathbb{E}[\hat{\mathbf{x}}])^{\mathsf{T}}\right] \\ \operatorname{Cov}(\mathbf{A}\hat{\mathbf{x}}) &= \mathbf{A}\mathbf{P}\mathbf{A}^{\mathsf{T}} \end{aligned}

그러므로 process noise를 아직 추가하지 않은 prediction은 다음 두 식으로 정리된다. 첫 번째 식은 상태 평균을 이동시키고, 두 번째 식은 그 상태의 불확실성을 시스템 행렬에 맞춰 전파한다.

\hat{\mathbf{x}}_k^- = \mathbf{A}\hat{\mathbf{x}}_{k-1}, \qquad \mathbf{P}_k^- = \mathbf{A}\mathbf{P}_{k-1}\mathbf{A}^{\mathsf{T}} + \mathbf{Q}

초기 위치에서 prediction을 시작하면 평균은 모델이 예상한 위치로 이동한다. 동시에 예측 분포는 넓어진다. 시스템이 진행되는 동안 모델이 설명하지 못하는 오차가 새로 더해지기 때문이다.

초기 위치에서 예측이 시작되는 그림 — 초기 상태와 초기 불확실성에서 prediction을 시작한다.

예측 위치로 이동하고 Q만큼 Gaussian 분포가 넓어지는 그림 — 평균은 예측 위치로 이동하고 process noise 때문에 분포는 넓어진다.

공분산의 예측식은 다음과 같다.

\mathbf{P}_k^- = \mathbf{A}\mathbf{P}_{k-1}\mathbf{A}^{\mathsf{T}} + \mathbf{Q}

A P A^T는 이전 불확실성이 시스템 모델을 거쳐 전달된 결과이고, Q는 이번 단계에서 새로 추가된 process noise다. Gaussian random variable에 독립적인 Gaussian noise가 더해지면 합도 Gaussian으로 표현할 수 있고, 공분산은 그 오차들의 공분산이 합쳐지는 형태가 된다.

Gaussian random variable의 합과 분산 증가를 설명하는 그림 — 독립적인 Gaussian 오차가 더해지면 공분산이 커지고 분포가 넓어진다.

Measurement에도 오차가 있다

Prediction만 계속하면 모델 오차가 누적된다. 그래서 센서 measurement를 사용해 현재 상태를 보정한다. 그러나 measurement 역시 정답은 아니다. LiDAR 검출 위치에도 센서 노이즈가 있고, 가려짐이나 반사 때문에 관측이 틀릴 수도 있다.

Prediction 이후 measurement를 입력해 update를 시작하는 알고리즘 그림 — Prediction 뒤에 measurement를 입력해 update를 수행한다.

LiDAR 측정값의 불확실성을 Gaussian으로 표현하는 그림 — 측정값의 평균은 관측값이고 측정 오차의 크기는 R로 표현한다.

측정값의 불확실성은 R로 표현한다. R이 작으면 센서를 더 정확한 정보로 보고, R이 크면 측정값을 덜 신뢰한다. Prediction의 공분산 P^-와 measurement noise R을 비교하는 것이 update의 핵심이다.

선형 측정 모델은 상태를 measurement 공간으로 옮기는 행렬 \(\mathbf{H}\)와 측정 노이즈 \(\mathbf{v}_k\)로 표현할 수 있다. 측정 노이즈는 평균이 0이고 공분산이 \(\mathbf{R}\)인 Gaussian으로 모델링한다.

\mathbf{z}_k = \mathbf{H}\mathbf{x}_k + \mathbf{v}_k, \qquad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0},\mathbf{R})

예측 위치와 측정값을 각각 Gaussian 분포로 표현한 그림 — 예측 상태의 Gaussian과 measurement Gaussian을 함께 놓는다.

두 Gaussian의 곱은 두 정보가 동시에 지지하는 posterior Gaussian이 된다. 일변량 예시에서는 다음 식으로 평균과 분산을 계산할 수 있다.

\mu' = \frac{\mu_1\sigma_0^2 + \mu_0\sigma_1^2}{\sigma_0^2 + \sigma_1^2}, \qquad (\sigma')^2 = \frac{\sigma_0^2\sigma_1^2}{\sigma_0^2 + \sigma_1^2}

예를 들어 \(\mu_0=10\), \(\sigma_0^2=8\), \(\mu_1=12\), \(\sigma_1^2=2\)라면 다음과 같다.

\mu' = \frac{12\times 8 + 10\times 2}{8+2} = 11.6, \qquad (\sigma')^2 = \frac{8\times 2}{8+2} = 1.6

Kalman gain과 추정값, 공분산을 계산하는 update 알고리즘 그림 — Update는 Kalman gain, 추정 상태, 오차 공분산을 계산한다.

예측 분포와 측정 분포를 결합하기 전의 그림 — 예측 분포와 측정 분포의 상대적인 확실성을 비교한다.

Bayes Filter로 이해하는 Update

Update는 Bayes 정리의 관점에서 이해하면 자연스럽다. 상태를 x, 새 measurement를 z라고 하면 다음 관계를 생각할 수 있다.

p(\mathbf{x}\mid\mathbf{z}) \propto p(\mathbf{z}\mid\mathbf{x})\,p(\mathbf{x})

p(x)는 measurement를 보기 전의 prior다. Kalman Filter에서는 prediction으로 얻은 상태 분포가 prior가 된다. p(z | x)는 상태가 주어졌을 때 해당 measurement가 관측될 가능도(likelihood)이고, 센서의 measurement noise R을 반영한다. 둘을 곱하고 정규화한 p(x | z)가 measurement를 반영한 posterior, 즉 현재의 추정 상태 분포다.

Prior와 likelihood를 곱해 posterior를 얻는 Bayes Filter 식 — posterior는 likelihood와 prior의 곱을 정규화한 결과다.

예측 상태가 prior가 되고 measurement가 likelihood가 되는 그림 — Prediction 분포가 현재 시점의 prior가 된다.

선형 시스템과 Gaussian noise라는 조건에서는 이 posterior도 Gaussian이 된다. 그래서 평균과 공분산만 저장하면서 다음 단계로 넘어갈 수 있다. Kalman Filter는 이 Gaussian Bayesian update를 계산 가능한 재귀식으로 구현한 것으로 이해할 수 있다.

예측 Gaussian과 측정 Gaussian의 곱으로 추정 상태 Gaussian을 만드는 그림 — 추정 상태는 prediction 분포와 measurement likelihood의 곱이다.

Gaussian 두 개를 곱했을 때 평균과 분산이 변하는 성질 — 더 좁은 분포 쪽으로 평균이 가까워지고 posterior 분산은 작아진다.

Gaussian product의 구체적인 예시 — 두 분포의 평균과 분산에 따라 결합 결과가 달라진다.

예측 위치와 측정값에서 추정 상태 분포를 만드는 그림 — 두 정보가 동시에 지지하는 영역이 posterior가 된다.

예측 상태 확률 분포와 측정값 확률 분포의 곱을 보여주는 그림 — Update를 Gaussian product로 다시 표현한 모습.

여기서 “posterior 분산이 더 작아진다”는 사실은 두 독립 정보를 결합했기 때문에 생긴다. 그렇다고 실제 시스템의 불확실성이 언제나 줄어든다는 뜻은 아니다. R을 잘못 작게 잡거나 outlier를 정상 측정으로 받아들이면 필터가 실제보다 자신을 과신할 수 있다. 공분산은 확률 분포에 대한 모델의 믿음이지, 현실의 오차를 자동으로 보장하는 측정값은 아니다.

Kalman Gain은 신뢰도의 비율을 계산한다

Gaussian product에서 posterior 평균과 공분산을 정리하면 반복해서 등장하는 공통 항이 있다. 이 항을 Kalman gain K라고 부른다. 선형 측정 모델 H를 사용하는 경우 대표적인 식은 다음과 같다.

\mathbf{K}_k = \mathbf{P}_k^-\mathbf{H}^{\mathsf{T}}\left(\mathbf{H}\mathbf{P}_k^-\mathbf{H}^{\mathsf{T}} + \mathbf{R}\right)^{-1}

일변량에서 위 식은 두 Gaussian product의 평균식을 Kalman Filter 형태로 다시 쓴 것이다. 예측 공분산이 클수록 measurement를 더 반영하고, 측정 노이즈 공분산이 클수록 prediction을 더 믿게 된다.

\mu' = \mu_0 + K(\mu_1-\mu_0), \qquad K = \frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}

상태 update는 다음처럼 쓸 수 있다.

\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k\left(\mathbf{z}_k - \mathbf{H}\hat{\mathbf{x}}_k^-\right)

상태 평균뿐 아니라 update 이후의 불확실성도 갱신한다. 선형 측정 모델의 공분산 갱신식은 다음과 같다.

\mathbf{P}_k = (\mathbf{I}-\mathbf{K}_k\mathbf{H})\mathbf{P}_k^-

괄호 안의 z_k - H x_k^-는 measurement residual 또는 innovation이다. Kalman gain은 이 잔차를 상태에 얼마나 반영할지 결정한다. 예측 공분산이 크면 “모델을 확신하기 어렵다”는 뜻이므로 측정값의 영향이 커지고, R이 크면 “센서를 믿기 어렵다”는 뜻이므로 측정값의 영향이 작아진다.

Gaussian product의 평균과 분산에서 공통 항을 정리하는 과정 — posterior 평균과 분산을 정리하면 공통된 가중치 항이 나타난다.

예측 공분산과 측정 공분산으로 Kalman gain을 도출하는 과정 — 공분산과 측정 모델로부터 Kalman gain을 얻는다.

Kalman gain으로 추정값과 오차 공분산을 계산하는 식 — Kalman gain으로 상태 평균과 오차 공분산을 함께 갱신한다.

이 점에서 Kalman gain은 1차 저역통과 필터의 alpha와 비슷한 역할을 한다. 하지만 alpha를 고정해 두는 것과 다르게 Kalman gain은 현재의 P^-, R, H로부터 매 단계 계산된다. 따라서 센서와 모델의 불확실성이 바뀌면 가중치도 자동으로 바뀐다.

Kalman Filter가 prediction과 measurement를 반복적으로 결합한다는 최종 정리 그림 — 사용자는 모델과 Q, R을 설계하고 필터는 상태를 재귀적으로 추정한다.

위치 측정으로 속도까지 추정할 수 있는 이유

마지막으로 가장 궁금했던 속도 추정의 원리를 정리해보자. 센서가 위치만 측정한다고 해서 상태 벡터에 위치만 넣어야 하는 것은 아니다. 위치와 속도를 함께 상태로 두면 다음과 같이 2차원 상태 공간을 만들 수 있다.

\mathbf{x}_k = \begin{bmatrix} p_k \\ v_k \end{bmatrix}

2차원 Gaussian에서는 공분산이 위치 오차와 속도 오차의 관계를 나타낸다. 공분산이 0이면 두 오차가 독립적이라 분포 타원의 기울기가 없고, 공분산이 0이 아니면 위치와 속도 사이에 상관관계가 있어 타원이 기울어진다.

위치와 속도를 함께 가진 2차원 Gaussian과 공분산에 따른 타원 방향 — 위치와 속도를 함께 상태로 두면 공분산을 가진 2차원 Gaussian이 된다.

measurement가 위치 하나만 관측하더라도 residual은 Kalman gain의 각 성분을 통해 상태 벡터의 여러 성분에 반영된다. 위치 성분에는 K_pos가 곱해지고 속도 성분에는 K_vel이 곱해진다.

\begin{aligned} \hat{p}_k &= \hat{p}_k^- + K_{\mathrm{pos}}\,y_k \\ \hat{v}_k &= \hat{v}_k^- + K_{\mathrm{vel}}\,y_k \end{aligned}

즉 속도를 직접 측정한 것이 아니다. motion model이 위치와 속도를 연결하고, 두 상태의 오차 공분산이 남아 있기 때문에 위치 measurement의 오차가 속도 상태에도 전달된다. 위치가 예측보다 크게 벗어났다면, 필터는 그 차이를 현재 속도 추정에도 반영해 다음 위치 예측을 바꾼다.

초기 상태에서 예측 위치와 측정값을 반복적으로 결합하는 Kalman Filter 흐름 — 예측 위치와 측정값을 반복해서 결합하며 위치와 속도 상태를 갱신한다.

마무리

Kalman Filter는 noisy한 센서값을 단순히 평균내는 필터가 아니다. 상태를 Gaussian 분포로 표현하고, 물리 모델로 상태와 불확실성을 prediction한 뒤, 새 measurement를 Bayes 관점에서 결합해 posterior를 계산하는 재귀적 상태 추정기다.

이 구조에서 모델링과 튜닝은 중요한 역할을 한다. 어떤 값을 상태로 둘지, 상태가 어떻게 변할지, process noise covariance Q와 measurement noise covariance R을 어떻게 설정할지를 사람이 결정한다. 필터는 그 설계를 바탕으로 각 시점의 Kalman gain과 상태를 계산한다.

또한 Gaussian product에서 update 후 공분산이 작아지는 것은 두 정보를 결합한 수학적 결과이지, 필터가 항상 현실을 정확히 안다는 뜻은 아니다. 잘못된 모델, outlier, 시간에 따라 달라지는 센서 노이즈까지 고려하려면 이후 adaptive noise, EKF, UKF, IMM, factor graph와 smoothing으로 공부를 확장해야 한다.