10.71 마호니 필터(Mahony Filter)의 원리와 구현

1. 마호니 필터의 개요

마호니 필터(Mahony filter)는 로버트 마호니(Robert Mahony), 토르 햄멜(Tarek Hamel), 장 미셸 플림랭(Jean-Michel Pflimlin)이 2008년에 제안한 자세 추정 알고리즘이다. 이 알고리즘은 특수 직교군 SO(3) 위에서 직접 동작하는 비선형 상보 필터(nonlinear complementary filter)로 정의되며, 가속도계와 지자기계 등의 절대 자세 측정값을 자이로스코프 측정값과 결합하여 강건한 자세 추정을 제공한다. 리아푸노프 안정성 이론에 기반한 엄밀한 수렴성 분석이 가능하다는 점에서 이론적 기반이 탄탄한 알고리즘으로 평가된다.

2. 알고리즘의 동기

2.1 기존 상보 필터의 한계

선형 상보 필터를 회전군에 직접 적용하면 다양체의 비선형성으로 인해 정확한 결과를 얻기 어렵다. 또한 칼만 필터는 우수한 정확도를 제공하지만 연산 비용이 크다. 마호니 등은 두 접근법의 절충점으로 회전군 위에서 정의된 비선형 상보 필터를 제안하였다.

2.2 SO(3)의 기하학적 구조 활용

마호니 필터의 핵심은 자세 오차를 회전군의 군 연산을 사용하여 정의하고, 비례-적분 보정을 회전 다양체 위의 적절한 형태로 적용하는 것이다. 이를 통해 알고리즘의 안정성을 리아푸노프 함수로 증명할 수 있다.

3. 자세 오차의 정의

3.1 회전 행렬 형태의 오차

추정 자세를 \hat{\mathbf{R}}, 참 자세를 \mathbf{R}이라 할 때, 자세 오차는 다음과 같이 정의된다.

\tilde{\mathbf{R}} = \hat{\mathbf{R}}^T\mathbf{R}

이 행렬은 추정 자세에서 참 자세로의 회전이며, 추정이 정확할 때 \tilde{\mathbf{R}} = \mathbf{I}가 된다.

3.2 측정 벡터 기반 오차

실제로는 참 자세를 알 수 없으므로, 가속도계와 지자기계의 측정값을 사용하여 오차를 간접적으로 정의한다. 지구 좌표계의 기준 벡터(중력 또는 자기장)를 \mathbf{v}_i^{\text{ref}}, 본체 좌표계에서 측정된 대응 벡터를 \mathbf{v}_i^{\text{meas}}라 할 때, 추정 자세를 사용하여 기준 벡터를 본체 좌표계로 회전한 예측값과 측정값을 비교한다.

\hat{\mathbf{v}}_i = \hat{\mathbf{R}}^T\mathbf{v}_i^{\text{ref}}

자세 오차 벡터는 두 단위 벡터의 외적으로 정의된다.

\boldsymbol{\omega}_{\text{mes}} = \sum_{i=1}^{n}k_i \mathbf{v}_i^{\text{meas}} \times \hat{\mathbf{v}}_i

여기서 k_i는 각 측정 벡터의 가중치이다. 이 오차 벡터는 자세 오차의 회전 축과 방향을 나타내는 자연스러운 표현이며, 추정이 정확할 때 0이 된다.

4. 마호니 필터의 갱신 식

4.1 보정된 각속도

자이로스코프 측정값 \boldsymbol{\omega}_{\text{meas}}에 비례 항과 적분 항을 추가하여 보정된 각속도를 계산한다.

\hat{\boldsymbol{\omega}} = \boldsymbol{\omega}_{\text{meas}} - \hat{\mathbf{b}}_g + k_P\boldsymbol{\omega}_{\text{mes}}

여기서

• \hat{\mathbf{b}}_g: 자이로 바이어스의 추정값
• k_P: 비례 이득

4.2 바이어스 갱신

자이로스코프 바이어스의 추정은 자세 오차 벡터의 적분으로 갱신된다.

\dot{\hat{\mathbf{b}}}_g = -k_I\boldsymbol{\omega}_{\text{mes}}

여기서 k_I는 적분 이득이다. 이 적분 항이 자이로 바이어스를 점진적으로 보상하여 장기간의 자세 추정 정확도를 유지한다.

4.3 자세의 갱신

회전 행렬 형태의 자세 갱신은 다음의 미분 방정식으로 표현된다.

\dot{\hat{\mathbf{R}}} = \hat{\mathbf{R}}[\hat{\boldsymbol{\omega}}]_\times

쿼터니언 형태로는 다음과 같다.

\dot{\hat{\mathbf{q}}} = \frac{1}{2}\hat{\mathbf{q}} \otimes \hat{\boldsymbol{\omega}}_{\text{quat}}

여기서 \hat{\boldsymbol{\omega}}_{\text{quat}}는 보정된 각속도를 순허수 쿼터니언으로 확장한 것이다.

5. 명시적 상보 필터(Explicit Complementary Filter)

마호니 등이 제안한 표준 형태인 명시적 상보 필터(ECF)는 위의 갱신 식들을 모두 포함한다. 이 필터의 구조는 다음과 같이 요약된다.

1. 측정 벡터로부터 자세 오차 \boldsymbol{\omega}_{\text{mes}} 계산
2. 자이로 측정에 PI 보정 적용 → 보정된 각속도
3. 보정된 각속도로 자세 적분
4. 바이어스 적분 갱신

이 구조는 본질적으로 PI 제어기에 의해 자세 오차를 0으로 유도하는 폐루프 시스템이다.

6. 직접 상보 필터와 수동 상보 필터

마호니 등은 두 가지 변형을 제안하였다.

6.1 직접 상보 필터(Direct Complementary Filter)

이 형태에서는 측정 벡터를 사용하여 직접 회전 행렬을 추정하는 단계가 포함된다. 그러나 이 방법은 측정 잡음에 민감하다는 단점이 있다.

6.2 수동 상보 필터(Passive Complementary Filter)

자이로 측정값을 주된 동역학으로 사용하고 측정 벡터의 잡음 영향을 최소화하는 형태이다. 명시적 상보 필터가 이 부류에 속한다.

7. 구현 단계

7.1 IMU 형태(가속도계와 자이로스코프)

가속도계만을 사용하는 경우, 측정 벡터는 단일 항이며 다음과 같이 처리된다.

1. 가속도 측정값을 정규화한다: \mathbf{a}_{\text{norm}} = \mathbf{a}/\lVert\mathbf{a}\rVert
2. 추정 자세로부터 중력 방향 예측값을 계산한다: \hat{\mathbf{v}}_a = \hat{\mathbf{R}}^T\mathbf{e}_3, 단 \mathbf{e}_3 = [0, 0, 1]^T
3. 자세 오차 벡터를 계산한다: \boldsymbol{\omega}_{\text{mes}} = \mathbf{a}_{\text{norm}} \times \hat{\mathbf{v}}_a
4. 보정된 각속도를 계산한다: \hat{\boldsymbol{\omega}} = \boldsymbol{\omega}_{\text{meas}} - \hat{\mathbf{b}}_g + k_P\boldsymbol{\omega}_{\text{mes}}
5. 바이어스를 갱신한다: \hat{\mathbf{b}}_g \leftarrow \hat{\mathbf{b}}_g - k_I\boldsymbol{\omega}_{\text{mes}}\Delta t
6. 쿼터니언을 적분한다.
7. 쿼터니언을 정규화한다.

7.2 MARG 형태(지자기계 추가)

지자기계가 있는 경우, 자세 오차 벡터는 두 측정의 합으로 정의된다.

\boldsymbol{\omega}_{\text{mes}} = k_a(\mathbf{a}_{\text{norm}} \times \hat{\mathbf{v}}_a) + k_m(\mathbf{m}_{\text{norm}} \times \hat{\mathbf{v}}_m)

여기서 \hat{\mathbf{v}}_m은 추정 자세로 회전된 자기장 기준 벡터이며, k_a와 k_m은 각각 가속도계와 지자기계의 가중치이다.

8. 리아푸노프 안정성 분석

마호니 필터의 가장 큰 이론적 장점은 안정성에 대한 엄밀한 증명이 가능하다는 점이다. 자세 오차 \tilde{\mathbf{R}}의 리아푸노프 후보 함수는 다음과 같이 정의된다.

V = \frac{1}{2}\mathrm{tr}(\mathbf{I} - \tilde{\mathbf{R}})

이 함수는 \tilde{\mathbf{R}} = \mathbf{I}일 때 0이고 그 외에는 양이다. 폐루프 시스템의 동역학을 대입하여 \dot{V}를 계산하면, 적절한 조건 하에서 음의 정부호이며 자세 오차가 점근적으로 항등으로 수렴함이 증명된다.

마호니 등은 거의 전역적 점근 수렴(almost global asymptotic convergence)을 보였다. “거의 전역적“이란 회전군의 위상적 제약으로 인해 단일 안정점에서의 전역 수렴이 불가능하지만, 측도 0의 집합을 제외한 모든 초기 조건에서 수렴함을 의미한다.

9. 매개변수 튜닝

9.1 비례 이득 k_P

비례 이득은 자세 오차에 대한 즉각적인 보정의 강도를 결정한다. k_P가 크면 응답이 빠르지만 측정 잡음의 영향이 커진다. 일반적으로 0.5에서 5 사이의 값이 사용된다.

9.2 적분 이득 k_I

적분 이득은 바이어스 추정의 수렴 속도를 결정한다. k_I가 너무 크면 일시적인 측정 오차가 바이어스 추정에 누적되어 진동이 발생할 수 있다. 일반적으로 비례 이득의 1/10에서 1/100 정도의 값을 사용한다.

9.3 가중치 k_a, k_m

가속도계와 지자기계의 상대적 신뢰도에 따라 가중치를 조정한다. 동적 운동이 많은 환경에서는 k_a를 작게 설정하여 가속도계의 영향을 줄인다.

10. 마호니 필터의 변형과 확장

10.1 적응형 가중치

동적 가속도가 큰 상황에서 가속도계 가중치를 자동으로 줄이는 적응형 변형이 제안되어 있다. 측정 가속도 크기와 중력의 차이에 따라 가중치를 조정한다.

k_a = k_{a,0}\exp\!\left(-\frac{(\lVert\mathbf{a}\rVert - g)^2}{2\sigma^2}\right)

이 형태는 가속도계가 신뢰할 수 있을 때만 보정에 활용된다.

10.2 다른 다양체로의 확장

마호니 필터의 원리는 SE(3)나 다른 매트릭스 리 군으로 확장될 수 있다. Bonnabel 등은 이러한 확장을 통해 보다 일반화된 비선형 관측기 이론을 발전시켰다.

11. 마호니 필터와 다른 알고리즘의 비교

12. 마호니 필터의 응용

12.1 무인 항공기

고정익 및 회전익 무인 항공기의 자세 추정에 광범위하게 사용된다. PX4 비행 제어 시스템의 일부 모드에서 마호니 필터 변형을 사용한다.

12.2 모바일 로봇

지상 모바일 로봇의 본체 자세 추정에 사용된다. 평지 운동이 많은 환경에서 가속도계 측정의 신뢰성이 높아 우수한 성능을 보인다.

12.3 보행 로봇

이족 및 사족 보행 로봇의 자세 추정에서 마호니 필터의 강건성이 충격이 많은 환경에 적합하다.

12.4 수중 로봇

수중에서는 자력 환경이 왜곡되어 있고 진동이 적으므로, 가속도계 위주의 마호니 필터가 효과적으로 동작한다.

12.5 가속도계 기반 자세 측정

산업용 진동 분석, 건축물 모니터링 등 가속도계 측정에서 자세를 추출하는 응용에 사용된다.

13. 오픈 소스 구현

마호니 필터는 다양한 오픈소스 프로젝트에서 구현되어 있다. ROS 패키지인 imu_filter_madgwick은 마호니 필터도 옵션으로 제공한다. PX4 비행 제어 시스템의 일부 모듈, 그리고 다양한 임베디드 IMU 라이브러리에서 마호니 필터의 변형이 사용되고 있다.

14. 마호니 필터의 한계

마호니 필터는 이론적으로 우수하지만 다음과 같은 한계도 있다.

• 동적 가속도가 큰 상황에서 가속도계 측정의 신뢰성이 떨어지므로, 적응형 변형 없이는 정확도가 저하된다.
• 자기 왜곡이 강한 환경에서 지자기계 기반 보정이 비효과적이다.
• 매개변수 k_P와 k_I의 튜닝이 응용 환경에 따라 달라져야 한다.
• 통계적 잡음 모형을 명시적으로 다루지 않으므로, 잡음 특성이 알려진 응용에서는 칼만 필터 계열이 더 우수할 수 있다.

15. 참고 문헌

• Mahony, R., Hamel, T., & Pflimlin, J. M. (2008). “Nonlinear Complementary Filters on the Special Orthogonal Group.” IEEE Transactions on Automatic Control, 53(5), 1203–1218.
• Hamel, T., & Mahony, R. (2006). “Attitude estimation on SO(3) based on direct inertial measurements.” Proceedings of the IEEE International Conference on Robotics and Automation, 2170–2175.
• Bonnabel, S., Martin, P., & Rouchon, P. (2009). “Non-linear symmetry-preserving observers on Lie groups.” IEEE Transactions on Automatic Control, 54(7), 1709–1713.
• Euston, M., Coote, P., Mahony, R., Kim, J., & Hamel, T. (2008). “A complementary filter for attitude estimation of a fixed-wing UAV.” IEEE/RSJ International Conference on Intelligent Robots and Systems, 340–345.
• Madgwick, S. O. H. (2010). “An efficient orientation filter for inertial and inertial/magnetic sensor arrays.” Technical Report, University of Bristol.

version: 1.0