관측 벡터로부터 자세 추정

개념에서 수식으로

먼저 이해할 내용

기준 방향을 알고 있고 센서에서 같은 방향을 측정했다면, 두 표현을 가장 잘 맞추는 회전을 찾을 수 있다. 이 글은 기준벡터를 현재 자세로 변환한 예측값과 실제 관측값의 차이를 줄여 자세를 추정하는 과정을 다룬다.

기호를 먼저 읽기

d̂ᴱ: 기준 좌표계에서 정한 방향
ŝˢ: 센서 좌표계에서 측정한 방향
q̂: 두 방향 표현을 연결할 자세
f: 변환한 기준 방향과 측정 방향의 차이
J: 자세 성분 변화에 대한 잔차의 민감도
ĝ, b̂, â, m̂: 중력·자기장 기준과 정규화된 관측 방향

이 글의 흐름

기준벡터와 측정벡터를 구별하고 동일한 좌표계로 맞춘다. 그 차이로 잔차를 만들고, 자코비안과 기울기를 이용해 자세를 갱신한다. 중력과 자기장에 같은 절차를 적용한 뒤 함께 묶는다.

주제와 표기

Orientation from vector observations

A tri-axis accelerometer (Linear accelerations due to motion)

기준 방향과 관측 방향을 구분한다

가속도계와 자기센서에 관한 문장은 어떤 방향 정보를 자세 추정에 사용할지 설명한다. d̂는 미리 정한 기준이고 ŝ는 센서가 관측한 방향이다. q는 이 두 표현 사이의 회전을 찾기 위해 조정할 값이다.

the field of gravity

A tri-axis magnetometer (Local magnetic flux and distortions)

the earth ' s magnetic field

Quaternion

_{E}^{S} \hat{q} = (\begin{matrix} q_{1} & q_{2} & q_{3} & q_{4} \end{matrix})

Predefined reference direction

같은 좌표계로 옮긴 뒤 차이를 구한다

q*⊗d̂⊗q는 기준 방향을 비교 가능한 표현으로 옮기는 항이다. 여기에서 측정한 ŝ를 빼면 예측과 관측의 차이 f가 된다. 서로 다른 좌표계에서 얻은 성분을 변환 없이 빼는 것이 아니라, 먼저 같은 기준에 놓는 것이 핵심이다.

^{E} \hat{d} = (\begin{matrix} 0 & d_{x} & d_{y} & d_{z} \end{matrix})

Measured direction

^{S} \hat{s} = (\begin{matrix} 0 & s_{x} & s_{y} & s_{z} \end{matrix})

Objective function

f [_{E}^{S} \hat{q},^{E} \hat{d},^{S} \hat{s}] =_{E}^{S} {\hat{q}}^{*} \otimes^{E} \hat{d} \otimes_{E}^{S} \hat{q} -^{S} \hat{s}

Quaternion may be found

min_{_{E}^{S} \hat{q} \in R^{4}} f [_{E}^{S} \hat{q},^{E} \hat{d},^{S} \hat{s}]

Gradient descent algorithm

잔차를 줄이는 방향으로 자세를 갱신한다

현재 q에서 잔차와 자코비안을 구하고, Jᵀf로 입력 성분 쪽의 변화 방향을 계산한다. 이 방향과 이동 크기를 이용해 다음 q를 구하는 것이 반복 갱신의 의미이다. 잔차벡터 자체를 하나의 최소값으로 비교한다기보다, 그 크기를 줄이는 문맥으로 읽는다.

Orientation estimation of $_{E}^{S}$ ${\hat{q}}_{n + 1}$

$수식$

step-size: α

Gradient of the solution surface (General form)

\nabla f [_{E}^{S} {\hat{q}}_{k},^{E} \hat{d},^{S} \hat{s}] = J^{T} [_{E}^{S} {\hat{q}}_{k},^{E} \hat{d}] f [_{E}^{S} {\hat{q}}_{k},^{E} \hat{d},^{S} \hat{s}]

Objective function

벡터 곱을 전개하여 자코비안에 연결한다

기준벡터와 측정벡터를 네 성분으로 쓰고, 켤레가 있는 곱을 성분별로 펼친다. 그 결과를 q의 각 성분으로 미분한 것이 자코비안이다. 긴 행렬과 성분별 결과는 이 연결을 구체화한 것이며, 이미지에만 있는 항의 계수는 실제 식을 확인해야 한다.

f [_{E}^{S} \hat{q},^{E} \hat{d},^{S} \hat{s}] =_{E}^{S} {\hat{q}}^{*} \otimes^{E} \hat{d} \otimes_{E}^{S} \hat{q} -^{S} \hat{s}

_{E}^{S} \hat{q} = (\begin{matrix} q_{1} & q_{2} & q_{3} & q_{4} \end{matrix})

^{E} \hat{d} = (\begin{matrix} 0 & d_{x} & d_{y} & d_{z} \end{matrix})

^{S} \hat{s} = (\begin{matrix} 0 & s_{x} & s_{y} & s_{z} \end{matrix})

_{E}^{S} {\hat{q}}^{*} \otimes^{E} \hat{d} \otimes_{E}^{S} \hat{q} = (\begin{matrix} q_{1} & - q_{2} & - q_{3} & - q_{4} \end{matrix}) \otimes (\begin{matrix} 0 & d_{x} & d_{y} & d_{z} \end{matrix}) \otimes (\begin{matrix} q_{1} & q_{2} & q_{3} & q_{4} \end{matrix})

$수식$

Jacobian matrix

$수식$

Gradient of the solution surface (General form)

\nabla f = J^{T} f

\nabla f_{g, b} = {J_{g, b} [_{E}^{S} \hat{q},^{E} \hat{b}]}^{T} f_{g, b} [_{E}^{S} \hat{q},^{S} \hat{a},^{E} \hat{b},^{S} \hat{m}]

$수식$

Calculation induction

$수식$

Direction of gravity (Vertical axis, z axis)

Appropriate convention (the equations simplify)

중력 기준을 선택하면 식이 단순해진다

기준 중력을 z축으로 두면 방향벡터의 대부분 성분이 0이 된다. 정규화한 가속도 측정 â와 비교하여 중력 잔차를 만들고, q의 네 성분에 대한 자코비안 J_g를 얻는다. 기준을 단순하게 정하는 것이 이후 식의 항 수를 줄여 주는 이유이다.

^{E} \hat{g} = (\begin{matrix} 0 & 0 & 0 & 1 \end{matrix})

Normalized accelermeter measurement

^{S} \hat{a} = (\begin{matrix} 0 & a_{x} & a_{y} & a_{z} \end{matrix})

Objective function

$수식$

Jacobian matrix

J_{g} [_{E}^{S} \hat{q}] = (\begin{matrix} - 2 q_{3} & 2 q_{4} & - 2 q_{1} & 2 q_{2} \\ 2 q_{2} & 2 q_{1} & 2 q_{4} & 2 q_{3} \\ 0 & - 4 q_{2} & - 4 q_{3} & 0 \end{matrix})

Earth's magnetic field (One horizontal axis, Vertical axis)

Appropriate convention (the equations simplify)

자기장도 같은 잔차 구조로 다룬다

기준 자기장은 수평 성분과 수직 성분으로 표현한다. 측정한 자기장 m̂를 이 기준과 비교하면 중력과 별도의 잔차를 만들 수 있다. 이 구간의 측정벡터는 m̂로 표기되어 있으므로, 가까이 있는 가속도 관련 제목만 보고 â와 같은 값으로 취급하지 않는다.

^{E} \hat{b} = (\begin{matrix} 0 & b_{x} & 0 & b_{z} \end{matrix})

Normalized accelermeter measurement

^{S} \hat{m} = (\begin{matrix} 0 & m_{x} & m_{y} & m_{z} \end{matrix})

Objective function

$수식$

Jacobian matrix

$수식$

Magnetic distortion compensation

Declination errors

Horizontal plane:earth's surface (Heading)

Inclination errors

Vertical plane: earth's surface (Sensor's attitude)

The measured direction of the earth's magnetic field in the earth frame at time t

측정 자기장을 기준 좌표계로 옮긴다

q와 q* 사이에 m̂를 두어 기준 좌표계에서의 자기장 h를 구한다. 그다음 hₓ와 hᵧ를 제곱해 더한 뒤 제곱근을 취하면 수평 성분의 크기가 된다. 원문은 이 값을 bₓ에, h_z를 b_z에 놓아 다음 비교에 사용할 기준벡터를 구성한다.

^{E} {\hat{h}}_{t} = (\begin{matrix} 0 & h_{x} & h_{y} & h_{z} \end{matrix}) =_{E}^{S} {\hat{q}}_{e s t, t - 1} \otimes^{S} {\hat{m}}_{t} \otimes_{E}^{S} {\hat{q}}_{e s t, t - 1}^{*}

(\begin{matrix} 0 & h_{x} & h_{y} & h_{z} \end{matrix}) =_{E}^{S} {\hat{q}}_{e s t, t - 1} \otimes^{S} {\hat{m}}_{t} \otimes_{E}^{S} {\hat{q}}_{e s t, t - 1}^{*}

= (\begin{matrix} q_{1} & q_{2} & q_{3} & q_{4} \end{matrix}) \otimes (\begin{matrix} 0 & m_{x} & m_{y} & m_{z} \end{matrix}) \otimes (\begin{matrix} q_{1} & - q_{2} & - q_{3} & - q_{4} \end{matrix})

$수식$

^{E} {\hat{b}}_{t} = (\begin{matrix} 0 & \sqrt{{h_{x}}^{2} + {h_{y}}^{2}} & 0 & h_{z} \end{matrix})

(\begin{matrix} 0 & b_{x} & b_{y} & b_{z} \end{matrix}) = (\begin{matrix} 0 & \sqrt{{h_{x}}^{2} + {h_{y}}^{2}} & 0 & h_{z} \end{matrix})

Solution surface → minimum

중력과 자기장 정보를 하나의 보정 문제로 묶는다

중력 잔차와 자기장 잔차를 함께 고려하면 두 기준 방향에 대한 불일치를 동시에 다루게 된다. 결합한 자코비안과 잔차의 곱으로 q 성분별 변화 방향을 얻는 것이 마지막 전개의 목적이다. 묶는 행렬의 실제 행·열 배치는 제시된 전개와 차원을 확인하며 읽는다.

\nabla f [_{E}^{S} {\hat{q}}_{k},^{E} \hat{d},^{S} \hat{s}] = {J [_{E}^{S} {\hat{q}}_{k},^{E} \hat{d}]}^{T} \cdot f [_{E}^{S} {\hat{q}}_{k},^{E} \hat{d},^{S} \hat{s}]

Objective function

$수식$

Jacobian matrix

J_{g, b} [_{E}^{S} \hat{q},^{E} \hat{b}] = (\begin{matrix} {J_{g}}^{T} [_{E}^{S} \hat{q}] \\ {J_{b}}^{T} [_{E}^{S} \hat{q},^{E} \hat{b}] \end{matrix})

$수식$

Gradient of the solution surface (General form)

$수식$

정리하면

센서의 숫자를 바로 자세라고 해석하지 않고, 기준 방향과 관측 방향이 얼마나 잘 맞는지를 기준으로 자세를 정한다. 이때 좌표계와 정규화 조건이 계산의 전제이다.

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

먼저 이해할 내용

기호를 먼저 읽기

이 글의 흐름

A tri-axis accelerometer (Linear accelerations due to motion)

01기준 방향과 관측 방향을 구분한다

A tri-axis magnetometer (Local magnetic flux and distortions)

Quaternion

Predefined reference direction

02같은 좌표계로 옮긴 뒤 차이를 구한다

Measured direction

Objective function

Quaternion may be found

Gradient descent algorithm

03잔차를 줄이는 방향으로 자세를 갱신한다

Orientation estimation of ESq^n+1

Gradient of the solution surface (General form)

Objective function

04벡터 곱을 전개하여 자코비안에 연결한다

Jacobian matrix

Gradient of the solution surface (General form)

Calculation induction

Direction of gravity (Vertical axis, z axis)

Appropriate convention (the equations simplify)

05중력 기준을 선택하면 식이 단순해진다

Normalized accelermeter measurement

Objective function

Jacobian matrix

Earth's magnetic field (One horizontal axis, Vertical axis)

Appropriate convention (the equations simplify)

06자기장도 같은 잔차 구조로 다룬다

Normalized accelermeter measurement

Objective function

Jacobian matrix

Magnetic distortion compensation

Declination errors

Inclination errors

The measured direction of the earth's magnetic field in the earth frame at time t

07측정 자기장을 기준 좌표계로 옮긴다

Solution surface → minimum

08중력과 자기장 정보를 하나의 보정 문제로 묶는다

Objective function

Jacobian matrix

Gradient of the solution surface (General form)

정리하면

함께 읽을 노트

기준 방향과 관측 방향을 구분한다

같은 좌표계로 옮긴 뒤 차이를 구한다

잔차를 줄이는 방향으로 자세를 갱신한다

Orientation estimation of $_{E}^{S}$ ${\hat{q}}_{n + 1}$

벡터 곱을 전개하여 자코비안에 연결한다

중력 기준을 선택하면 식이 단순해진다

자기장도 같은 잔차 구조로 다룬다

측정 자기장을 기준 좌표계로 옮긴다

중력과 자기장 정보를 하나의 보정 문제로 묶는다