각속도로부터 자세 추정

개념에서 수식으로

먼저 이해할 내용

자이로 센서는 현재 자세가 아니라 회전하는 속도를 측정한다. 따라서 현재 자세를 얻으려면 이전 자세에 시간 동안 누적된 변화를 더해야 한다. 이 글은 이러한 예측 과정과, 느리게 누적되는 편향을 따로 추정해 빼는 과정을 연결한다.

기호를 먼저 읽기

ωₓ, ωᵧ, ω_z: 센서의 세 축 각속도
q̇: 시간에 따른 쿼터니언 변화율
Δt: 두 계산 시점 사이의 시간 간격
q_est,t−1: 이전 시점의 자세 추정값
ω_b, ω_c: 추정한 편향과 편향을 뺀 각속도
ζ, η: 편향 누적과 오차 보정의 크기에 관련된 계수

이 글의 흐름

각속도를 네 성분 형태로 넣어 q의 변화율을 구한다. 이를 Δt 동안 누적해 이전 자세에 더하고, 오차 방향에서 얻은 편향 추정값을 자이로 측정에서 뺀다.

주제와 표기

Orientation from angular rate

A tri-axis gyroscope

Angular rate

각속도와 자세 변화율을 구별한다

세 축 각속도 앞에 0을 붙여 쿼터니언 곱에 사용할 형태를 만든다. 이전 자세와 이 각속도를 곱한 식은 현재의 자세 변화율 q̇를 나타낸다. q̇는 자세 그 자체가 아니라 단위 시간 동안 얼마나 변하는지에 해당한다.

^{S} ω = (\begin{matrix} 0 & ω_{x} & ω_{y} & ω_{z} \end{matrix})

Quaternion

_{E}^{S} \dot{q} = \frac{1}{2}_{E}^{S} \hat{q} \otimes^{S} ω

Measured at time t

_{E}^{S} {\dot{q}}_{ω, t} = \frac{1}{2}_{E}^{S} {\hat{q}}_{e s t, t - 1} \otimes^{S} ω_{t}

$수식$

Orientation of the earth frame at time t

변화율에 시간 간격을 곱해 누적한다

이전 자세에 q̇Δt를 더하면 이번 시간 간격의 변화를 반영한 자세가 된다. 여기서 Δt가 빠지면 회전속도를 그대로 자세에 더하는 오류가 생긴다. 이해를 위한 단순 계산으로, 일정한 각속도 1 rad/s가 0.01 s 동안 유지된다면 그 구간의 회전각 증가는 0.01 rad이다. 이 예는 시간 누적의 의미를 보여주는 것이며 쿼터니언 성분 증가와 같은 숫자라는 뜻은 아니다.

_{E}^{S} q_{ω, t} =_{E}^{S} {\hat{q}}_{e s t, t - 1} +_{E}^{S} {\dot{q}}_{ω, t} Δ t

The samling period Δt

$수식$

Gyroscope bias drift compensation

With temperature and motion

Karman-based approaches → estimate the gyroscope bias

Mahony et al → gyroscope bias drift (the integral feedback of the error)

빠른 변화와 느린 편향을 분리한다

원문은 온도와 움직임에 따른 자이로 편향을 별도 문제로 다룬다. 오차에서 얻은 각속도 성분을 시간에 따라 더하고 ζ를 곱하여 편향 추정값을 갱신한다. 한 번의 오차만 빼는 것이 아니라, 계속 누적되는 성분을 추적하는 구조로 읽는다.

Normalized direction of the estimated error in the rate of change of orientation $_{E}^{S}$ ${\dot{\hat{q}}}_{ϵ}$

Gyroscope bias $^{S}$ $ω_{b}$

^{S} ω_{b, t} = ζ {\sum_{t}}^{S} ω_{ϵ, t} Δ t

The integral gain ζ

자세 오차의 방향을 각속도 오차로 연결한다

현재 추정 자세의 켤레와 자세 변화율을 결합하면 각속도 형태로 표현할 수 있다. 원문은 이 관계를 오차 변화율에도 적용하여 ω_ε를 구성한다. 이어서 측정 각속도에서 ω_b를 빼면 보정한 ω_c가 된다.

DC component of $^{S}$ $ω_{ϵ}$

_{E}^{S} {\dot{q}}_{ω, t} = \frac{1}{2}_{E}^{S} {\hat{q}}_{e s t, t - 1} \otimes^{S} ω_{t}

2_{E}^{S} {\hat{q}}_{e s t, t - 1}^{*} \otimes_{E}^{S} {\dot{q}}_{ω, t} =^{S} ω_{t}

^{S} ω_{ϵ, t} = 2_{E}^{S} {\hat{q}}_{e s t, t - 1}^{*} \otimes_{E}^{S} {\dot{\hat{q}}}_{ϵ, t}

$수식$

Gyroscope measurements $^{S}$ $ω_{c}$

^{S} ω_{c, t} =^{S} ω_{t} -^{S} ω_{b, t}

편향 갱신과 측정값 보정을 차례로 수행한다

첫 벡터 식은 각 축 편향에 오차 성분·시간 간격·이득의 곱을 더하는 갱신이다. 다음 벡터 식은 갱신한 편향을 각 축 측정값에서 빼는 보정이다. 같은 기호가 등호 양쪽에 쓰인 갱신식은 이전 값과 새 값을 구별하여 읽는다.

(\begin{matrix} w_{b x} \\ w_{b y} \\ w_{b z} \end{matrix}) = (\begin{matrix} w_{b x} \\ w_{b y} \\ w_{b z} \end{matrix}) + (\begin{matrix} w_{ϵ x} \\ w_{ϵ y} \\ w_{ϵ z} \end{matrix}) * Δ t * ζ

(\begin{matrix} w_{x} \\ w_{y} \\ w_{z} \end{matrix}) = (\begin{matrix} w_{x} \\ w_{y} \\ w_{z} \end{matrix}) - (\begin{matrix} w_{b x} \\ w_{b y} \\ w_{b z} \end{matrix})

Filter gains

Estimated mean zero gyroscope measurement error of each axis ${\tilde{ω}}_{η}$

Filter gain

필터 계수는 오차의 크기와 연결된다

마지막 부분은 자이로 측정오차와 편향 변화율을 필터 계수에 연결한다. 계수는 단순히 큰 값이 좋은 값이 아니라 어떤 오차를 얼마나 반영할지 나타내는 값이다. 원문 마지막 계수식은 같은 ζ가 양쪽에 나타나므로, 이를 즉시 대입 가능한 독립적인 수치 산정식으로 단정하지 않는다.

η = | \frac{1}{2} \hat{q} \otimes (\begin{matrix} 0 & {\tilde{ω}}_{η} & {\tilde{ω}}_{η} & {\tilde{ω}}_{η} \end{matrix}) | = \frac{3}{\sqrt{4}} {\tilde{ω}}_{η}

Estimated rate of gyroscope bias drift in each axis ${\dot{ω}}_{ζ}$

Filter gain

ζ = ζ \frac{3}{\sqrt{4}} {\dot{\tilde{ω}}}_{ζ}

정리하면

각속도에서 자세로 가는 연결에는 반드시 시간 간격이 들어간다. 또한 측정 변화와 편향 변화를 구별해야 누적 오차를 이해할 수 있다.

Rest Math · https://rmath.com/articles/gyroscope-orientation.html · 2026.09.08 v5 · 해설·음성판

먼저 이해할 내용

기호를 먼저 읽기

이 글의 흐름

A tri-axis gyroscope

Angular rate

01각속도와 자세 변화율을 구별한다

Quaternion

Measured at time t

Orientation of the earth frame at time t

02변화율에 시간 간격을 곱해 누적한다

Gyroscope bias drift compensation

With temperature and motion

03빠른 변화와 느린 편향을 분리한다

Gyroscope bias Sωb

04자세 오차의 방향을 각속도 오차로 연결한다

DC component of Sωϵ

Gyroscope measurements Sωc

05편향 갱신과 측정값 보정을 차례로 수행한다

Filter gains

Filter gain

06필터 계수는 오차의 크기와 연결된다

Filter gain

정리하면

함께 읽을 노트

각속도와 자세 변화율을 구별한다

변화율에 시간 간격을 곱해 누적한다

빠른 변화와 느린 편향을 분리한다

Gyroscope bias $^{S}$ $ω_{b}$

자세 오차의 방향을 각속도 오차로 연결한다

DC component of $^{S}$ $ω_{ϵ}$

Gyroscope measurements $^{S}$ $ω_{c}$

편향 갱신과 측정값 보정을 차례로 수행한다

필터 계수는 오차의 크기와 연결된다