10.64 곱셈 확장 칼만 필터(MEKF)의 원리

1. MEKF의 개요

곱셈 확장 칼만 필터(Multiplicative Extended Kalman Filter, MEKF)는 쿼터니언 기반 자세 추정에 특화된 확장 칼만 필터의 변형이다. Lefferts, Markley, Shuster가 1982년 논문에서 체계적으로 정립하였으며, 우주선 자세 추정의 표준 알고리즘이 되었다.

MEKF는 쿼터니언의 매니폴드 구조와 비선형성을 효과적으로 처리하기 위해 설계되었다. 본 절에서는 MEKF의 원리와 구체적 알고리즘을 다룬다.

2. MEKF의 핵심 아이디어

2.1 명목 상태와 오차 상태의 분리

MEKF의 핵심 아이디어는 상태를 두 부분으로 분리하는 것이다.

명목 상태 (Nominal State): 현재 최선의 자세 추정
오차 상태 (Error State): 명목 상태와 실제 상태의 작은 차이

명목 상태는 비선형적으로 업데이트되고 매니폴드 구조가 보존된다. 오차 상태는 선형 공간에서 처리되며 표준 칼만 필터 이론이 적용된다.

2.2 곱셈적 오차

오차는 곱셈적으로 정의된다.

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

여기서 $\mathbf{q}$ 는 실제 쿼터니언, $\hat{\mathbf{q}}$ 는 명목 쿼터니언, $\delta\mathbf{q}$ 는 오차 쿼터니언이다. 덧셈이 아닌 곱셈으로 정의되므로 “곱셈 EKF“라 한다.

3. 상태 벡터

3.1 명목 상태

명목 상태는 자세와 기타 변수(예: 편향)로 구성된다.

$\hat{\mathbf{x}} = \begin{bmatrix}\hat{\mathbf{q}} \\ \hat{\mathbf{b}}_g\end{bmatrix}$

여기서 $\hat{\mathbf{q}}$ 는 4 성분 쿼터니언, $\hat{\mathbf{b}}_g$ 는 3 성분 자이로스코프 편향이다. 차원은 7이다.

3.2 오차 상태

오차 상태는 작은 변수로 구성된다.

$\delta\mathbf{x} = \begin{bmatrix}\delta\boldsymbol{\phi} \\ \delta\mathbf{b}_g\end{bmatrix}$

여기서 $\delta\boldsymbol{\phi}$ 는 3 성분 작은 회전 벡터, $\delta\mathbf{b}_g$ 는 3 성분 편향 오차이다. 차원은 6이다.

이는 EKF의 7차원 상태와 비교하여 매개변수 수가 줄어든 것이다.

4. MEKF의 주요 단계

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

초기화
예측 단계 (Propagation)
측정 업데이트
리셋 (Reset)

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

5. 초기화

5.1 초기 쿼터니언

초기 자세 추정을 설정한다. 이는 가속도계와 자력계의 첫 측정값이나 TRIAD 알고리즘으로 얻을 수 있다.

$\hat{\mathbf{q}}_0 = \text{initial estimate}$

5.2 초기 오차 공분산

$\mathbf{P}_0 = \begin{bmatrix}\mathbf{P}_{\phi\phi} & \mathbf{0} \\ \mathbf{0} & \mathbf{P}_{bb}\end{bmatrix}$

여기서 $\mathbf{P}_{\phi\phi}$ 는 자세 오차 공분산(rad²), $\mathbf{P}_{bb}$ 는 편향 오차 공분산이다. 초기 불확실성을 반영한다.

6. 예측 단계

예측 단계에서는 동적 모델을 사용하여 상태를 전파한다.

6.1 명목 쿼터니언의 전파

자이로스코프 측정값을 사용한다.

$\boldsymbol{\omega}_{\text{corrected}} = \boldsymbol{\omega}_m - \hat{\mathbf{b}}_g$

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

이는 정확한 지수 적분이며, 단위 노름을 보존한다.

6.2 편향의 전파

편향은 랜덤 워크로 모델링된다.

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

랜덤 워크는 공분산 전파에 반영된다.

6.3 오차 공분산 전파

선형화된 상태 전이 행렬 $\boldsymbol{\Phi}_k$ 를 사용한다.

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

6.4 상태 전이 행렬

$\boldsymbol{\Phi}_k = \begin{bmatrix}\mathbf{I} - [\boldsymbol{\omega}_{\text{corrected}}]_\times\Delta t & -\mathbf{I}\Delta t \\ \mathbf{0} & \mathbf{I}\end{bmatrix}$

이는 작은 시간 단계의 1차 근사이다.

6.5 공정 잡음 공분산

$\mathbf{Q}_k = \begin{bmatrix}\sigma_g^2\Delta t\mathbf{I} & \mathbf{0} \\ \mathbf{0} & \sigma_{bw}^2\Delta t\mathbf{I}\end{bmatrix}$

여기서 $\sigma_g$ 는 자이로스코프 잡음, $\sigma_{bw}$ 는 편향 랜덤 워크 잡음이다.

7. 측정 업데이트

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

7.1 측정 모델

일반적인 측정 모델은

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

여기서 $\mathbf{v}_k$ 는 측정 잡음이다. 예를 들어, 중력 벡터 측정:

$\mathbf{z}_k = \mathbf{R}^T(\mathbf{q}_k)\mathbf{g} + \mathbf{v}_k$

7.2 예측 측정

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

7.3 측정 잔차 (혁신)

$\mathbf{r}_k = \mathbf{z}_k - \hat{\mathbf{z}}_k$

7.4 측정 자코비안

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

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

7.5 칼만 이득

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

7.6 오차 상태 업데이트

$\delta\mathbf{x}_k = \mathbf{K}_k\mathbf{r}_k$

이는 오차 상태의 추정값이다.

7.7 공분산 업데이트

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

또는 Joseph 형태로 (더 안정적):

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

8. 리셋

리셋 단계는 MEKF의 핵심 특징이다. 오차 상태를 명목 상태에 합치고 오차를 0으로 리셋한다.

8.1 명목 쿼터니언 리셋

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

작은 회전 오차를 명목 쿼터니언에 곱셈적으로 합친다.

8.2 편향 리셋

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

편향 오차를 명목 편향에 더한다.

8.3 오차 상태 리셋

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

오차 상태를 0으로 리셋한다. 오차 정보는 이미 명목 상태에 반영되었다.

8.4 공분산의 처리

오차 상태를 리셋해도 공분산 $\mathbf{P}_k$ 는 그대로 유지된다. 오차의 불확실성은 여전히 공분산에 있다.

9. MEKF의 장점

9.1 매니폴드 구조 보존

명목 쿼터니언이 곱셈적으로 업데이트되므로 단위 노름이 자동으로 보존된다. 매니폴드 구조가 유지된다.

9.2 효율성

오차 상태가 6차원이므로 공분산이 $6\times 6$ 이다. 이는 EKF의 $7\times 7$ 또는 쿼터니언과 편향을 분리한 필터보다 효율적이다.

9.3 정확성

선형화가 오차 상태에만 적용되므로, 작은 오차에서 선형 근사가 정확하다.

9.4 안정성

명목 상태의 비선형 업데이트와 오차 상태의 선형 필터링이 결합되어 수치적으로 안정적이다.

10. MEKF의 단점

10.1 구현 복잡성

EKF보다 구현이 복잡하다. 명목/오차 상태 분리, 리셋 단계 등이 추가된다.

10.2 개념의 이해

명목 상태와 오차 상태의 분리 개념을 이해해야 한다.

10.3 측정 자코비안의 주의

측정 자코비안이 오차 상태에 대해 정의되므로 주의가 필요하다.

11. 측정 예시

11.1 가속도계 측정

가속도계는 중력과 본체 가속도를 측정한다.

$\mathbf{z}_{\text{acc}} = \mathbf{R}^T\mathbf{g} + \mathbf{a}_{\text{body}} + \mathbf{n}_{\text{acc}}$

정지 상태에서는 $\mathbf{a}_{\text{body}} = \mathbf{0}$ 이므로 중력만 측정된다.

11.2 자력계 측정

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

11.3 별 추적기 측정 (우주선)

$\mathbf{z}_{\text{star}} = \mathbf{R}^T\mathbf{s}_{\text{ref}} + \mathbf{n}_{\text{star}}$

11.4 비전 측정

카메라가 알려진 랜드마크를 관찰하는 경우, 이미지 좌표가 측정값이다.

12. 측정 자코비안의 예

중력 벡터 측정의 경우, 오차 상태 $\delta\boldsymbol{\phi}$ 에 대한 자코비안은

$\mathbf{H} = \begin{bmatrix}[\mathbf{R}^T\mathbf{g}]_\times & \mathbf{0}\end{bmatrix}$

이는 작은 회전이 측정값에 미치는 영향을 1차 근사한 것이다.

13. 간략화된 MEKF 알고리즘

class MEKF:
    def __init__(self, q_initial, b_initial, P_initial):
        self.q = q_initial
        self.b = b_initial
        self.P = P_initial
    
    def predict(self, omega_measured, dt):
        # 편향 보정
        omega = omega_measured - self.b
        
        # 명목 쿼터니언 전파
        dq = quaternion_from_axis_angle(omega, dt)
        self.q = quaternion_multiply(self.q, dq)
        
        # 오차 공분산 전파
        Phi = compute_state_transition(omega, dt)
        Q = compute_process_noise(dt)
        self.P = Phi @ self.P @ Phi.T + Q
    
    def update(self, z, h_func, H_func, R):
        # 예측 측정
        z_pred = h_func(self.q)
        
        # 잔차
        r = z - z_pred
        
        # 자코비안
        H = H_func(self.q)
        
        # 칼만 이득
        K = self.P @ H.T @ np.linalg.inv(H @ self.P @ H.T + R)
        
        # 오차 상태 업데이트
        delta_x = K @ r
        
        # 공분산 업데이트 (Joseph form)
        I_KH = np.eye(6) - K @ H
        self.P = I_KH @ self.P @ I_KH.T + K @ R @ K.T
        
        # 리셋
        delta_phi = delta_x[0:3]
        delta_b = delta_x[3:6]
        
        # 명목 쿼터니언 리셋
        dq_reset = quaternion_from_axis_angle(delta_phi, 1.0)
        self.q = quaternion_multiply(self.q, dq_reset)
        
        # 편향 리셋
        self.b = self.b + delta_b

14. 성능 평가

14.1 자세 오차

실제 자세와 추정 자세의 차이를 회전 각으로 측정한다.

14.2 편향 추정

자이로스코프 편향이 실제 편향에 수렴하는지 확인한다.

14.3 일관성

공분산이 실제 오차와 일치하는지 (NIS, NEES 테스트).

14.4 수렴 속도

초기 오차에서 수렴하는 속도이다.

15. MEKF의 응용

15.1 우주선 자세 추정

NASA와 ESA의 많은 우주선이 MEKF를 사용한다. 별 추적기, 자이로스코프, 태양 센서 등을 융합한다.

15.2 드론 자세 추정

드론의 자세 추정에 MEKF가 사용된다. IMU, 자력계, GPS를 융합한다.

15.3 자율 주행 차량

자율 주행에서 차량의 자세 추정에 MEKF가 응용된다.

15.4 로봇 자세 추정

이동 로봇과 매니퓰레이터의 자세 추정에 사용된다.

16. MEKF의 변형

16.1 Extended MEKF

편향뿐만 아니라 스케일 팩터, 비직교성 등 추가 매개변수를 포함한다.

16.2 Indirect Kalman Filter

오차 상태만을 추정하는 형태. MEKF와 유사하다.

16.3 USQUE (Unscented Quaternion Estimator)

UKF의 쿼터니언 버전. 시그마 포인트를 회전 벡터 공간에서 생성한다.

16.4 ESKF (Error State Kalman Filter)

MEKF를 일반화한 형태. 위치, 속도, 편향 등 다양한 상태를 포함한다.

17. MEKF의 이론적 기반

17.1 리 군 필터링

MEKF는 리 군 $SO(3)$ 상의 필터링이다. 명목 상태가 리 군에서 진화하고 오차 상태가 리 대수에서 선형화된다.

17.2 점근 최적성

작은 오차에서 MEKF는 최적에 가깝다. 대규모 오차에서는 정확도가 떨어질 수 있다.

17.3 매니폴드 구조 보존

MEKF의 핵심 특징은 매니폴드 구조 보존이다. 이는 장시간 안정성을 제공한다.

18. MEKF의 한계

18.1 선형화 오차

작은 오차의 선형 근사이므로 큰 오차에서는 부정확하다.

18.2 측정 자코비안의 복잡성

측정이 비선형일 때 자코비안 계산이 복잡할 수 있다.

18.3 초기화 민감성

초기 자세 추정이 부정확하면 수렴하지 않을 수 있다.

19. 결론

MEKF는 쿼터니언 기반 자세 추정의 표준 알고리즘이다. 명목 상태와 오차 상태의 분리를 통해 매니폴드 구조를 보존하면서 선형 칼만 필터 이론을 적용한다. 우주선 자세 추정에서 오랫동안 사용되었으며, 현대 드론과 로봇에도 적용된다. MEKF의 원리를 이해하면 더 일반적인 ESKF와 매니폴드 필터링을 이해하는 기반이 된다.

20. 참고 문헌

Lefferts, E. J., Markley, F. L., & Shuster, M. D. (1982). “Kalman Filtering for Spacecraft Attitude Estimation.” Journal of Guidance, Control, and Dynamics, 5(5), 417–429.
Markley, F. L. (2003). “Attitude Error Representations for Kalman Filtering.” Journal of Guidance, Control, and Dynamics, 26(2), 311–317.
Markley, F. L., & Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. Springer.
Sola, J. (2017). “Quaternion Kinematics for the Error-State Kalman Filter.” arXiv:1711.02508.
Trawny, N., & Roumeliotis, S. I. (2005). “Indirect Kalman Filter for 3D Attitude Estimation.” Technical Report, University of Minnesota.

version: 1.0