6.141 확장 칼만 필터와 자코비안의 역할

1. 비선형 시스템과 확장 칼만 필터

실제 로봇 시스템의 동역학과 관측 모델은 대부분 비선형이다. 확장 칼만 필터(Extended Kalman Filter, EKF)는 비선형 시스템에 대하여 칼만 필터를 적용하기 위한 가장 기본적인 방법으로, 비선형 함수를 현재 추정값에서의 자코비안(Jacobian)을 이용하여 선형화한다.

비선형 시스템 모델은 다음과 같다.

상태 전이 모델:

\mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k

관측 모델:

\mathbf{z}_k = \mathbf{h}(\mathbf{x}_k) + \mathbf{v}_k

여기서 \mathbf{f}: \mathbb{R}^n \times \mathbb{R}^l \to \mathbb{R}^n은 비선형 상태 전이 함수이고, \mathbf{h}: \mathbb{R}^n \to \mathbb{R}^m은 비선형 관측 함수이다. \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k), \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k)는 가우시안 잡음이다.

2. 자코비안 행렬의 정의와 계산

2.1 상태 전이 자코비안

상태 전이 함수 \mathbf{f}의 자코비안 행렬 \mathbf{F}_k는 현재 추정값 \hat{\mathbf{x}}_{k\vert k}에서 평가한다.

\mathbf{F}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} \bigg\vert_{\hat{\mathbf{x}}_{k\vert k}, \mathbf{u}_k} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \cdots & \frac{\partial f_2}{\partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_n}{\partial x_1} & \frac{\partial f_n}{\partial x_2} & \cdots & \frac{\partial f_n}{\partial x_n} \end{bmatrix}_{\hat{\mathbf{x}}_{k\vert k}, \mathbf{u}_k}

\mathbf{F}_k \in \mathbb{R}^{n \times n}의 (i,j) 원소는 j번째 상태 변수의 미소 변화가 i번째 상태 변수의 다음 시각 값에 미치는 영향의 1차 근사를 나타낸다.

2.2 관측 자코비안

관측 함수 \mathbf{h}의 자코비안 행렬 \mathbf{H}_k는 사전 추정값 \hat{\mathbf{x}}_{k\vert k-1}에서 평가한다.

\mathbf{H}_k = \frac{\partial \mathbf{h}}{\partial \mathbf{x}} \bigg\vert_{\hat{\mathbf{x}}_{k\vert k-1}} \in \mathbb{R}^{m \times n}

\mathbf{H}_k의 (i,j) 원소는 j번째 상태 변수의 변화가 i번째 관측값에 미치는 영향의 1차 근사이다.

3. EKF 알고리즘

3.1 예측 단계

\hat{\mathbf{x}}_{k+1\vert k} = \mathbf{f}(\hat{\mathbf{x}}_{k\vert k}, \mathbf{u}_k)

\mathbf{P}_{k+1\vert k} = \mathbf{F}_k \mathbf{P}_{k\vert k} \mathbf{F}_k^T + \mathbf{Q}_k

상태 예측에서는 비선형 함수 \mathbf{f}를 직접 적용하고, 공분산 전파에서는 자코비안 \mathbf{F}_k를 이용한 선형 근사를 사용한다.

3.2 갱신 단계

\tilde{\mathbf{z}}_k = \mathbf{z}_k - \mathbf{h}(\hat{\mathbf{x}}_{k\vert k-1})

\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k\vert k-1} \mathbf{H}_k^T + \mathbf{R}_k

\mathbf{K}_k = \mathbf{P}_{k\vert k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1}

\hat{\mathbf{x}}_{k\vert k} = \hat{\mathbf{x}}_{k\vert k-1} + \mathbf{K}_k \tilde{\mathbf{z}}_k

\mathbf{P}_{k\vert k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k\vert k-1}

혁신 계산에서도 비선형 함수 \mathbf{h}를 직접 적용하되, 칼만 이득과 공분산 갱신에서는 자코비안 \mathbf{H}_k를 사용한다.

4. 로봇공학에서의 자코비안 계산 예

4.1 차동 구동 로봇(Differential Drive Robot)

상태 벡터 \mathbf{x} = [x, y, \theta]^T, 제어 입력 \mathbf{u} = [v, \omega]^T(선속도, 각속도)에 대한 운동 모델은 다음과 같다.

\mathbf{f}(\mathbf{x}, \mathbf{u}) = \begin{bmatrix} x + v \Delta t \cos\theta \\ y + v \Delta t \sin\theta \\ \theta + \omega \Delta t \end{bmatrix}

상태 전이 자코비안은 다음과 같다.

\mathbf{F} = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{bmatrix} 1 & 0 & -v \Delta t \sin\theta \\ 0 & 1 & v \Delta t \cos\theta \\ 0 & 0 & 1 \end{bmatrix}

이 행렬의 구조를 분석하면 다음을 알 수 있다.

• 위치 (x, y)는 이전 위치에 직접 의존한다 (대각 원소 1).
• 방향 \theta의 변화는 위치 갱신에 비선형적으로 영향을 미친다 (\sin\theta, \cos\theta 항).
• \mathbf{F}는 상삼각에 가까운 구조를 가지며, 이는 방향이 위치에 영향을 주지만 역은 아님을 반영한다.

4.2 거리-방위 관측 모델(Range-Bearing Observation)

랜드마크의 세계 좌표 (l_x, l_y)에 대한 거리-방위 관측 모델은

\mathbf{h}(\mathbf{x}) = \begin{bmatrix} \sqrt{(l_x - x)^2 + (l_y - y)^2} \\ \text{atan2}(l_y - y, l_x - x) - \theta \end{bmatrix}

관측 자코비안은 다음과 같다.

\mathbf{H} = \begin{bmatrix} -\frac{l_x - x}{r} & -\frac{l_y - y}{r} & 0 \\ \frac{l_y - y}{r^2} & -\frac{l_x - x}{r^2} & -1 \end{bmatrix}

여기서 r = \sqrt{(l_x - x)^2 + (l_y - y)^2}이다.

이 자코비안의 특성은 다음과 같다.

• 거리 관측의 자코비안(1행)은 로봇에서 랜드마크 방향의 단위 벡터에 비례한다.
• 방위 관측의 자코비안(2행)은 1/r^2에 비례하므로, 가까운 랜드마크에 대한 관측이 더 많은 정보를 제공한다.
• r \to 0이면 자코비안이 특이(singular)에 가까워지므로, 수치적으로 불안정해질 수 있다.

4.3 로봇 팔의 정기구학

n개의 관절을 가진 로봇 팔의 말단 장치 위치 \mathbf{p} = \mathbf{h}(\mathbf{q})에 대한 관측 자코비안은 기하학적 자코비안(geometric Jacobian)에 해당한다.

\mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{h}}{\partial \mathbf{q}} \in \mathbb{R}^{6 \times n}

이 자코비안은 관절 속도와 말단 속도의 관계 \dot{\mathbf{p}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}를 나타내며, EKF에서 관절 각도의 불확실성을 말단 장치 위치의 불확실성으로 전파하는 데 사용된다.

\mathbf{P}_{\text{end-effector}} = \mathbf{J}(\mathbf{q}) \mathbf{P}_q \mathbf{J}(\mathbf{q})^T

5. 자코비안의 수치적 계산

해석적 자코비안을 구하기 어려운 경우, 유한 차분(finite difference)으로 근사할 수 있다.

전방 차분:

\frac{\partial f_i}{\partial x_j} \approx \frac{f_i(\mathbf{x} + \delta \mathbf{e}_j) - f_i(\mathbf{x})}{\delta}

중심 차분:

\frac{\partial f_i}{\partial x_j} \approx \frac{f_i(\mathbf{x} + \delta \mathbf{e}_j) - f_i(\mathbf{x} - \delta \mathbf{e}_j)}{2\delta}

여기서 \mathbf{e}_j는 j번째 표준 기저 벡터이다. 중심 차분은 2차 정확도를 가지며, 전방 차분의 1차 정확도보다 우수하다. n차원 자코비안을 계산하기 위해서는 전방 차분에서 n번, 중심 차분에서 2n번의 함수 평가가 필요하다.

복소수 스텝(complex-step) 방법은 f_i(\mathbf{x} + i\delta\mathbf{e}_j)의 허수부를 \delta로 나누어 미분을 구하며, 반올림 오차 없이 기계 정밀도(machine precision)에 가까운 정확도를 달성한다.

6. EKF의 선형화 오차와 한계

6.1 일관성(Consistency) 문제

EKF의 선형화는 추정값에서 수행되므로, 추정 오차가 크면 자코비안이 참값 근방의 것과 크게 달라질 수 있다. 이로 인하여 다음의 문제가 발생한다.

1. 과소 추정된 공분산: 비선형성이 강한 경우, 1차 근사에 의한 공분산이 실제 오차의 통계를 과소 추정하여 필터가 과신(overconfident)하게 된다.

2. 관측 가능성 차원 불일치: 비선형 시스템을 자코비안으로 선형화할 때, 참값이 아닌 추정값에서 평가한 자코비안이 시스템의 관측 불가능 부분 공간(unobservable subspace)을 왜곡시킬 수 있다. 이는 EKF 기반 SLAM에서 로봇의 절대 위치와 방향에 대한 불확실성이 부당하게 감소하는 현상으로 나타난다.

6.2 반복 EKF(Iterated EKF, IEKF)

갱신 단계에서의 선형화 오차를 줄이기 위하여, 갱신된 추정값에서 자코비안을 다시 계산하고 갱신을 반복하는 방법이다.

\hat{\mathbf{x}}_{k\vert k}^{(j+1)} = \hat{\mathbf{x}}_{k\vert k-1} + \mathbf{K}_k^{(j)} \bigl(\mathbf{z}_k - \mathbf{h}(\hat{\mathbf{x}}_{k\vert k}^{(j)}) - \mathbf{H}_k^{(j)}(\hat{\mathbf{x}}_{k\vert k-1} - \hat{\mathbf{x}}_{k\vert k}^{(j)})\bigr)

여기서 \mathbf{H}_k^{(j)}와 \mathbf{K}_k^{(j)}는 j번째 반복에서의 추정값 \hat{\mathbf{x}}_{k\vert k}^{(j)}에서 평가한 것이다. 이 반복은 가우스-뉴턴법과 동치이며, 국소적으로 이차 수렴한다.

7. EKF와 대안적 필터의 비교

EKF는 자코비안을 해석적으로 구할 수 있고 비선형성이 약한 경우에 효율적이다. 자코비안의 정확한 계산은 EKF의 성능에 결정적 영향을 미치므로, 가능한 한 해석적 자코비안을 사용하는 것이 바람직하다.

8. 참고 문헌

• Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.
• Simon, D. (2006). Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. Wiley.
• Bar-Shalom, Y., Li, X. R., & Kirubarajan, T. (2001). Estimation with Applications to Tracking and Navigation. Wiley.
• Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
• Huang, G. P., Mourikis, A. I., & Roumeliotis, S. I. (2010). Observability-based rules for designing consistent EKF SLAM estimators. International Journal of Robotics Research, 29(5), 502–528.

v 0.1