
Rotate Frame#
κ°λ κ³μ°μ μν λͺκ°μ§ κ°μ μ νκ² μ΅λλ€.
- global(0) frameμ κ΄μ°°μκ° λ³Ό λ 0aλ ν¬κΈ°μ λ°©ν₯μ΄ λ°λμ§ μλλ€.
- 0 frameμμ νμ μ μμνμ¬ n λ²μ§Έ κ΄μΈ‘λ μ’νκ³λ₯Ό n frameμ΄λΌνκ³ , μΆ©λΆν μ§§μ μκ° κ°κ²©μΌλ‘ κ΄μΈ‘νλ€.
- k frameμμ k+1 frameμΌλ‘ νμ ν λ x, y, z μΆ κΈ°μ€μΌλ‘ νμ κ°μ μΈ‘μ ν μ μλ€.
- μΆ©λΆν μ§§μ μκ° κ΄μΈ‘νλ―λ‘, μΈ‘μ λ νμ κ°μ small angle approximationμ μ μ©ν΄λ λ λ§νΌ μΆ©λΆν μλ€.
k frameμμ k+1 frameμΌλ‘ νμ ν λ, xμΆμΌλ‘ dΟ, yμΆμΌλ‘ dΞΈ, zμΆμΌλ‘ dΟλ§νΌ νμ νλ€λ©΄ κ° μΆμ κΈ°μ€μΌλ‘ νμ μΏΌν°λμΈμ μλμ κ°μ΅λλ€.
qβ(x^,dΟ)qβ(y^β,dΞΈ)qβ(z^,dΟ)β=[sin2dΟββ0β0βcos2dΟββ]T=[0βsin2dΞΈββ0βcos2dΞΈββ]T=[0β0βsin2dΟββcos2dΟββ]Tββ[2dΟββ0β0β1β]Tβ[0β2dΞΈββ0β1β]Tβ[0β0β2dΟββ1β]Tβ(dΟβ0)(dΞΈβ0)(dΟβ0)β νμ κ°μ΄ ν° κ²½μ° νμ μμμ λ°λΌ κ²°κ³Όκ° λ¬λΌμ§μ§λ§ μΆ©λΆν μμ κ²½μ° νμ μμμ λ°λ₯Έ κ²°κ³Όκ° ν¬κ² λ€λ₯΄μ§ μμ΅λλ€.
q(a1β,ΞΈ1β)q(a2β,ΞΈ2β)ξ =q(a2β,ΞΈ2β)q(a1β,ΞΈ1β) q(a1β,dΞΈ1β)q(a2β,dΞΈ2β)βq(a2β,dΞΈ2β)q(a1β,dΞΈ1β)dΞΈβ0 k frameμμ k+1 frameμΌλ‘ νμ ν λ, μΆ©λΆν μμ κ°λ λ³νλΌκ³ κ°μ νμΌλ―λ‘ μμλ‘ Z-Y-X μμΌλ‘ νμ νλ€κ³ κ°μ νμ¬ qβk+1kβλ₯Ό ꡬνλ©΄ μλμ κ°μ΅λλ€.
qk+1kβqβk+1kβββββq(z^,dΟ)q(y^β,dΞΈ)q(x^,dΟ)[2dΟββ2dΞΈββ2dΟββ1β]Tβ κ° κ΅¬κ°λ³ νμ κ° μΈ‘μ μ ν΅ν΄ 0 frameμμ n frameμ΄ λλ νμ μΏΌν°λμΈμ ꡬν μ μμ΅λλ€.
qn0β=q10βq21ββ―qnnβ1β qβn0β=[qxββqyββqzββqwββ]T small angle approximationμ μν΄ μκ°μ΄ νλ¦μ λ°λΌ λμ μ€μ°¨κ° μ¦κ°ν©λλ€.
0a0aββ=qn0βnaqΛβn0β=R(qΛβn0β)L(qn0β)naβ=β£β’β’β’β‘β1β2(qyβ2+qzβ2)2(qxβqyβ+qzβqwβ)2(qxβqzββqyβqwβ)0β2(qxβqyββqzβqwβ)1β2(qxβ2+qzβ2)2(qyβqzβ+qxβqwβ)0β2(qxβqzβ+qyβqwβ)2(qyβqzββqxβqwβ)1β2(qxβ2+qyβ2)0β0001ββ¦β₯β₯β₯β€βnaββ Euler angles#
xμΆ, yμΆ, zμΆ κ° μΆμ κΈ°μ€μΌλ‘νλ νμ νλ ¬μ μλμ κ°μ΅λλ€.
rot(X,Ο)rot(Y,ΞΈ)rot(Z,Ο)β===ββ£β’β’β’β‘β1000β0cosΟsinΟ0β0βsinΟcosΟ0β0001ββ¦β₯β₯β₯β€ββ£β’β’β’β‘βcosΞΈ0βsinΞΈ0β0100βsinΞΈ0cosΞΈ0β0001ββ¦β₯β₯β₯β€ββ£β’β’β’β‘βcosΟsinΟ00ββsinΟcosΟ00β0010β0001ββ¦β₯β₯β₯β€ββ Euler anglesμλ proper Euler anglesκ³Ό Tait-Bryan angles λ κ·Έλ£Ήμ΄ μμ΅λλ€. κ° κ·Έλ£Ήμ 6κ°μ νμ μμλ₯Ό κ°μ΅λλ€.
AHRS ꡬμ±μ μν΄ Z-Y-X μμλ₯Ό μ¬μ©νλ Tait-Bryan angleμ μ¬μ©νκ² μ΅λλ€.
μΌλ°μ μΌλ‘ ZμΆμ Yaw, YμΆμ Pitch, XμΆμ Rollμ΄λΌκ³ λΆλ¦
λλ€. λ°©ν₯μ μλ κ·Έλ¦Όκ³Ό κ°μ΄ μ€μ νκ² μ΅λλ€.

Z-Y-X Euler angleμ μν νμ νλ ¬μ μλμ κ°μ΅λλ€.
0aββ=rot(Z,Ο)rot(Y,ΞΈ)rot(X,Ο)naβ=Rn0β=β£β’β’β’β‘βcosΞΈcosΟcosΞΈsinΟβsinΞΈ0βR12βR22βsinΟcosΞΈ0βR13βR23βcosΟcosΞΈ0β0001ββ¦β₯β₯β₯β€βnaββ R12βR13βR22βR23ββ=sinΟsinΞΈcosΟβcosΟsinΟ=cosΟsinΞΈcosΟ+sinΟsinΟ=sinΟsinΞΈsinΟ+cosΟcosΟ=cosΟsinΞΈsinΟβsinΟcosΟβ Quaternions and Euler angles#
νμ κ°μ μΈ‘μ μ ν΅ν΄ μΏΌν°λμΈ νμ νλ ¬μ κ²½μ° κ°μ μ μ μλ νλ ¬μ
λλ€. μΏΌν°λμΈ νμ νλ ¬κ³Ό Euler angle(Z-Y-X) νμ νλ ¬μ λΉκ΅νλ©΄ AHRS λ₯Ό ν΅ν΄ ꡬνκ³ μ νλ Attitudeμ Headingμ ꡬν μ μμ΅λλ€.
β£β’β’β’β‘β1β2(qyβ2+qzβ2)2(qxβqyβ+qzβqwβ)2(qxβqzββqyβqwβ)0β2(qxβqyββqzβqwβ)1β2(qxβ2+qzβ2)2(qyβqzβ+qxβqwβ)0β2(qxβqzβ+qyβqwβ)2(qyβqzββqxβqwβ)1β2(qxβ2+qyβ2)0β0001ββ¦β₯β₯β₯β€β=β£β’β’β’β‘βcosΞΈcosΟcosΞΈsinΟβsinΞΈ0βR12βR22βsinΟcosΞΈ0βR13βR23βcosΟcosΞΈ0β0001ββ¦β₯β₯β₯β€ββ β£β’β‘βΟΞΈΟββ¦β₯β€β=β£β’β‘βatan2(2(qyβqzβ+qxβqwβ),1β2(qx2β+qy2β))asin(2(qyβqwββqxβqzβ))atan2(2(qxβqyβ+qzβqwβ),1β2(qy2β+qz2β))ββ¦β₯β€ββ