센서융합 알고리즘

개념에서 수식으로

먼저 이해할 내용

자이로로 예측한 자세와 관측 방향으로 보정한 자세는 서로 다른 경로에서 얻은 정보이다. 이 글은 두 값을 섞는 식에서 출발해, 이전 자세에 예측 변화율을 더하고 오차 방향을 빼는 하나의 갱신식으로 정리한다.

기호를 먼저 읽기

q_ω,t: 각속도로 예측한 자세
q_∇,t: 관측 잔차의 기울기로 보정한 자세
β_t: 두 경로의 비중을 정하는 0~1 사이의 값
η: 오차 방향에 적용하는 보정 크기 관련 계수
q̇_ε: 정규화된 오차 방향
Δt: 한 단계의 시간 간격

이 글의 흐름

가중 결합식을 읽고, 각 경로의 식을 대입한다. 발산과 수렴의 균형에서 β_t를 정리한 뒤 원문이 제시한 근사 조건을 거쳐 하나의 변화율 갱신식으로 모은다.

주제와 표기

Filter fusion algorithm

Estimated orientation

두 추정값을 비중에 따라 결합한다

β_t가 기울기 기반 추정에, 1−β_t가 자이로 기반 추정에 곱해진다. β_t=0이면 자이로 경로만, β_t=1이면 기울기 경로만 남는다. 중간값에서는 두 경로를 함께 쓰지만, 결합 후 단위 쿼터니언 조건이 자동으로 보장된다고 가정하지 않는다.

_{E}^{S} q_{e s t, t} = β_{t}_{E}^{S} q_{\nabla, t} + (1 - β_{t})_{E}^{S} q_{ω, t}, 0 \leq β_{t} \leq 1

_{E}^{S} q_{e s t, t} = β_{t}_{E}^{S} q_{\nabla, t} + (1 - β_{t})_{E}^{S} q_{ω, t}, 0 \leq β_{t} \leq 1

각 경로의 식을 대입해 공통 항을 묶는다

두 추정값에 이전 자세가 공통으로 포함되어 있으므로, 대입 후 이전 자세와 변화 항을 분리할 수 있다. 자이로 항에는 Δt가, 기울기 항에는 α_t와 정규화된 방향이 붙는다. 같은 식을 다른 묶음으로 표현한 등호들을 따라가며 각 항의 출처를 확인한다.

$수식$

_{E}^{S} {\hat{q}}_{e s t, t} =_{E}^{S} {\hat{q}}_{e s t, t - 1} - β_{t} α_{t} \nabla \frac{f}{| \nabla f |} + (1 - β_{t})_{E}^{S} {\dot{q}}_{ω, t} Δ t

_{E}^{S} {\hat{q}}_{e s t, t} =_{E}^{S} {\hat{q}}_{e s t, t - 1} + ((1 - β_{t})_{E}^{S} {\dot{q}}_{ω, t} - β_{t} \frac{α_{t}}{Δ} t \nabla \frac{f}{| \nabla f |}) Δ t

$수식$

가중치의 선택을 균형식으로 읽는다

원문은 자이로 경로의 가중 오차와 보정 경로의 가중 수렴을 맞추는 관계를 둔다. 이 식을 β_t에 대해 정리하면 η와 α_t/Δt가 들어간 비율이 나온다. 가중치를 정하는 기준을 먼저 놓고 그 결과를 얻은 것이므로, 분수만 독립적으로 외우지 않는다.

η, $\frac{α_{t}}{Δ} t$

$Weighted divergence o f_{E}^{S} q_{ω}$	==	$Weighted convergence o f_{E}^{S} q_{\nabla}$
η		$\frac{α_{t}}{Δ} t$

Magnitude of a quaternion derivative == Gyroscope measurement error

(1 - β_{t}) η = β_{t} \frac{α_{t}}{Δ} t

β_{t} = \frac{η}{\frac{α_{t}}{Δ} t + η}

Optimal fusion

근사 조건 아래에서 식을 단순화한다

이후의 근사 기호는 앞 식과 모든 조건에서 정확히 같다는 뜻이 아니다. 원문에서 설명한 수렴 속도 관계를 전제로 β_t와 보정 추정을 단순화한다. 근사를 적용하기 전과 후에 어떤 항을 남기는지 확인해야 최종식의 적용 범위를 이해할 수 있다.

Convergence rate of $_{E}^{S}$ $q_{\nabla}$ ≥γ → Physical rate of change of orientation

_{E}^{S} q_{\nabla, t} \approx - α_{t} \nabla \frac{f}{| \nabla f |}

β_{t} \approx \frac{η Δ t}{α_{t}}

Simplify

Estimated orientation

예측 변화와 보정 변화를 한 줄에 모은다

이전 자세에 자이로 변화율×Δt를 더하고, 정규화된 오차 방향×η×Δt를 뺀다. 괄호로 묶으면 두 변화율의 차이에 Δt를 곱한 형태가 된다. 즉 센서융합은 두 센서 값을 무조건 평균하는 것이 아니라, 예측과 오차 보정을 같은 시간 단계에 연결하는 과정이다.

_{E}^{S} q_{e s t, t} = \frac{η Δ t}{α_{t}} (- α_{t} \nabla \frac{f}{| \nabla f |}) + (1 - 0) (_{E}^{S} {\hat{q}}_{e s t, t - 1} +_{E}^{S} {\dot{q}}_{ω, t} Δ t)

$수식$

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

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

_{E}^{S} q_{e s t, t} =_{E}^{S} {\hat{q}}_{e s t, t - 1} + (_{E}^{S} {\dot{q}}_{ω, t} - η \nabla \frac{f}{| \nabla f |}) Δ t

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

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

_{E}^{S} q_{e s t, t} =_{E}^{S} {\hat{q}}_{e s t, t - 1} + (\frac{1}{2}_{E}^{S} {\hat{q}}_{e s t, t - 1} \otimes^{S} ω_{t} - η \nabla \frac{f}{| \nabla f |}) Δ t

Estimated orientation rate $_{E}^{S}$ ${\dot{q}}_{est}$

Rate of change of orientation measured by gyroscopes $_{E}^{S}$ ${\dot{q}}_{ω}$

Direction of the estimated error $_{E}^{S}$ ${\dot{\hat{q}}}_{ϵ}$

Simplified to equation

최종 구현식의 각 항을 확인한다

q̇_est는 q̇_ω에서 ηq̇_ε를 뺀 값으로 정의된다. q̇_ω는 이전 q와 측정 각속도로 계산하고, q̇_ε는 잔차 기울기를 정규화해 얻는다. 마지막 자코비안 전개는 이 오차 방향을 성분별로 계산하기 위한 구간이며, 잔차와 행렬의 차원을 함께 확인한다.

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

Estimated rate of change of orientation

_{E}^{S} {\dot{q}}_{e s t, t} =_{E}^{S} {\dot{q}}_{ω, t} - η_{E}^{S} {\dot{\hat{q}}}_{ϵ, t}

Quaternion derivative measured at time t

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

Direction of error of $_{E}^{S}$ ${\dot{q}}_{e s t, t}$

_{E}^{S} {\dot{\hat{q}}}_{ϵ, t} = \nabla \frac{f}{| \nabla f |}

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)

\nabla f = J^{T} f

$수식$

Calculation induction

$수식$

({{-2q_3, 2q_4, -2q_1, 2q_2}, {2q_2, 2q_1, 2q_4, 2q_3}, {0, -4q_2, -4q_3, 0}})  . ({{f_1}, {f_2}, {f_3}})//MatrixForm

(\begin{matrix} 2 f_{2} q_{2} - 2 f_{1} q_{3} \\ 2 f_{2} q_{1} - 4 f_{3} q_{2} + 2 f_{1} q_{4} \\ - 2 f_{1} q_{1} - 4 f_{3} q_{3} + 2 f_{2} q_{4} \\ 2 f_{1} q_{2} + 2 f_{2} q_{3} \end{matrix})

$수식$

정리하면

최종식은 이전 자세, 자이로가 예측하는 변화, 관측이 제공하는 보정의 세 부분으로 읽을 수 있다. 근사 전후의 식과 계수 역할을 구별하는 것이 중요하다.

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

먼저 이해할 내용

기호를 먼저 읽기

이 글의 흐름

Estimated orientation

01두 추정값을 비중에 따라 결합한다

02각 경로의 식을 대입해 공통 항을 묶는다

03가중치의 선택을 균형식으로 읽는다

η, α t Δ t

Magnitude of a quaternion derivative == Gyroscope measurement error

Optimal fusion

04근사 조건 아래에서 식을 단순화한다

Convergence rate of E S q ∇ ≥γ → Physical rate of change of orientation

Simplify

Estimated orientation

05예측 변화와 보정 변화를 한 줄에 모은다

Simplified to equation

06최종 구현식의 각 항을 확인한다

Estimated rate of change of orientation

Quaternion derivative measured at time t

Direction of error of E S q ˙ e s t , t

Objective function

Jacobian matrix

Gradient of the solution surface (General form)

Calculation induction

정리하면

함께 읽을 노트

두 추정값을 비중에 따라 결합한다

각 경로의 식을 대입해 공통 항을 묶는다

가중치의 선택을 균형식으로 읽는다

η, $\frac{α_{t}}{Δ} t$

근사 조건 아래에서 식을 단순화한다

Convergence rate of $_{E}^{S}$ $q_{\nabla}$ ≥γ → Physical rate of change of orientation

예측 변화와 보정 변화를 한 줄에 모은다

최종 구현식의 각 항을 확인한다

Direction of error of $_{E}^{S}$ ${\dot{q}}_{e s t, t}$