10.70 마드윅 필터(Madgwick Filter)의 원리와 구현

1. 마드윅 필터의 개요

마드윅 필터(Madgwick filter)는 세바스티안 마드윅(Sebastian Madgwick)이 2010년에 제안한 자세 추정 알고리즘이며, 관성 측정 장치(IMU) 또는 자기-관성 측정 장치(MARG)의 측정값으로부터 단위 쿼터니언으로 표현된 자세를 효율적으로 추정한다. 경사 하강법(gradient descent)에 기반하여 가속도계와 지자기계의 측정값에 대한 자세 오차의 경사를 계산하고, 이를 자이로스코프 적분에 보정 항으로 결합하는 구조를 가진다. 계산량이 매우 적으면서도 칼만 필터에 근접한 정확도를 제공하기 때문에 임베디드 시스템과 웨어러블 기기에서 널리 채택되고 있다.

2. 알고리즘 설계의 동기

2.1 효율성 요구

확장 칼만 필터는 자세 추정의 사실상 표준이지만, 공분산 행렬의 갱신과 야코비안 계산으로 인한 연산 부하가 크다. 저전력 마이크로컨트롤러나 고속 샘플링이 요구되는 응용에서는 더 가벼운 알고리즘이 필요하다. 마드윅 필터는 이러한 요구를 충족하기 위해 설계되었다.

2.2 쿼터니언 기반 표현

마드윅 필터는 쿼터니언을 자세 표현으로 채택한다. 쿼터니언은 짐벌 락이 없고 회전 합성이 단순하며, 미소 회전의 표현이 안정적이다. 이러한 특성은 자세 추정 알고리즘에 적합하다.

3. 자이로스코프에 의한 자세 갱신

본체 좌표계의 각속도 측정값 $\boldsymbol{\omega} = [\omega_x, \omega_y, \omega_z]^T$ 로부터 쿼터니언의 시간 미분은 다음과 같다.

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

여기서 $\boldsymbol{\omega}_{\text{quat}} = [0, \omega_x, \omega_y, \omega_z]^T$ 이며, $\hat{\mathbf{q}}$ 는 현재 쿼터니언 추정값이다. 이산 시간 형태의 적분은 다음과 같다.

$\hat{\mathbf{q}}_{\boldsymbol{\omega},k+1} = \hat{\mathbf{q}}_k + \dot{\mathbf{q}}_{\boldsymbol{\omega}}\Delta t$

이것이 자이로 단독으로 산출하는 자세 갱신이지만, 자이로 바이어스로 인한 표류가 누적되므로 추가 보정이 필요하다.

4. 가속도계 측정에 의한 자세 보정

4.1 보정의 원리

가속도계는 정적 상태에서 중력 방향을 측정한다. 참 자세를 가진 강체에서 본체 좌표계의 단위 가속도 벡터는 회전된 중력 단위 벡터와 일치해야 한다. 추정 자세가 정확하지 않으면 측정값과 예측값 사이에 차이가 발생하며, 이 차이가 자세 오차의 단서이다.

4.2 비용 함수의 정의

마드윅 필터는 자세 오차를 다음의 비용 함수로 정의한다.

$\mathbf{f}(\hat{\mathbf{q}}, {}^E\mathbf{g}, {}^B\hat{\mathbf{a}}) = \hat{\mathbf{q}}^* \otimes {}^E\mathbf{g} \otimes \hat{\mathbf{q}} - {}^B\hat{\mathbf{a}}$

여기서

${}^E\mathbf{g} = [0, 0, 0, 1]^T$ : 지구 좌표계의 단위 중력 벡터(쿼터니언 형태)
${}^B\hat{\mathbf{a}}$ : 본체 좌표계에서 측정된 정규화된 가속도 벡터(쿼터니언 형태)
$\hat{\mathbf{q}}^*$ : 추정 쿼터니언의 켤레

이 함수는 추정 자세가 정확할 때 0이 되며, 부정확할 때 0에서 멀어진다. 따라서 자세 추정 문제는 $\mathbf{f}$ 의 노름을 최소화하는 $\hat{\mathbf{q}}$ 를 찾는 최적화 문제로 환원된다.

4.3 명시적 형태

비용 함수를 풀어 쓰면 다음의 3차원 벡터 형태가 된다.

$\mathbf{f}_g(\hat{\mathbf{q}}, {}^B\hat{\mathbf{a}}) = \begin{bmatrix} 2(\hat{q}_x\hat{q}_z - \hat{q}_w\hat{q}_y) - \hat{a}_x \\ 2(\hat{q}_w\hat{q}_x + \hat{q}_y\hat{q}_z) - \hat{a}_y \\ 2(\tfrac{1}{2} - \hat{q}_x^2 - \hat{q}_y^2) - \hat{a}_z \end{bmatrix}$

여기서 $\hat{\mathbf{q}} = [\hat{q}_w, \hat{q}_x, \hat{q}_y, \hat{q}_z]^T$ 이고, ${}^B\hat{\mathbf{a}} = [\hat{a}_x, \hat{a}_y, \hat{a}_z]^T$ 는 정규화된 가속도이다.

4.4 야코비안

비용 함수의 쿼터니언 성분에 대한 야코비안은 다음과 같다.

$\mathbf{J}_g(\hat{\mathbf{q}}) = \begin{bmatrix} -2\hat{q}_y & 2\hat{q}_z & -2\hat{q}_w & 2\hat{q}_x \\ 2\hat{q}_x & 2\hat{q}_w & 2\hat{q}_z & 2\hat{q}_y \\ 0 & -4\hat{q}_x & -4\hat{q}_y & 0 \end{bmatrix}$

이 야코비안과 비용 함수를 사용하여 경사를 계산한다.

4.5 경사의 계산

비용 함수의 노름의 경사는 다음과 같다.

$\nabla\mathbf{f} = \mathbf{J}_g^T(\hat{\mathbf{q}})\mathbf{f}_g(\hat{\mathbf{q}}, {}^B\hat{\mathbf{a}})$

이 경사가 자세 추정의 보정 방향을 제공한다.

4.6 정규화

정규화된 경사는 다음과 같이 계산된다.

$\nabla\mathbf{f}_{\text{norm}} = \frac{\nabla\mathbf{f}}{\lVert\nabla\mathbf{f}\rVert}$

이 정규화는 보정의 크기를 일정하게 유지하여 알고리즘의 안정성을 보장한다.

5. 지자기계 측정에 의한 자세 보정

5.1 지자기계의 역할

요(yaw) 축의 자세는 가속도계만으로 결정되지 않으므로, 지자기계의 측정값을 사용하여 보정한다. 지구 좌표계에서 지자기 벡터를 ${}^E\mathbf{b} = [0, b_x, 0, b_z]^T$ 로 가정하면, 측정된 본체 좌표계의 지자기 벡터 ${}^B\hat{\mathbf{m}}$ 과의 차이가 자세 오차를 제공한다.

5.2 자기 왜곡 보상

지자기계 측정값은 주변 자기 환경에 의해 왜곡된다. 마드윅 필터는 측정된 지자기 벡터를 추정 자세로 회전시켜 지구 좌표계로 변환한 후, 수평 성분만을 사용하여 기준 벡터를 산출한다. 이 방법으로 일시적 자기 왜곡의 영향을 최소화한다.

${}^E\mathbf{h} = \hat{\mathbf{q}} \otimes {}^B\hat{\mathbf{m}} \otimes \hat{\mathbf{q}}^*$

${}^E\mathbf{b} = [0, \sqrt{h_x^2 + h_y^2}, 0, h_z]^T$

이렇게 산출된 ${}^E\mathbf{b}$ 가 갱신된 지자기 기준 벡터로 사용된다.

5.3 결합된 비용 함수와 야코비안

가속도계와 지자기계의 비용 함수를 결합하면 6차원 비용 함수가 된다.

$\mathbf{f}_{g,b}(\hat{\mathbf{q}}, {}^B\hat{\mathbf{a}}, {}^E\mathbf{b}, {}^B\hat{\mathbf{m}}) = \begin{bmatrix} \mathbf{f}_g(\hat{\mathbf{q}}, {}^B\hat{\mathbf{a}}) \\ \mathbf{f}_b(\hat{\mathbf{q}}, {}^E\mathbf{b}, {}^B\hat{\mathbf{m}}) \end{bmatrix}$

대응하는 야코비안 $\mathbf{J}_{g,b}$ 도 6 $\times$ 4 행렬로 확장된다.

6. 자이로 보정과 결합

6.1 경사에 의한 보정 항

가속도계와 지자기계로부터 계산된 경사를 자이로스코프 적분에 보정 항으로 추가한다.

$\dot{\hat{\mathbf{q}}} = \dot{\mathbf{q}}_{\boldsymbol{\omega}} - \beta \cdot \nabla\mathbf{f}_{\text{norm}}$

여기서 $\beta$ 는 알고리즘의 보정 이득이며, 보정의 강도를 결정한다.

6.2 보정 이득의 의미

매개변수 $\beta$ 는 자이로 측정 잡음과 보정 항 사이의 균형을 결정한다. $\beta$ 가 크면 가속도계와 지자기계의 측정값이 자이로 적분에 강하게 영향을 주고, $\beta$ 가 작으면 자이로 적분이 우세하다. 마드윅은 자이로스코프 측정 오차의 표준 편차 $\sigma_\omega$ 로부터 $\beta$ 를 결정하는 식을 제안하였다.

$\beta = \sqrt{\frac{3}{4}}\sigma_\omega$

이 값은 자이로 잡음의 통계적 특성을 반영하여 보정 강도를 자동으로 결정한다.

7. 알고리즘의 구현 단계

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

각속도 측정값과 가속도 측정값을 입력 받는다.
가속도 측정값을 정규화한다.
비용 함수 $\mathbf{f}_g$ 와 야코비안 $\mathbf{J}_g$ 를 계산한다.
경사 $\nabla\mathbf{f} = \mathbf{J}_g^T\mathbf{f}_g$ 를 계산한다.
경사를 정규화한다.
자이로 적분과 보정 항을 결합하여 쿼터니언 미분을 계산한다.
적분하여 새 쿼터니언을 산출한다.
새 쿼터니언을 정규화한다.

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

지자기계가 있는 경우 위 절차에 다음 단계가 추가된다.

지자기 측정값을 정규화한다.
결합된 비용 함수 $\mathbf{f}_{g,b}$ 와 야코비안 $\mathbf{J}_{g,b}$ 를 사용한다.
추정 자세로 지자기 벡터를 회전시켜 지구 좌표계의 기준 벡터를 갱신한다.

8. 알고리즘의 의사 코드

입력: 자이로 ω, 가속도 a, (지자기 m), 이전 쿼터니언 q, 샘플 주기 Δt
1. a를 정규화: a ← a / ||a||
2. (선택) m을 정규화: m ← m / ||m||
3. 비용 함수 f와 야코비안 J 계산
4. 경사 ∇f = J^T f
5. 경사 정규화: s ← ∇f / ||∇f||
6. 쿼터니언 미분: dq = (1/2)q ⊗ ω - β·s
7. 적분: q ← q + dq·Δt
8. 정규화: q ← q / ||q||
출력: 갱신된 q

9. 마드윅 필터의 수학적 기반

9.1 경사 하강법과의 관계

마드윅 필터의 보정 항은 자세 비용 함수의 경사 하강 방향이다. 표준 경사 하강법은 다음과 같다.

$\hat{\mathbf{q}}_{k+1} = \hat{\mathbf{q}}_k - \mu\frac{\nabla\mathbf{f}}{\lVert\nabla\mathbf{f}\rVert}$

여기서 $\mu$ 는 학습률이다. 마드윅 필터는 이 갱신을 자이로 적분과 결합한 형태로 변형한 것이다.

9.2 비례 미분 이득의 통합

마드윅의 핵심 통찰은 학습률 $\mu$ 를 자이로 잡음 특성으로부터 직접 결정하는 것이며, 이를 통해 알고리즘이 자체적으로 적응적 동작을 할 수 있도록 하였다.

10. 알고리즘의 매개변수 튜닝

10.1 $\beta$ 의 결정

$\beta$ 의 값은 자이로 잡음의 표준 편차에 비례한다. 일반적인 MEMS 자이로의 경우 $\beta \approx 0.033$ 정도가 좋은 시작점이며, 응용에 따라 조정한다. 너무 큰 $\beta$ 는 가속도계 잡음에 민감해지고, 너무 작은 $\beta$ 는 바이어스 표류를 효과적으로 보정하지 못한다.

10.2 샘플 주기

샘플 주기 $\Delta t$ 는 자이로의 출력률에 의해 결정된다. 일반적으로 100 Hz에서 1000 Hz의 범위가 사용된다. 높은 샘플 주파수가 더 정확한 적분을 제공하지만 계산 부하가 증가한다.

11. 마드윅 필터의 장단점

11.1 장점

계산량이 매우 적어 임베디드 시스템에 이상적이다.
매개변수가 단 하나( $\beta$ )뿐이라 튜닝이 간편하다.
소스 코드가 공개되어 있어 즉시 사용 가능하다.
임베디드 환경에서 검증된 성능을 제공한다.
쿼터니언 기반이라 특이점이 없다.

11.2 단점

통계적 최적성이 보장되지 않는다.
자이로 바이어스의 명시적 추정이 표준 형태에 포함되지 않는다.
가속도계의 동적 가속도에 민감할 수 있다.
보정 이득 $\beta$ 가 시변하는 잡음 특성에 자동 적응하지 않는다.

12. 마호니 필터와의 비교

마드윅 필터와 마호니 필터는 모두 가속도계 및 지자기계를 사용한 비선형 자세 추정 알고리즘이지만, 보정 방식에 차이가 있다.

항목	마드윅 필터	마호니 필터
보정 원리	경사 하강	PI 제어 (외적 기반)
매개변수	$\beta$ (1개)	$k_P$ , $k_I$ (2개)
바이어스 추정	표준 형태에서 미포함	적분 항으로 포함
계산량	매우 적음	매우 적음
안정성 분석	휴리스틱	리아푸노프 안정성 증명

두 알고리즘 모두 임베디드 환경에서 우수한 성능을 보이며, 응용 환경에 따라 선택된다.

13. 마드윅 필터의 응용

13.1 무인 항공기

소형 드론과 무인 비행 시스템의 자세 추정에서 마드윅 필터는 표준 알고리즘으로 사용된다. 오픈소스 비행 제어 시스템인 PX4와 ArduPilot의 일부 모듈이 이를 채택한다.

13.2 웨어러블 기기

손목 착용형 활동 추적기와 스마트 워치에서 사용자의 자세와 동작을 추정하는 데 사용된다. 저전력 동작이 가능하다는 점이 핵심 장점이다.

13.3 인간 동작 분석

스포츠 과학과 재활 의학에서 신체 부위의 자세를 분석하는 데 마드윅 필터가 광범위하게 사용된다. 여러 IMU를 신체에 부착하여 전신 동작을 캡처할 수 있다.

13.4 가상 현실 컨트롤러

저가형 VR 컨트롤러의 자세 추적에 사용된다. 정확도와 응답성의 균형이 우수하다.

13.5 로봇 보조 장비

모바일 로봇의 IMU 자세 추정에서 메인 알고리즘 또는 백업 알고리즘으로 활용된다.

14. 오픈 소스 구현

마드윅은 자신의 알고리즘을 C 언어와 MATLAB 소스 코드로 공개하였다. 이 코드는 수많은 프로젝트에 통합되어 있으며, 다양한 언어로 포팅되었다. ROS 패키지인 imu_filter_madgwick은 ROS 환경에서 마드윅 필터를 사용할 수 있게 해 준다.

15. 참고 문헌

Madgwick, S. O. H. (2010). “An efficient orientation filter for inertial and inertial/magnetic sensor arrays.” Technical Report, University of Bristol.
Madgwick, S. O. H., Harrison, A. J. L., & Vaidyanathan, R. (2011). “Estimation of IMU and MARG orientation using a gradient descent algorithm.” IEEE International Conference on Rehabilitation Robotics, 1–7.
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.
Sabatini, A. M. (2011). “Estimating three-dimensional orientation of human body parts by inertial/magnetic sensing.” Sensors, 11(2), 1489–1525.

version: 1.0