10.65 오차 상태 칼만 필터(ESKF)의 구조

1. ESKF의 개요

오차 상태 칼만 필터(Error State Kalman Filter, ESKF)는 상태를 명목 상태(nominal state)와 오차 상태(error state)로 분리하여 처리하는 칼만 필터의 변형이다. 이는 MEKF의 일반화 형태로, 자세뿐만 아니라 위치, 속도, 편향 등 다양한 상태를 함께 추정한다.

ESKF는 현대 관성 항법, 시각 관성 오도메트리(VIO), 자율 주행 시스템 등에서 널리 사용되는 표준 필터링 기법이다.

2. ESKF의 핵심 구조

2.1 상태의 분리

ESKF는 상태를 두 부분으로 분리한다.

1. 명목 상태: 현재 최선의 상태 추정
2. 오차 상태: 명목 상태와 실제 상태의 작은 차이

진정한 상태는 두 부분의 “합“으로 표현된다.

\mathbf{x}_{\text{true}} = \mathbf{x}_{\text{nominal}} \oplus \delta\mathbf{x}

여기서 \oplus는 각 상태 변수에 적합한 연산이다. 예를 들어 자세는 쿼터니언 곱, 위치는 벡터 덧셈이다.

2.2 명목 상태의 비선형 업데이트

명목 상태는 비선형 동역학으로 업데이트된다. 이는 매니폴드 구조(예: 쿼터니언의 단위 노름)를 보존한다.

2.3 오차 상태의 선형 업데이트

오차 상태는 작은 값이므로 선형화할 수 있다. 표준 칼만 필터 이론이 적용된다.

3. ESKF의 상태 구성

일반적인 관성 항법 시스템의 ESKF 상태 구성을 살펴본다.

3.1 명목 상태

\mathbf{x}_{\text{nominal}} = \begin{bmatrix}\mathbf{p} \\ \mathbf{v} \\ \mathbf{q} \\ \mathbf{b}_a \\ \mathbf{b}_g\end{bmatrix}

여기서

• \mathbf{p} \in \mathbb{R}^3: 위치
• \mathbf{v} \in \mathbb{R}^3: 속도
• \mathbf{q} \in S^3: 쿼터니언 (단위 노름)
• \mathbf{b}_a \in \mathbb{R}^3: 가속도계 편향
• \mathbf{b}_g \in \mathbb{R}^3: 자이로스코프 편향

명목 상태의 차원은 3 + 3 + 4 + 3 + 3 = 16이다.

3.2 오차 상태

\delta\mathbf{x} = \begin{bmatrix}\delta\mathbf{p} \\ \delta\mathbf{v} \\ \delta\boldsymbol{\phi} \\ \delta\mathbf{b}_a \\ \delta\mathbf{b}_g\end{bmatrix}

여기서

• \delta\mathbf{p}, \delta\mathbf{v}, \delta\mathbf{b}_a, \delta\mathbf{b}_g \in \mathbb{R}^3: 벡터 공간의 오차
• \delta\boldsymbol{\phi} \in \mathbb{R}^3: 작은 회전 벡터 (자세 오차)

오차 상태의 차원은 3 + 3 + 3 + 3 + 3 = 15이다.

3.3 차원의 차이

명목 상태는 16차원이지만 오차 상태는 15차원이다. 이는 쿼터니언이 4차원이지만 단위 노름 제약으로 자유도가 3이기 때문이다.

4. 오차 상태의 정의

4.1 위치 오차

\mathbf{p} = \hat{\mathbf{p}} + \delta\mathbf{p}

표준 벡터 덧셈이다.

4.2 속도 오차

\mathbf{v} = \hat{\mathbf{v}} + \delta\mathbf{v}

마찬가지로 벡터 덧셈이다.

4.3 자세 오차

\mathbf{q} = \hat{\mathbf{q}}\otimes\delta\mathbf{q}

여기서 \delta\mathbf{q} \approx (1, \delta\boldsymbol{\phi}/2)는 작은 회전에 해당하는 쿼터니언이다. 곱셈적 오차이다.

4.4 편향 오차

\mathbf{b}_a = \hat{\mathbf{b}}_a + \delta\mathbf{b}_a

\mathbf{b}_g = \hat{\mathbf{b}}_g + \delta\mathbf{b}_g

편향은 단순 덧셈 오차이다.

5. ESKF의 주요 단계

ESKF는 다음의 단계로 구성된다.

1. 초기화
2. 명목 상태 전파
3. 오차 상태 전파
4. 측정 업데이트
5. 오차 주입 (Injection)
6. 리셋

각 단계를 상세히 설명한다.

6. 초기화

초기 명목 상태와 초기 공분산을 설정한다.

\hat{\mathbf{x}}_0 = \text{initial estimate}

\mathbf{P}_0 = \text{initial covariance}

초기 공분산은 초기 추정의 불확실성을 반영한다.

7. 명목 상태 전파

IMU 측정값(가속도와 각속도)을 사용하여 명목 상태를 비선형적으로 전파한다.

7.1 편향 보정

\mathbf{a} = \mathbf{a}_m - \hat{\mathbf{b}}_a

\boldsymbol{\omega} = \boldsymbol{\omega}_m - \hat{\mathbf{b}}_g

7.2 속도 전파

\hat{\mathbf{v}}_{k+1} = \hat{\mathbf{v}}_k + (\mathbf{R}(\hat{\mathbf{q}}_k)\mathbf{a} + \mathbf{g})\Delta t

여기서 \mathbf{g}는 중력 가속도이다.

7.3 위치 전파

\hat{\mathbf{p}}_{k+1} = \hat{\mathbf{p}}_k + \hat{\mathbf{v}}_k\Delta t + \frac{1}{2}(\mathbf{R}(\hat{\mathbf{q}}_k)\mathbf{a} + \mathbf{g})\Delta t^2

7.4 자세 전파

\hat{\mathbf{q}}_{k+1} = \hat{\mathbf{q}}_k\otimes\exp(\boldsymbol{\omega}\Delta t/2)

7.5 편향 전파

편향은 랜덤 워크이므로 평균은 그대로 유지된다.

\hat{\mathbf{b}}_{a,k+1} = \hat{\mathbf{b}}_{a,k}

\hat{\mathbf{b}}_{g,k+1} = \hat{\mathbf{b}}_{g,k}

8. 오차 상태 전파

오차 상태의 공분산을 선형화된 전이 행렬로 전파한다.

8.1 오차 상태 전이 행렬

\boldsymbol{\Phi}_k = \mathbf{I} + \mathbf{F}_k\Delta t

여기서 \mathbf{F}_k는 연속 시간 오차 동역학 행렬이다.

8.2 구체적 형태

\mathbf{F}_k = \begin{bmatrix} \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -\mathbf{R}[\mathbf{a}]_\times & -\mathbf{R} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -[\boldsymbol{\omega}]_\times & \mathbf{0} & -\mathbf{I} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \end{bmatrix}

이 행렬은 다음의 관계를 반영한다.

• 속도 오차는 위치 오차에 기여
• 가속도 오차(자세 오차와 편향 오차에서 유래)는 속도 오차에 기여
• 각속도 오차는 자세 오차에 기여

8.3 공분산 전파

\mathbf{P}_{k+1}^- = \boldsymbol{\Phi}_k\mathbf{P}_k\boldsymbol{\Phi}_k^T + \mathbf{Q}_k

8.4 공정 잡음 공분산

\mathbf{Q}_k = \mathbf{G}\mathbf{Q}_{\text{noise}}\mathbf{G}^T\Delta t

여기서 \mathbf{G}는 잡음 입력 행렬, \mathbf{Q}_{\text{noise}}는 잡음 공분산이다.

9. 측정 업데이트

측정값을 사용하여 오차 상태를 업데이트한다.

9.1 측정 모델

\mathbf{z}_k = h(\mathbf{x}_{\text{true},k}) + \mathbf{n}_k

9.2 예측 측정

\hat{\mathbf{z}}_k = h(\hat{\mathbf{x}}_k^-)

9.3 측정 자코비안

자코비안은 오차 상태에 대한 측정의 미분이다.

\mathbf{H}_k = \frac{\partial h}{\partial\delta\mathbf{x}}\bigg|_{\hat{\mathbf{x}}_k^-}

9.4 칼만 이득

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

9.5 오차 상태 업데이트

\delta\mathbf{x}_k = \mathbf{K}_k(\mathbf{z}_k - \hat{\mathbf{z}}_k)

9.6 공분산 업데이트

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

또는 Joseph 형태로 더 안정적으로 업데이트한다.

10. 오차 주입

오차 상태의 추정값을 명목 상태에 “주입“한다.

10.1 위치와 속도

\hat{\mathbf{p}}_{k+1} = \hat{\mathbf{p}}_k^- + \delta\mathbf{p}_k

\hat{\mathbf{v}}_{k+1} = \hat{\mathbf{v}}_k^- + \delta\mathbf{v}_k

단순 벡터 덧셈이다.

10.2 자세

\hat{\mathbf{q}}_{k+1} = \hat{\mathbf{q}}_k^-\otimes\exp(\delta\boldsymbol{\phi}_k/2)

곱셈적 업데이트이다. 이는 매니폴드 구조를 보존한다.

10.3 편향

\hat{\mathbf{b}}_{a,k+1} = \hat{\mathbf{b}}_{a,k}^- + \delta\mathbf{b}_{a,k}

\hat{\mathbf{b}}_{g,k+1} = \hat{\mathbf{b}}_{g,k}^- + \delta\mathbf{b}_{g,k}

11. 리셋

오차 상태를 0으로 리셋한다. 오차의 정보는 이미 명목 상태에 주입되었으므로 오차 상태는 더 이상 필요 없다.

\delta\mathbf{x}_k = \mathbf{0}

11.1 공분산의 처리

공분산 \mathbf{P}_k는 일반적으로 그대로 유지된다. 그러나 엄밀하게는 리셋으로 인한 약간의 변화가 있을 수 있으며, 특히 자세 공분산에 대한 “Jacobian of the reset“이 있다.

12. ESKF의 장점

12.1 매니폴드 구조 보존

쿼터니언, SE(3) 등의 매니폴드 구조가 자동으로 보존된다.

12.2 선형 필터링

오차 상태는 작으므로 선형 근사가 정확하다. 표준 칼만 필터 이론이 적용된다.

12.3 최소 차원

오차 상태의 차원이 자유도와 같다. 공분산 행렬이 가장 작다.

12.4 수치 안정성

명목 상태의 비선형 업데이트와 오차 상태의 선형 처리의 결합이 수치적으로 안정적이다.

12.5 일반성

다양한 상태(위치, 속도, 자세, 편향)를 통합적으로 처리한다.

13. ESKF의 응용

13.1 관성 항법 시스템 (INS)

고전적 INS에서 ESKF가 사용된다. 위치, 속도, 자세를 동시에 추정한다.

13.2 GPS/INS 융합

GPS 측정값으로 INS 드리프트를 보정한다.

13.3 시각 관성 오도메트리 (VIO)

카메라와 IMU를 융합하여 자세와 위치를 추정한다. ESKF가 표준 프레임워크 중 하나이다.

13.4 LiDAR 관성 오도메트리 (LIO)

LiDAR와 IMU를 융합한다.

13.5 자율 주행

자율 주행 차량의 위치, 속도, 자세 추정에 ESKF가 사용된다.

13.6 드론

드론의 상태 추정에 ESKF가 사용된다. PX4 등 주요 비행 제어 소프트웨어가 구현한다.

14. 측정 예시

14.1 GPS 측정

\mathbf{z}_{\text{GPS}} = \mathbf{p} + \mathbf{n}_{\text{GPS}}

자코비안: \mathbf{H}_{\text{GPS}} = [\mathbf{I}_3, \mathbf{0}, \mathbf{0}, \mathbf{0}, \mathbf{0}]

14.2 자력계 측정

\mathbf{z}_{\text{mag}} = \mathbf{R}^T\mathbf{m}_{\text{ref}} + \mathbf{n}_{\text{mag}}

자코비안: \mathbf{H}_{\text{mag}}는 자세 오차에 대해 비영이다.

14.3 비전 측정

카메라가 본 랜드마크의 이미지 좌표가 측정값이다. 자코비안은 위치와 자세 오차에 대해 복잡하게 정의된다.

15. MEKF와 ESKF의 관계

MEKF는 ESKF의 특수한 경우로, 자세만 추정하는 필터이다. ESKF는 일반화된 형태로 위치, 속도, 편향 등 추가 상태를 포함한다.

15.1 구조적 유사성

• 명목 상태와 오차 상태의 분리
• 곱셈적 자세 오차
• 오차 주입과 리셋

15.2 차이점

• ESKF는 더 많은 상태 변수
• ESKF는 추가적인 상태 간 결합(예: 자세 오차가 속도 오차에 영향)

16. 오차 상태 전이 행렬의 세부

16.1 자세 오차의 전파

\delta\dot{\boldsymbol{\phi}} = -[\boldsymbol{\omega}]_\times\delta\boldsymbol{\phi} - \delta\mathbf{b}_g

이는 자이로스코프 편향 오차가 자세 오차에 직접 영향을 주는 것을 반영한다.

16.2 속도 오차의 전파

\delta\dot{\mathbf{v}} = -\mathbf{R}[\mathbf{a}]_\times\delta\boldsymbol{\phi} - \mathbf{R}\delta\mathbf{b}_a

자세 오차가 속도 오차에 영향을 주고, 가속도계 편향 오차도 속도 오차에 영향을 준다.

16.3 위치 오차의 전파

\delta\dot{\mathbf{p}} = \delta\mathbf{v}

이는 속도가 위치의 미분이라는 당연한 관계이다.

16.4 편향 오차의 전파

\delta\dot{\mathbf{b}}_a = \mathbf{w}_a, \quad \delta\dot{\mathbf{b}}_g = \mathbf{w}_g

편향은 랜덤 워크이므로 오차의 시간 미분이 잡음이다.

17. 이산화

연속 시간 오차 동역학을 이산 시간으로 변환한다.

17.1 차 근사

\boldsymbol{\Phi}_k = \mathbf{I} + \mathbf{F}_k\Delta t

17.2 더 정확한 근사

행렬 지수를 사용한다.

\boldsymbol{\Phi}_k = \exp(\mathbf{F}_k\Delta t)

이는 계산 비용이 더 크지만 정확도가 높다.

18. 실용적 고려

18.1 샘플링 주기

IMU의 주기(보통 200-1000 Hz)에 맞춰 명목 상태 전파를 수행한다. 측정 업데이트는 센서 주기에 따라 변한다.

18.2 수치 정밀도

공분산 행렬이 양정치성을 유지해야 한다. Joseph 형태 업데이트와 정기적 대칭화가 도움이 된다.

18.3 초기화

초기 자세를 정확히 추정해야 한다. 가속도계와 자력계의 단일 측정으로 추정할 수 있다.

18.4 관측 가능성

특정 상태 변수가 관측 가능한지 확인한다. 예를 들어 GPS 없이 위치가 드리프트할 수 있다.

19. 오차 상태 칼만 필터의 변형

19.1 Iterated ESKF

측정 업데이트를 반복하여 더 정확한 해를 얻는다.

19.2 Square-Root ESKF

공분산 대신 공분산의 제곱근을 전파한다. 수치 안정성이 향상된다.

19.3 Adaptive ESKF

공정/측정 잡음 공분산을 적응적으로 조정한다.

20. 결론

오차 상태 칼만 필터(ESKF)는 관성 항법과 상태 추정의 표준 프레임워크이다. 명목 상태와 오차 상태의 분리를 통해 매니폴드 구조 보존과 선형 필터링을 결합한다. MEKF의 일반화로 위치, 속도, 자세, 편향 등을 통합적으로 추정할 수 있으며, 현대 자율 시스템(자율 주행, 드론, 로봇)에서 핵심 역할을 한다.

21. 참고 문헌

• Sola, J. (2017). “Quaternion Kinematics for the Error-State Kalman Filter.” arXiv:1711.02508.
• Madyastha, V., Ravindra, V., Mallikarjunan, S., & Goyal, A. (2011). “Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation.” AIAA Guidance, Navigation, and Control Conference.
• Mourikis, A. I., & Roumeliotis, S. I. (2007). “A Multi-State Constraint Kalman Filter for Vision-Aided Inertial Navigation.” IEEE ICRA, 3565–3572.
• Bloesch, M., Omari, S., Hutter, M., & Siegwart, R. (2015). “Robust Visual Inertial Odometry Using a Direct EKF-Based Approach.” IEEE IROS, 298–304.
• Markley, F. L., & Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. Springer.

version: 1.0