선형 칼만 필터(Linear Kalman Filter, KF)는 ‘추정(Estimation)’ 문제에 있어 거의 표준과 같은 알고리즘이다. 하지만 이 강력한 도구는 ‘선형(Linear)’이라는 엄격한 조건이 붙은 세계에서만 최적의 성능을 발휘한다.1 선형 칼만 필터의 핵심은 두 가지 단계, 예측(Prediction)과 업데이트(Update)의 끝없는 재귀적 반복에 있다.
이 과정이 재귀적(recursive)이라는 점은 칼만 필터의 가장 큰 효율성 중 하나다. 과거의 모든 데이터를 저장하고 매번 다시 계산할 필요 없이, 바로 직전 스텝의 추정값(상태와 공분산)만 있으면 현재 상태를 추정할 수 있다.3 이는 메모리 사용량이 적고 계산 속도가 빨라 실시간 시스템에 매우 적합하다는 의미다.
하지만 이 모든 아름다운 과정은 두 가지 핵심적인 가정 위에 세워져 있다. 첫째, 시스템의 움직임(상태 전이)과 측정 과정이 모두 행렬 곱으로 표현되는 선형 관계여야 한다($\mathbf{x}k = \mathbf{A}\mathbf{x}{k-1} + \mathbf{B}\mathbf{u}_{k-1}$, $\mathbf{z}_k = \mathbf{H}\mathbf{x}_k$). 둘째, 시스템 모델의 불확실성(프로세스 노이즈)과 측정값의 불확실성(측정 노이즈)이 모두 평균이 0인 가우시안 분포(정규 분포)를 따라야 한다.1
문제는 현실 세계가 선형적이지 않다는 점이다.1 자율주행 자동차가 코너를 도는 움직임, 로봇팔이 관절을 꺾어 물체를 집는 동작, 심지어 레이더 센서가 물체의 위치를 측정하는 방식까지, 그 어떤 것도 완벽한 선형 관계로 설명되지 않는다. 예를 들어, 레이더는 물체까지의 거리($\rho$)와 방위각($\phi$)을 측정하는데, 이를 직교 좌표계의 위치 ($p_x, p_y$)로 변환하려면 $\rho = \sqrt{p_x^2 + p_y^2}$나 $\phi = \operatorname{atan2}(p_y, p_x)$와 같은 삼각함수와 제곱근이 포함된 비선형 함수가 필요하다.5
선형 칼만 필터의 수학적 아름다움과 최적성은 ‘가우시안 분포의 보존’이라는 특성에서 기인한다. 가우시안 분포를 따르는 확률 변수를 선형 변환(행렬 곱)하면, 그 결과 역시 가우시안 분포를 유지한다. 덕분에 우리는 상태를 평균과 공분산만으로 완벽하게 표현하고 추적할 수 있다.4 하지만 이 가우시안 분포가 비선형 함수를 통과하는 순간, 그 결과는 더 이상 아름다운 종 모양의 가우시안 분포가 아니라, 찌그러지고 비대칭적인 복잡한 형태의 분포로 변해버린다.4 이렇게 되면 평균과 공분산만으로는 상태의 불확실성을 제대로 표현할 수 없게 되고, 선형 칼만 필터의 수학적 기반이 송두리째 흔들리게 된다.6 이것이 바로 확장 칼만 필터가 해결해야만 하는 근본적인 문제다.
확장 칼만 필터(Extended Kalman Filter, EKF)의 핵심 아이디어는 매우 직관적이다. “전체 곡선을 한 번에 다룰 수 없다면, 우리가 관심 있는 한 지점 근처에서 아주 짧은 직선으로 잘라내어 근사하자”는 것이다.1 EKF는 다루기 힘든 비선형 함수를 현재 상태 추정치라는 특정 지점에서 ‘테일러 급수 전개(Taylor Series Expansion)’를 이용해 1차 선형 함수, 즉 접선으로 근사한다.7 그리고 이 근사된 선형 모델을 가지고 기존 선형 칼만 필터의 예측 및 업데이트 메커니즘을 거의 그대로 적용한다.
이 ‘순간적인’ 선형화는 EKF의 가장 큰 장점이자 동시에 가장 치명적인 약점이 된다.
EKF의 선형화 아이디어는 테일러 급수(Taylor Series)에서 출발한다. 어떤 미분 가능한 함수 $f(x)$를 특정 지점 $a$ 근처에서 다항식의 합으로 근사하는 방법이다.
\(f(x) = f(a) + \frac{f'(a)}{1!}(x-a) + \frac{f''(a)}{2!}(x-a)^2 + \dots\) EKF는 이 복잡한 식에서 과감하게 2차 이상의 고차항을 모두 무시하고 1차 항까지만 사용한다.7
\(f(x) \approx f(a) + f'(a)(x-a)\) 이것은 다름 아닌 점 $a$에서 함수 $f(x)$에 접하는 접선의 방정식이다. 즉, EKF는 복잡한 곡선을 아주 작은 구간에서는 직선으로 취급하겠다는 전략을 사용한다.7
하지만 우리가 다루는 시스템의 상태는 위치, 속도 등 여러 변수로 구성된 벡터($\mathbf{x}$)이고, 이를 통해 예측하거나 측정하는 값 역시 여러 변수로 구성된 벡터($\mathbf{y}$)다. 이처럼 입력과 출력이 모두 여러 개인 다변수 함수에서는 단순한 미분계수 $f’(a)$ 대신, 모든 입력 변수에 대한 모든 출력 함수의 편미분(partial derivative) 값을 모아놓은 행렬, 즉 자코비안 행렬(Jacobian Matrix)을 사용한다.1
예를 들어, 입력 벡터가 $\mathbf{x} = [x_1, x_2]^T$이고 출력 벡터가 $\mathbf{y} = f(\mathbf{x}) = [y_1(x_1, x_2), y_2(x_1, x_2)]^T$인 함수가 있다고 하자. 이 함수의 자코비안 행렬 $\mathbf{J}$는 다음과 같이 정의된다.
\[\mathbf{J} = \frac{\partial \mathbf{y}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_1}{\partial x_2} \\ \frac{\partial y_2}{\partial x_1} & \frac{\partial y_2}{\partial x_2} \end{bmatrix}\]EKF에서 자코비안 행렬은 선형 칼만 필터의 상태 전이 행렬 $\mathbf{A}$와 측정 행렬 $\mathbf{H}$의 역할을 비선형 시스템에서 대신 수행한다. 혼동을 피하기 위해 EKF에서는 이들을 각각 $\mathbf{F}$와 $\mathbf{H}$로 표기하는 경우가 많다.1 중요한 차이점은 이들이 더 이상 고정된 상수 행렬이 아니라는 점이다. 자코비안은 선형화의 기준점, 즉 현재 상태 추정치 $\hat{\mathbf{x}}$에 따라 그 값이 매번 달라지는 동적인 행렬이다.3 이 때문에 EKF는 매 스텝마다 자코비안을 새로 계산해야 하는 추가적인 계산 부담을 안게 된다.11
EKF가 다루는 비선형 시스템 모델은 일반적으로 다음과 같이 표현된다.2
\(\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}) + \mathbf{w}_{k-1}\) \(\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k\) 여기서 $f$는 현재 상태가 이전 상태로부터 어떻게 변하는지를 나타내는 비선형 상태 전이 함수이고, $h$는 현재 상태가 센서에서 어떻게 측정되는지를 나타내는 비선형 측정 함수다. $\mathbf{w}{k-1}$와 $\mathbf{v}_k$는 각각 평균이 0이고 공분산이 $\mathbf{Q}{k-1}$과 $\mathbf{R}_k$인 가우시안 분포를 따르는 프로세스 노이즈와 측정 노이즈를 의미한다.
| 예측 단계의 목표는 이전 시간($k-1$)의 최종 추정치($\hat{\mathbf{x}}_{k-1 | k-1}, \mathbf{P}_{k-1 | k-1}$)를 바탕으로 현재 시간($k$)의 상태($\hat{\mathbf{x}}_{k | k-1}$)와 그 불확실성($\mathbf{P}_{k | k-1}$)을 예측하는 것이다. |
상태 예측 (State Prediction) \(\hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k-1})\) 상태 자체를 예측할 때는 선형화된 모델이 아닌, 실제 비선형 상태 전이 함수 $f$를 직접 사용한다.3 이는 선형화로 인한 오차를 상태 예측 자체에는 개입시키지 않으려는 의도이며, 가능한 한 실제 시스템의 움직임을 그대로 반영하여 더 정확한 예측을 하기 위함이다.3
오차 공분산 예측 (Error Covariance Prediction)
\(\mathbf{P}_{k|k-1} = \mathbf{F}_{k-1} \mathbf{P}_{k-1|k-1} \mathbf{F}_{k-1}^T + \mathbf{Q}_{k-1}\) 상태의 불확실성이 시간이 지남에 따라 어떻게 커지는지를 예측한다. 여기서는 상태 자체가 아닌, 오차의 전파를 다루기 때문에 선형화된 모델, 즉 자코비안 $\mathbf{F}{k-1}$을 사용한다.3 수식의 구조는 선형 KF와 동일하며, 단지 상태 전이 행렬 $\mathbf{A}$가 자코비안 $\mathbf{F}{k-1}$으로 대체되었을 뿐이다. 이 자코비안은 상태 전이 함수 $f$를 이전 스텝의 최종 추정치 $\hat{\mathbf{x}}_{k-1|k-1}$에 대해 편미분하여 구한다.
\[\mathbf{F}_{k-1} = \left. \frac{\partial f}{\partial \mathbf{x}} \right|_{\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k-1}}\]| 업데이트 단계에서는 센서로부터 들어온 실제 측정값($\mathbf{z}k$)을 사용하여 예측 단계에서 계산한 예측치($\hat{\mathbf{x}}{k | k-1}, \mathbf{P}_{k | k-1}$)를 보정하고, 현재 시간($k$)의 최종 추정치($\hat{\mathbf{x}}_{k | k}, \mathbf{P}_{k | k}$)를 계산한다. |
혁신 또는 잔차 계산 (Innovation / Residual)
\(\tilde{\mathbf{y}}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})\) 예측된 측정값과 실제 측정값 사이의 차이를 계산한다.3 이 차이를 ‘혁신(Innovation)’ 또는 ‘잔차(Residual)’라고 부르는데, 이는 예측이 얼마나 ‘놀라운’ 결과를 낳았는지를 나타내기 때문이다. 상태 예측과 마찬가지로, 예측된 측정값을 계산할 때는 비선형 측정 함수
| $h$를 직접 사용한다. $h(\hat{\mathbf{x}}_{k | k-1})$는 “만약 우리의 상태 예측이 맞다면, 센서는 이런 값을 보여줬을 것이다”라는 의미를 가진다. |
혁신 공분산 계산 (Innovation Covariance)
\(\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k\) 이 ‘놀라움’이 얼마나 큰 불확실성을 가지는지를 계산한다.3 이는 두 가지 불확실성의 합으로 구성된다. 첫째는 예측된 상태의 불확실성($\mathbf{P}{k|k-1}$)이 측정 공간으로 투영된 것($\mathbf{H}_k \mathbf{P}{k|k-1} \mathbf{H}k^T$)이고, 둘째는 측정 자체의 불확실성($\mathbf{R}_k$)이다. 여기서 자코비안 $\mathbf{H}_k$는 측정 함수 $h$를 현재 스텝의 예측치 $\hat{\mathbf{x}}{k|k-1}$에 대해 편미분하여 구한다.
\(\mathbf{H}_k = \left. \frac{\partial h}{\partial \mathbf{x}} \right|_{\hat{\mathbf{x}}_{k|k-1}}\) 칼만 이득 계산 (Kalman Gain) \(\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1}\) 칼만 이득 $\mathbf{K}k$는 예측의 불확실성($\mathbf{P}{k|k-1}$)과 측정의 불확실성($\mathbf{S}_k$)을 종합적으로 고려하여, 우리가 계산한 ‘잔차’ $\tilde{\mathbf{y}}_k$를 얼마나 신뢰하고 반영할지를 결정하는 최적의 가중치다.3
상태 업데이트 (State Update)
\(\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \tilde{\mathbf{y}}_k\) 최종적으로 현재 상태를 추정한다. 예측된 상태 $\hat{\mathbf{x}}_{k|k-1}$에 ‘칼만 이득 × 잔차’ 만큼을 더하여 보정한다.3
오차 공분산 업데이트 (Error Covariance Update) \(\mathbf{P}_{k|k} = (I - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}\) 측정값을 통해 새로운 정보를 얻었으므로, 상태의 불확실성은 예측 단계보다 줄어들어야 한다. $(I - \mathbf{K}_k \mathbf{H}_k)$ 항은 이 불확실성 감소 효과를 수학적으로 나타낸다.3
| 여기서 한 가지 중요한 질문이 생긴다. 왜 상태 전이 자코비안 $\mathbf{F}{k-1}$은 이전 스텝의 최종 추정치 $\hat{\mathbf{x}}{k-1 | k-1}$을 기준으로 계산하고, 측정 자코비안 $\mathbf{H}k$는 현재 스텝의 예측치 $\hat{\mathbf{x}}{k | k-1}$을 기준으로 계산하는 것일까? 이는 각 자코비안이 사용되는 목적과 시점을 생각해보면 명확해진다. 핵심 원리는 “각 계산이 수행되는 시점에서 가장 최신의, 그리고 가장 적절한 정보를 사용한다”는 것이다. |
| $\mathbf{F}{k-1}$의 역할:** 자코비안 $\mathbf{F}{k-1}$은 $k-1$ 시점에서 $k$ 시점으로의 **오차 공분산을 예측하는 데 사용된다 ($\mathbf{P}_{k | k-1} = \mathbf{F}{k-1} \mathbf{P}{k-1 | k-1} \mathbf{F}{k-1}^T + \mathbf{Q}{k-1}$). 이 계산은 $k$ 시점의 측정값이 들어오기 전에 수행된다. 이 시점에서 우리가 가진 가장 정확하고 보정된 상태 정보는 바로 $k-1$ 시점의 모든 정보를 종합하여 얻은 최종 추정치, 즉 $\hat{\mathbf{x}}_{k-1 | k-1}$이다. 따라서 이 지점에서 운동 모델 $f$를 선형화하여 불확실성을 전파시키는 것이 가장 합리적인 선택이다.3 |
| **$\mathbf{H}k$의 역할:** 자코비안 $\mathbf{H}_k$는 $k$ 시점의 측정값을 이용해 예측을 보정하는 업데이트 단계에서 사용된다. 이 단계의 핵심은 실제 측정값 $\mathbf{z}_k$와 예측된 측정값 $h(\hat{\mathbf{x}}{k | k-1})$을 비교하는 것이다. 자코비안 $\mathbf{H}k$는 상태 예측치 $\hat{\mathbf{x}}{k | k-1}$의 작은 변화가 예측된 측정값 $h(\hat{\mathbf{x}}_{k | k-1})$에 어떤 영향을 미치는지를 나타내는 선형 관계를 모델링한다. 따라서 이 둘 사이의 관계를 가장 잘 나타내는 지역적 선형 모델을 얻기 위해서는, 당연히 예측치 $\hat{\mathbf{x}}_{k | k-1}$를 기준으로 측정 함수 $h$를 선형화해야 한다.3 |
결론적으로, 각 자코비안은 해당 계산이 수행되는 시점에서 가장 최신의, 그리고 가장 적절한 상태 추정치를 기준으로 선형화된다. 이는 EKF가 매 스텝마다 변화하는 상황에 맞춰 ‘최선의’ 선형 모델을 동적으로 찾아가려는 노력의 일환이라고 볼 수 있다.
| 단계 (Step) | 목표 (Goal) | 상태 계산 (State Calculation) | 공분산 계산 (Covariance Calculation) | 핵심 변수 (Key Variables) | 사용 함수 (Functions Used) | ||
|---|---|---|---|---|---|---|---|
| 예측 (Prediction) | 이전 상태로부터 현재 상태와 불확실성을 예측 | $\hat{\mathbf{x}}_{k | k-1} = f(\hat{\mathbf{x}}_{k-1 | k-1}, \mathbf{u}_{k-1})$ | $\mathbf{P}_{k | ||
| 업데이트 (Update) | 측정값을 이용해 예측을 보정하고 최종 추정치 생성 | $\hat{\mathbf{x}}_{k | k} = \hat{\mathbf{x}}_{k | k-1} + \mathbf{K}_k \tilde{\mathbf{y}}_k$ | $\mathbf{P}_{k |
이제 이론을 실제 문제에 적용해 보자. 자율주행이나 로봇 공학에서 가장 흔하게 접하는 문제 중 하나는 움직이는 차량을 추적하는 것이다. 이를 위해 널리 사용되는 CTRV(Constant Turn Rate and Velocity) 모델을 EKF에 적용하는 과정을 단계별로 살펴보자.
CTRV 모델은 이름에서 알 수 있듯이, 아주 짧은 시간 간격($\Delta t$) 동안에는 차량의 선회율(turn rate, $\dot{\psi}$)과 속력(velocity, $v$)이 일정하다고 가정하는 운동 모델이다.12 이는 단순히 등속 직선 운동(Constant Velocity, CV)을 가정하는 것보다 회전하는 차량의 움직임을 훨씬 더 현실적으로 묘사할 수 있어 널리 사용된다.
상태 전이 함수 $f(\mathbf{x})$는 $k-1$ 시점의 상태 $\mathbf{x}_{k-1}$이 주어졌을 때, $\Delta t$ 시간이 흐른 뒤의 $k$ 시점의 상태 $\mathbf{x}_k$를 예측하는 비선형 함수다. 각 상태 변수가 어떻게 변하는지 살펴보자.
속력 $v$, 주행 방향 $\psi$, 선회율 $\dot{\psi}$의 업데이트는 CTRV 가정에 따라 매우 간단하다.
위치 $p_x, p_y$의 업데이트는 적분을 통해 계산해야 한다. 시간 $t$에서의 위치 변화율은 $\dot{p}x(t) = v(t) \cos(\psi(t))$, $\dot{p}_y(t) = v(t) \sin(\psi(t))$이다. CTRV 가정에 따라 $v(t) = v{k-1}$이고 $\psi(t) = \psi_{k-1} + \dot{\psi}{k-1}(t-t{k-1})$이므로, 이를 $t_{k-1}$부터 $t_k$까지 $\Delta t$ 동안 적분하면 된다.12
선회율이 0이 아닐 때는 삼각함수의 적분을 통해 다음과 같은 식을 얻는다.12
\[\begin{align*} p_{x,k} &= p_{x,k-1} + \int_{t_{k-1}}^{t_k} v_{k-1} \cos(\psi_{k-1} + \dot{\psi}_{k-1}(t-t_{k-1})) dt \\ &= p_{x,k-1} + \left[ \frac{v_{k-1}}{\dot{\psi}_{k-1}} \sin(\psi_{k-1} + \dot{\psi}_{k-1}(t-t_{k-1})) \right]_{t_{k-1}}^{t_k} \\ &= p_{x,k-1} + \frac{v_{k-1}}{\dot{\psi}_{k-1}} (\sin(\psi_{k-1} + \dot{\psi}_{k-1}\Delta t) - \sin(\psi_{k-1})) \end{align*}\]\(\\)
\begin{align}
p_{y,k} &= p_{y,k-1} + \int_{t_{k-1}}^{t_k} v_{k-1} \sin(\psi_{k-1} + \dot{\psi}_{k-1}(t-t_{k-1})) dt
&= p_{y,k-1} + \left[ -\frac{v_{k-1}}{\dot{\psi}_{k-1}} \cos(\psi_{k-1} + \dot{\psi}_{k-1}(t-t_{k-1})) \right]_{t_{k-1}}^{t_k}
&= p_{y,k-1} + \frac{v_{k-1}}{\dot{\psi}_{k-1}} (-\cos(\psi_{k-1} + \dot{\psi}_{k-1}\Delta t) + \cos(\psi_{k-1}))
\end{align}
$$
위 식에서 $\dot{\psi}{k-1}$이 0에 가까워지면 분모가 0이 되어 수치적으로 불안정해지는 문제가 발생한다. 이는 단순히 수학적 특이점(singularity)일 뿐만 아니라 물리적 현실을 반영한다. 선회율이 0이라는 것은 차량이 회전하지 않고 직선 운동을 한다는 의미다. 이 경우, $\psi(t)$는 $\psi{k-1}$로 상수이므로 적분이 훨씬 간단해진다.12
\(\begin{align*}
p_{x,k} &= p_{x,k-1} + \int_{t_{k-1}}^{t_k} v_{k-1} \cos(\psi_{k-1}) dt = p_{x,k-1} + v_{k-1} \cos(\psi_{k-1}) \Delta t \\
p_{y,k} &= p_{y,k-1} + \int_{t_{k-1}}^{t_k} v_{k-1} \sin(\psi_{k-1}) dt = p_{y,k-1} + v_{k-1} \sin(\psi_{k-1}) \Delta t
\end{align*}\)
실제 코드를 구현할 때는 if abs(yaw_rate) < 0.0001: 과 같이 아주 작은 임계값을 설정하여 이 두 경우를 분기 처리하는 것이 필수적이다. 이 분기 처리는 수학적 오류를 피하는 것뿐만 아니라, 물리적으로 다른 두 운동 모델(원운동과 직선운동)을 올바르게 전환하는 과정이다.12
이제 EKF 구현의 가장 까다로운 부분인 자코비안 $\mathbf{F}k$를 유도할 차례다. $\mathbf{F}_k$는 상태 전이 함수 $f(\mathbf{x})$의 5개 출력 함수($p{x,k}, p_{y,k}, v_k, \psi_k, \dot{\psi}k$)를 5개의 입력 상태 변수($p{x,k-1}, p_{y,k-1}, v_{k-1}, \psi_{k-1}, \dot{\psi}_{k-1}$) 각각에 대해 편미분한 5x5 행렬이다.15
\(\mathbf{F}_k = \left. \frac{\partial f}{\partial \mathbf{x}} \right|_{\hat{\mathbf{x}}_{k-1|k-1}}\) 대부분의 항은 미분하면 0, 1, 또는 $\Delta t$가 되어 간단하다. 예를 들어, $\frac{\partial p_{x,k}}{\partial p_{x,k-1}} = 1$, $\frac{\partial \psi_k}{\partial \psi_{k-1}} = 1$, $\frac{\partial \psi_k}{\partial \dot{\psi}_{k-1}} = \Delta t$ 이다. 복잡한 부분은 위치 $p_x, p_y$를 속도, 주행 방향, 선회율($v, \psi, \dot{\psi}$)로 편미분하는 항들이다.
편의상 $v = v_{k-1}, \psi = \psi_{k-1}, \dot{\psi} = \dot{\psi}_{k-1}$ 라고 간단히 표기하자.
이를 모두 모으면 자코비안 $\mathbf{F}_k$는 다음과 같다.
\[\mathbf{F}_k = \begin{bmatrix} 1 & 0 & \frac{1}{\dot{\psi}}(\sin(\psi+\dot{\psi}\Delta t) - \sin(\psi)) & \frac{v}{\dot{\psi}}(\cos(\psi+\dot{\psi}\Delta t) - \cos(\psi)) & \frac{\partial p_{x,k}}{\partial \dot{\psi}} \\ 0 & 1 & \frac{1}{\dot{\psi}}(-\cos(\psi+\dot{\psi}\Delta t) + \cos(\psi)) & \frac{v}{\dot{\psi}}(\sin(\psi+\dot{\psi}\Delta t) - \sin(\psi)) & \frac{\partial p_{y,k}}{\partial \dot{\psi}} \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}\]직진 운동 시에는 상태 전이 함수가 더 단순하므로 자코비안도 간단해진다.
이때의 자코비안은 다음과 같다.
\(\mathbf{F}_k = \begin{bmatrix} 1 & 0 & \cos(\psi)\Delta t & -v\sin(\psi)\Delta t & -\frac{1}{2}v\Delta t^2 \sin(\psi) \\ 0 & 1 & \sin(\psi)\Delta t & v\cos(\psi)\Delta t & \frac{1}{2}v\Delta t^2 \cos(\psi) \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}\) 이처럼 자코비안을 손으로 직접 유도하는 과정은 EKF 구현에서 가장 까다롭고 버그가 발생하기 쉬운 부분이다. 사소한 부호 실수나 미분 계산 착오가 필터 전체의 성능을 망가뜨릴 수 있다. 바로 이 점이 많은 개발자들이 자코비안 계산이 필요 없는 무향 칼만 필터(UKF)를 선호하게 되는 주된 이유 중 하나다.18 하지만 이 과정을 상세히 따라가 보는 것은 EKF의 실제적인 어려움과 그 작동 원리를 깊이 이해하는 데 필수적인 학습 과정이다.
이제 EKF의 또 다른 핵심 응용 분야인 센서 퓨전(Sensor Fusion)을 살펴보자. 자율주행차는 보통 라이다, 레이더, 카메라 등 여러 종류의 센서를 함께 사용하는데, 각 센서의 장단점을 보완하여 더 강건하고 정확한 인지를 하기 위함이다. EKF는 이처럼 서로 다른 종류의 측정값을 하나의 일관된 상태 추정치로 통합하는 데 매우 효과적인 도구다.
측정 모델 $h(\mathbf{x})$는 추상적인 상태 공간(State Space)의 변수들을 센서가 실제로 측정하는 물리적인 측정 공간(Measurement Space)으로 변환하는 다리 역할을 한다.19 예를 들어, CTRV 모델의 상태 벡터 $\mathbf{x} = [p_x, p_y, v, \psi, \dot{\psi}]^T$를 레이더가 측정하는 ($\rho, \phi, \dot{\rho}$) 값으로 변환하는 함수가 바로 레이더의 측정 모델 $h_{Radar}(\mathbf{x})$이다.
라이다(Lidar)는 레이저 펄스를 이용해 주변 환경을 스캔하고, 매우 정밀한 3D 포인트 클라우드를 생성한다. 차량 추적의 맥락에서, 라이다는 보통 객체의 직교 좌표계 위치 ($p_x, p_y$)를 직접적으로 측정한다고 모델링할 수 있다.
측정 벡터: $\mathbf{z}_{Lidar} = [p_x, p_y]^T$
측정 함수: 상태 벡터 $\mathbf{x}$에서 위치 정보인 $p_x$와 $p_y$를 그대로 가져오면 된다. 이는 선형 변환으로 표현할 수 있다.
\[h_{Lidar}(\mathbf{x}) = \begin{bmatrix} p_x \\ p_y \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{bmatrix} \mathbf{x}\]자코비안 $\mathbf{H}_{Lidar}$: 측정 함수가 이미 선형이므로, 이를 상태 벡터 $\mathbf{x}$에 대해 편미분한 자코비안은 상수 행렬이 된다.
\[\mathbf{H}_{Lidar} = \frac{\partial h_{Lidar}}{\partial \mathbf{x}} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{bmatrix}\]라이다의 측정 모델은 선형이기 때문에, 라이다 측정값으로 업데이트를 수행할 때는 EKF가 사실상 선형 칼만 필터처럼 동작한다. 비선형성은 시스템의 움직임을 예측하는 단계와, 아래에서 다룰 비선형적인 레이더 측정값을 처리하는 단계에서만 나타난다.
레이더(Radar)는 전파를 이용하여 객체를 탐지하며, 보통 극좌표계(polar coordinates)로 측정값을 제공한다: 센서로부터 객체까지의 거리(range, $\rho$), 방위각(bearing, $\phi$), 그리고 거리가 변하는 속도(range rate, $\dot{\rho}$).5 이 측정값들과 상태 변수 ($p_x, p_y, v, \psi$) 사이의 관계는 비선형적이다.
측정 벡터: $\mathbf{z}_{Radar} = [\rho, \phi, \dot{\rho}]^T$
측정 함수 $h_{Radar}(\mathbf{x})$: 상태 벡터 $\mathbf{x}$를 레이더 측정 벡터 $\mathbf{z}_{Radar}$로 변환하는 함수는 다음과 같다.
\(h_{Radar}(\mathbf{x}) = \begin{bmatrix} \rho \\ \phi \\ \dot{\rho} \end{bmatrix} = \begin{bmatrix} \sqrt{p_x^2 + p_y^2} \\ \operatorname{atan2}(p_y, p_x) \\ \frac{p_x v \cos(\psi) + p_y v \sin(\psi)}{\sqrt{p_x^2 + p_y^2}} \end{bmatrix}\) 여기서 $\dot{\rho}$ 유도 과정은 다음과 같다. 위치 벡터 $\mathbf{p} = [p_x, p_y]^T$와 속도 벡터 $\mathbf{v}_{xy} = [v\cos(\psi), v\sin(\psi)]^T$의 내적을 거리 $\rho$로 나눈 값, 즉 속도 벡터를 위치 벡터 방향으로 투영(projection)한 크기다. 이는 $\rho^2 = p_x^2 + p_y^2$를 시간에 대해 미분하여 유도할 수 있다: $2\rho\dot{\rho} = 2p_x\dot{p}_x + 2p_y\dot{p}_y$. 따라서 $\dot{\rho} = (p_x\dot{p}_x + p_y\dot{p}_y)/\rho$ 이고, $\dot{p}_x$와 $\dot{p}_y$에 CTRV 모델의 속도 성분을 대입하면 위 식을 얻는다.5
자코비안 $\mathbf{H}{Radar}$: 이제 측정 함수 $h{Radar}$를 5개의 상태 변수($p_x, p_y, v, \psi, \dot{\psi}$) 각각에 대해 편미분하여 3x5 크기의 자코비안 행렬 $\mathbf{H}_{Radar}$를 구해야 한다. 이 과정은 연쇄 법칙(chain rule)과 분수, 제곱근, 아크탄젠트 함수의 미분 등 다소 복잡한 계산을 요구한다.5
\(\mathbf{H}_{Radar} = \frac{\partial h_{Radar}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial \rho}{\partial p_x} & \frac{\partial \rho}{\partial p_y} & \frac{\partial \rho}{\partial v} & \frac{\partial \rho}{\partial \psi} & \frac{\partial \rho}{\partial \dot{\psi}} \\ \frac{\partial \phi}{\partial p_x} & \frac{\partial \phi}{\partial p_y} & \frac{\partial \phi}{\partial v} & \frac{\partial \phi}{\partial \psi} & \frac{\partial \phi}{\partial \dot{\psi}} \\ \frac{\partial \dot{\rho}}{\partial p_x} & \frac{\partial \dot{\rho}}{\partial p_y} & \frac{\partial \dot{\rho}}{\partial v} & \frac{\partial \dot{\rho}}{\partial \psi} & \frac{\partial \dot{\rho}}{\partial \dot{\psi}} \end{bmatrix}\) 각 항을 계산해 보면 다음과 같다. (편의상 $\rho = \sqrt{p_x^2+p_y^2}$로 표기)
이 자코비안 행렬의 각 원소는 중요한 물리적 의미를 담고 있다. 이는 단순히 계산을 위한 도구가 아니라, 특정 측정값이 각 상태 변수에 대해 얼마나 민감한 정보(sensitivity)를 담고 있는지를 나타내는 지표이기도 하다. 예를 들어, $\frac{\partial \phi}{\partial v} = 0$ 이라는 사실은 레이더의 방위각 측정값 $\phi$가 차량의 속력 $v$를 추정하는 데 아무런 직접적인 정보를 주지 못한다는 것을 의미한다. 반면, $\frac{\partial \dot{\rho}}{\partial v}$ 항은 0이 아니므로, 거리 변화율 $\dot{\rho}$ 측정값은 속력 $v$를 추정하는 데 직접적인 단서가 된다. 이처럼 자코비안을 분석하면 어떤 센서가 어떤 상태 변수를 추정하는 데 더 기여하는지 통찰을 얻을 수 있다.
| 특성 (Characteristic) | 라이다 (Lidar) | 레이더 (Radar) |
|---|---|---|
| 측정 벡터 $\mathbf{z}$ | $[p_x, p_y]^T$ (직교 좌표) | $[\rho, \phi, \dot{\rho}]^T$ (극 좌표) |
| 측정 함수 $h(\mathbf{x})$ | $\begin{bmatrix} p_x \ p_y \end{bmatrix}$ | $\begin{bmatrix} \sqrt{p_x^2+p_y^2} \ \operatorname{atan2}(p_y, p_x) \ \frac{p_x (v \cos\psi) + p_y (v \sin\psi)}{\sqrt{p_x^2+p_y^2}} \end{bmatrix}$ |
| 선형성 (Linearity) | 선형 (Linear) | 비선형 (Non-linear) |
| 자코비안 $\mathbf{H}$ | 상수 행렬, 계산 용이 | 상태 의존적, 계산 복잡 |
| 주요 제공 정보 | 정확한 위치 | 위치, 방향, 그리고 직접적인 속도 정보 |
| 장점 | 높은 위치 정밀도, 조밀한 데이터 | 전천후 성능, 속도 직접 측정 가능, 원거리 탐지 |
| 단점 | 악천후(비, 눈, 안개)에 취약, 속도 직접 측정 불가 | 낮은 위치 정밀도, 희소한 데이터 |
EKF는 수많은 비선형 문제에 성공적으로 적용되어 왔지만, 근본적인 한계를 가지고 있다. 이 한계를 이해하는 것은 더 발전된 필터인 무향 칼만 필터(UKF)와 파티클 필터(PF)의 필요성을 이해하는 데 중요하다.
EKF는 다음과 같은 상황에서 심각한 성능 저하를 겪거나 발산할 수 있다.
if 문이나 abs() 함수처럼 미분 불가능한 지점이 포함되어 있다면, 자코비안 자체를 계산할 수 없으므로 EKF를 원천적으로 적용할 수 없다.EKF에서 UKF, 그리고 PF로의 발전은 단순히 정확도의 점진적 향상이 아니다. 이는 비선형성을 다루는 핵심 전략의 근본적인 철학적 변화를 나타낸다.
무향 칼만 필터(Unscented Kalman Filter, UKF)는 EKF의 선형화 오차 문제를 해결하기 위해 등장했다. UKF의 핵심 철학은 “비선형 함수를 근사하는 것보다, 확률 분포를 근사하는 것이 더 쉽고 정확하다”는 것이다.22
UKF의 동작 방식은 다음과 같다.
EKF가 분포의 평균 지점 단 하나만을 사용하여 선형화를 수행하는 반면, UKF는 분포의 ‘형태’(분산)까지 고려한 여러 개의 지점(시그마 포인트)을 사용한다. 이 덕분에 UKF는 비선형 변환 후의 실제 평균과 공분산을 EKF보다 훨씬 더 정확하게(테일러 급수 기준으로 최소 2차 항까지, 종종 3차 항까지) 근사할 수 있다.27 자코비안을 계산할 필요가 없어 복잡한 편미분 유도 과정에서 해방된다는 점은 매우 큰 실용적 장점이다.18
파티클 필터(Particle Filter, PF)는 EKF와 UKF가 여전히 고수하고 있는 ‘가우시안’이라는 가정마저 완전히 버린, 가장 유연한 비선형 필터다. PF의 핵심 아이디어는 어떤 복잡한 형태의 확률 분포라도, 수많은 무작위 샘플, 즉 파티클(Particles)들의 집합으로 근사할 수 있다는 것이다.25
PF의 동작 방식은 직관적인 몬테카를로 시뮬레이션에 가깝다.
PF는 가우시안 가정을 완전히 버렸기 때문에, 확률 분포가 두 개 이상의 봉우리를 갖는 다봉성(multi-modal) 문제나 매우 비대칭적인 분포를 갖는 문제를 유일하게 제대로 해결할 수 있다. 예를 들어, 로봇이 복도를 가다가 누군가에게 납치되어 다른 위치로 순간 이동했다고 가정해보자. 이 경우 로봇의 위치에 대한 확률 분포는 원래 있던 위치와 새로 옮겨진 위치, 두 곳에 봉우리를 갖게 된다. EKF나 UKF는 이 두 봉우리의 어중간한 중간 지점을 평균으로 계산하여 완전히 틀린 추정을 할 수 있지만, PF는 두 위치에 모두 파티클을 유지함으로써 이러한 상황을 올바르게 표현할 수 있다.6
| 기준 (Criteria) | 확장 칼만 필터 (EKF) | 무향 칼만 필터 (UKF) | 파티클 필터 (PF) |
|---|---|---|---|
| 핵심 아이디어 | 비선형 함수를 선형으로 근사 (테일러 1차) | 확률 분포를 시그마 포인트로 근사 | 확률 분포를 파티클(샘플)로 근사 |
| 비선형성 처리 | 자코비안 행렬을 통한 선형화 | 무향 변환 (Unscented Transform) | 순차적 몬테카를로 (SMC) |
| 확률 분포 가정 | 가우시안 (Gaussian) | 가우시안 (Gaussian) | 임의의 분포 (Non-Gaussian) |
| 장점 | 계산적으로 가장 효율적, 구현이 (비교적) 간단 | EKF보다 정확, 자코비안 불필요 | 매우 유연, 비가우시안/다봉성 문제 해결 가능 |
| 단점 | 강한 비선형성에 취약, 발산 위험, 자코비안 유도 필요 | EKF보다 계산량 많음, 여전히 가우시안 가정 | 계산량 가장 많음, 파티클 수에 성능 의존, 파티클 고갈(degeneracy) 문제 |
| 계산 복잡도 | 낮음 (상태 차원 $n$에 대해 $O(n^3)$) | 중간 ($O(n^3)$, EKF보다 상수배 높음) | 높음 (파티클 수 $N$에 대해 $O(N)$) |
| 언제 사용하는가? | 시스템이 약한 비선형성을 띠고, 계산 자원이 제한적일 때 18 | 시스템이 강한 비선형성을 띠지만, 분포는 가우시안으로 가정할 수 있을 때 9 | 시스템이 매우 비선형적이거나, 분포가 명백히 비가우시안일 때 25 |
확장 칼만 필터는 비선형 상태 추정의 세계로 들어가는 첫 관문이자, 여전히 많은 분야에서 현역으로 활약하는 강력한 도구다. EKF의 성공적인 적용은 알고리즘 자체의 이해를 넘어, 모델링과 튜닝의 디테일에 달려 있다.
EKF를 구현하고 디버깅할 때 다음 체크리스트를 따라가면 많은 실수를 예방할 수 있다.
| 초기값의 합리성: 초기 상태 $\hat{\mathbf{x}}_{0 | 0}$와 초기 공분산 $\mathbf{P}_{0 | 0}$을 합리적으로 설정했는가? 초기 상태를 전혀 모른다면 공분산을 큰 값으로 설정하여 필터가 초기에는 측정값을 더 신뢰하도록 만들 수 있다. 하지만 너무 큰 값은 수치적 불안정성을 야기할 수 있다. |
이론을 이해했다면 직접 코드를 작성해보는 것이 최고의 학습 방법이다. numpy 라이브러리를 사용하면 EKF의 행렬 연산을 효율적으로 구현할 수 있다. 다음은 EKF 업데이트 단계의 핵심 로직을 보여주는 Python 코드 스니펫의 예시다.
# H는 상태에 따라 계산된 자코비안 행렬
# P_pred는 예측된 공분산, R은 측정 노이즈 공분산
# np는 numpy 라이브러리
S = H @ P_pred @ H.T + R
# 칼만 이득 계산
K = P_pred @ H.T @ np.linalg.inv(S)
# z는 실제 측정값, hx는 예측된 측정값 (h(x_pred))
y = z - hx
# 상태 및 공분산 업데이트
x_est = x_pred + K @ y
P_est = (np.eye(n_states) - K @ H) @ P_pred
CTRV 모델과 라이다/레이더 센서 퓨전을 구현한 완전한 형태의 Python 예제는 GitHub 등에서 쉽게 찾아볼 수 있다.28 또한, 모든 것을 밑바닥부터 구현할 필요는 없다.
FilterPy와 같은 잘 알려진 오픈소스 라이브러리는 EKF, UKF 등 다양한 필터를 구현해 놓아 사용자가 자신의 문제에 집중할 수 있도록 돕는다.30
이 문서를 통해 EKF의 기초를 다졌다면, 더 깊이 있는 학습을 위해 다음 자료들을 참고하는 것이 좋다.
EKF는 완벽하지 않다. 하지만 그 한계를 명확히 이해하고, 문제의 특성에 맞게 UKF나 파티클 필터와 같은 대안을 고려할 수 있을 때, 비로소 진정한 상태 추정 전문가로 거듭날 수 있을 것이다.
| The math behind Extended Kalman Filtering | by Sasha Przybylski - Medium, accessed July 29, 2025, https://medium.com/@sasha_przybylski/the-math-behind-extended-kalman-filtering-0df981a87453 |
| Comparative evaluation of Kalman filters and motion models in vehicular state estimation and path prediction | The Journal of Navigation - Cambridge University Press, accessed July 29, 2025, https://www.cambridge.org/core/journals/journal-of-navigation/article/comparative-evaluation-of-kalman-filters-and-motion-models-in-vehicular-state-estimation-and-path-prediction/8E194806C5BBE5249A351067801E9CE9 |
| LiDAR and Radar Sensor Fusion using Unscented Kalman Filter | by Nikhil Nair | Medium, accessed July 29, 2025, https://medium.com/@nikhilnair8490/lidar-and-radar-sensor-fusion-using-unscented-kalman-filter-5b20de0ab1d1 |