Representation of orientation

Pose of a Rigid Body

[Graphics:HTMLFiles/index_1.gif]

Position

o^′ = ({{o_x^′}, {o_y^′}, {o_z^′}})

Orientation

x^′ = x_x^′x + x_y^′y + x_z^′z

y^′ = y_x^′x + y_y^′y + y_z^′z

z^′ = z_x^′x + z_y^′y + z_z^′z

ROTATION MATRIX

RR = I

R = R^(-1)

Analysis on Position And Orientation

Cartesian Coordinate (직교좌표계)

unit vector

[Graphics:HTMLFiles/index_10.gif]

One Point P Vector

P = POverscript[x,^] + POverscript[y,^] + POverscript[z,^] = p_xOverscript[x,^] + p_yOverscript[y,^] + p_zOverscript[z,^]

Direction Vector : P = ({{p_x}, {p_y}, {p_z}})

Representation of a vector

[Graphics:HTMLFiles/index_13.gif]

P = ({{p_x}, {p_y}, {p_z}})

P^′ = ({{p_x^′}, {p_y^′}, {p_z^′}})

p = ({{x^′, y^′, z^′}}) p^′ = R p^′

p^′ = Rp

Example

[Graphics:HTMLFiles/index_18.gif]

p_x = p_x^′Cos[α] - p_y^′Sin[α]

p_y = p_x^′Sin[α] + p_y^′Cos[α]

p_z = p_z^′

Elementary Rotations

2D Rotation

({{x^′}, {y^′}}) = ({{Cos[θ], -Sin[θ]}, {Sin[θ], Cos[θ]}}) . ({{x}, {y}}) = ( {{x Cos[θ] - y Sin[θ]}, {y Cos[θ] + x Sin[θ]}} )

3D Rotation

[Graphics:HTMLFiles/index_23.gif]

rotation of α about z

R_z[α] = ({{Cos[α], -Sin[α], 0}, {Sin[α], Cos[α], 0}, {0, 0, 1}})

rotation of β about y

R_y[β] = ({{Cos[β], 0, Sin[β]}, {0, 1, 0}, {-Sin[β], 0, Cos[β]}})

rotation of γ about x

R_x[γ] = ({{1, 0, 0}, {0, Cos[γ], -Sin[γ]}, {0, Sin[γ], Cos[γ]}})

위치 표현 (Position and Orientation)

[Graphics:HTMLFiles/index_27.gif]

좌표계 {A}, {B} 에서 본 점 P의 위치

Overscript[x,^] _B, Overscript[y,^] _B, Overscript[z,^] _B를 좌표계 {A} 의 좌표성분으로 표시하면

Overscript[x,^] _B = Overscript[x,^] _BOverscript[x,^] _A + Overscript[x,^] _BOverscript[y,^] _A + Overscript[x,^] _BOverscript[z,^] _A

Overscript[x,^] _B = ({{Overscript[x,^] _BOverscript[x,^] _A}, {Overscript[x,^] _BOverscript[y,^] _A}, {Overscript[x,^] _BOverscript[z,^] _A}})

좌표계 {A} 에서 본 벡터의 방향 , Direction Vector (방향 벡터)

P = ({{p_x}, {p_y}, {p_z}})

상대 개념 위치

상대위치표현 ^AP, ^BP의 표현

P = ^AP = ^Ap_xOverscript[x,^] _A + ^Ap_yOverscript[y,^] _A + ^Ap_zOverscript[z,^] _A

= ^BP = ^Bp_xOverscript[x,^] _B + ^Bp_yOverscript[y,^] _B + ^Bp_zOverscript[z,^] _B

3D Orthonormal Basis → Unit Vectors

Overscript[x,^] _A, Overscript[y,^] _A, Overscript[z,^] _A

| Overscript[x,^] _A | = | Overscript[y,^] _A | = | Overscript[z,^] _A | = 1

Overscript[x,^] _A·Overscript[y,^] _A = 0, Overscript[y,^] _A·Overscript[z,^] _A = 0, Overscript[z,^] _A·Overscript[x,^] _A = 0

Overscript[x,^] _A×Overscript[y,^] _A = Overscript[z,^] _A

Low of Cosines

[Graphics:HTMLFiles/index_43.gif]

c^2 = a^2 + b^2 - 2 a b Cos[γ]

a^2 = b^2 + c^2 - 2 b c Cos[α]

b^2 = a^2 + c^2 - 2 a c Cos[β]

Inner Product Space

x . y = | x || y | Cos[θ]

Half-Angle Identities

Cos[θ/2] = (1 + Cos[θ])/2^(1/2)

Sin[θ/2] = (1 - Cos[θ])/2^(1/2)

Tan[θ] = Sin[θ]/Cos[θ]

Sec[θ] = 1/Cos[θ]

연속좌표변환: 절대변환과 상대변환

이동좌표계 (moving frame)

변환 (회전 또는 병진) 에 의해 새롭게 얻어지는 좌표계

고정좌표계 (fixed frame)

변환에 관계없이 처음과 동일하게 고정된 좌표계

절대변환 (absolute transform)

상대변환 (relative transform)

좌표계{A}와{B} 사이의 변환을 나태내는 동차변환행렬

_B^AT = Trans[p_x, p_y, p_z] . Rot[z_A, α]

[Graphics:HTMLFiles/index_57.gif]

O: 이동좌표계를 따라 병진 후 이동좌표계에 대해 회전

[Graphics:HTMLFiles/index_58.gif]

X: 이동좌표계를 따라 회전 후 이동좌표계에 대해 병진

[Graphics:HTMLFiles/index_59.gif]

O: 고정좌표계를 따라 회전 후 고정좌표계에 대해 병진

[Graphics:HTMLFiles/index_60.gif]

X: 고정좌표계를 따라 병진 후 고정좌표계에 대해 회전

[Graphics:HTMLFiles/index_61.gif]

올바른 변환을 나타내는 조합

두가지 모두 동일한 결과 _B^AT를 나타냄

_B^AT = Trans[p_x, p_y, p_z] . Rot[α, β, γ]

∴상대변환의 구현 (post - multiply)

∴절대변환의 구현 (pre - multiply)

^Ar_D = ({{a}, {b}, {c}})

^Br_D = ({{a}, {b}, {c}}) = ^Br_D^'

^Ar_D^' = _B^AT . ^Br_D^'

^Ar_C^' = _B^AT . ^Br_C^'

방위의 다른 표현

방위의 표현

EULER ANGLES

rotation matrix

9 parameters with 6 constraints

minimal representation of orientation

3 independent parameters

Euler angle과 Roll-Pitch-Yaw 방법

각각 12가지 표현법 존재

Z-Y-Z 오일러 각

좌표계 {A} 의 Z축에 대해 α만큼 회전 후

_B^AT = R_Overscript[z,^][α] R_Overscript[y,^][β] R_Overscript[z,^][γ]

[Graphics:HTMLFiles/index_91.gif]

ZYZ angles

[Graphics:HTMLFiles/index_92.gif]

R[φ] = R_z[φ] R_y^′[ϑ] R_z^″[ψ]

Inverse problem

R = ({{r_11, r_12, r_13}, {r_21, r_22, r_23}, {r_31, r_32, r_33}})

the three ZYZ angles are (ϑ∈ (0, π))

φ = ArcTan[r_13, r_23]

ϑ = ArcTan[r_33, (r_13^2 + r_23^2)^(1/2)]

ψ = ArcTan[-r_31, r_32]

or (ϑ∈ (-π, 0))

φ = ArcTan[-r_13, -r_23]

ϑ = ArcTan[r_33, -(r_13^2 + r_23^2)^(1/2)]

ψ = ArcTan[r_31, -r_32]

Z-Y-X 오일러 각 (이동 좌표계)

좌표계 {A} 의 Z축에 대해 α만큼 회전 후

_B^AT = R_Overscript[z,^][α] R_Overscript[y,^][β] R_Overscript[x,^][γ]

[Graphics:HTMLFiles/index_112.gif]

Current frame rotation

R_2^0 = R_1^0R_2^1

[Graphics:HTMLFiles/index_114.gif]

Roll-Pitch-Yaw (고정 좌표계)

고정기준좌표계 {A} 의 X축에 대해 γ만큼 회전 후

고정좌표계의 Y축에 대해 β만큼 회전한 후,

Z - Y - X 오일러와 동일

_B^AT = R_Overscript[z,^][α] R_Overscript[y,^][β] R_Overscript[x,^][γ]

[Graphics:HTMLFiles/index_123.gif]

Fixed frame rotation

R_2^0 = R_2^1R_1^0

[Graphics:HTMLFiles/index_125.gif]

Direction Cosine

단위벡터의 내적→두 벡터 사이 가의 코사인 값

v = v e = ({{v_x, v_y, v_z}}) . ({{e_x}, {e_y}, {e_z}}) = v_xe_x + v_ye_y + v_ze_z

α = Cos[a] = (v e_x)/(| v |) = v_x/(v_x^2 + v_y^2 + v_z^2)^(1/2)

β = Cos[b] = (v e_y)/(| v |) = v_y/(v_x^2 + v_y^2 + v_z^2)^(1/2)

γ = Cos[c] = (v e_z)/(| v |) = v_z/(v_x^2 + v_y^2 + v_z^2)^(1/2)

α^2 + β^2 + γ^2 = Cos[a]^2 + Cos[b]^2 + Cos[c]^2 = 1

Rotation Matrix _B^AR

Rotation Matrix

Direction Cosine Matrix

좌표계{A}에 대한 좌표계{B}의 회전행렬

Overscript[x,^] _B = Overscript[x,^] _BOverscript[x,^] _A + Overscript[x,^] _BOverscript[y,^] _A + Overscript[x,^] _BOverscript[z,^] _A

Overscript[y,^] _B = Overscript[y,^] _BOverscript[x,^] _A + Overscript[y,^] _BOverscript[y,^] _A + Overscript[y,^] _BOverscript[z,^] _A

Overscript[z,^] _B = Overscript[z,^] _BOverscript[x,^] _A + Overscript[z,^] _BOverscript[y,^] _A + Overscript[z,^] _BOverscript[z,^] _A

물체의 방위 (상대적 회전량) 를 나타냄

^AP = _B^AR^BP

{B} 좌표계 성분을 {A} 좌표계 성분으로 변환

^AP = _B^AR^BP

회전행렬의 성질 (_A^BR=_B^AR^(-1)=_B^AR)

좌표계 {B} 에 대한 좌표계 {A} 의 회전행렬 : _A^BR

좌표계 {A} 에 대한 좌표계 {B} 의 회전행렬 : _B^AR

_A^BR = _B^AR

{{{^AP = _B^AR^BP}, {^BP = _A^BR ^AP}} ⇒ ^AP = _B^AR^BP = _B^AR_A^BR^AP ⇒ _B^AR_A^BR = I

_A^BR = _B^AR^(-1) = _B^AR

Cartesian coordinate (직교좌표계)

| Overscript[x,^] _A | = | Overscript[x,^] _B | = 1

Inner Product Space

Overscript[x,^] _B·Overscript[x,^] _A = | Overscript[x,^] _B || Overscript[x,^] _A | Cos[γ]

Low of Cosines

| Overscript[Overscript[x,^] _AOverscript[x,^] _B, →] |^2 = | Overscript[x,^] _A |^2 +| Overscript[x,^] _B |^2 -2 | Overscript[x,^] _A || Overscript[x,^] _B | Cos[γ]

Overscript[x,^] _B·Overscript[x,^] _A = | Overscript[x,^] _B || Overscript[x,^] _A | Cos[γ]

Cos[γ] = 1 - ((x_B_x - x_A_x)^2 + (x_B_y - x_A_y)^2 + (x_B_z - x_A_z)^2)/2

Overscript[x,^] _B·Overscript[x,^] _A = 1 - ((x_B_x - x_A_x)^2 + (x_B_y - x_A_y)^2 + (x_B_z - x_A_z)^2)/2

_B^AR : Transformation Matrix (변환행렬)

방위의 표현: 회전행렬

R_Overscript[z,^][ψ] = ( {{Cos[ψ], -Sin[ψ], 0}, {Sin[ψ], Cos[ψ], 0}, {0, 0, 1}} )

R_Overscript[y,^][θ] = ( {{Cos[θ], 0, Sin[θ]}, {0, 1, 0}, {-Sin[θ], 0, Cos[θ]}} )

R_Overscript[x,^][φ] = ( {{1, 0, 0}, {0, Cos[φ], -Sin[φ]}, {0, Sin[φ], Cos[φ]}} )

Rotation of a vector

p = R p^′

pp = p^′ RR p^′

Example

[Graphics:HTMLFiles/index_175.gif]

p_x = p_x^′Cos[α] - p_y^′Sin[α]

p_y = p_x^′Sin[α] + p_y^′Cos[α]

p_z = p_z^′

p = R_z[α] p^′

Composition Of Rotation Matrices

it describes the mutual orientation between two coordinate

frames ; its column vectors are the direction cosines of the

axes of the rotated frame with respect to the original frame

it represents the coordinate transformation between the

coordinates of a point expressed in two different frames

(with common origin)

it is the operator that allows the rotation of a vector in the

same coordinate frame

Composition Of Rotation Matrices

p^1 = R_2^1p^2

p^0 = R_1^0p^1

p^0 = R_2^0p^2

R_i^j = (R_j^i)^(-1) = (R_j^i) 

Mathematica

R_x[φ_] := ({{1, 0, 0}, {0, Cos[φ], -Sin[φ]}, {0, Sin[φ], Cos[φ]}})//MatrixForm

R_y[θ_] := ({{Cos[θ], 0, Sin[θ]}, {0, 1, 0}, {-Sin[θ], 0, Cos[θ]}})//MatrixForm

R_z[ψ_] := ({{Cos[ψ], -Sin[ψ], 0}, {Sin[ψ], Cos[ψ], 0}, {0, 0, 1}})//MatrixForm

R_z[ψ] . R_y[θ] . R_x[φ]

Rotation Matrix and Small Angels

Cos[α] ≈1, Sin[α] ≈α

R_zyx = R_z[ψ] R_y[θ] R_x[φ]

( {{1, -ψ, 0}, {ψ, 1, 0}, {0, 0, 1}} ) . ( {{1, 0, θ}, {0, 1, 0}, {-θ, 0, 1}} ) . ( {{1, 0, 0}, {0, 1, -φ}, {0, φ, 1}} )//MatrixForm

R_xyz = ( {{1, 0, 0}, {0, 1, -φ}, {0, φ, 1}} ) . ( {{1, 0, θ}, {0, 1, 0}, {-θ, 0, 1}} ) . ( {{1, -ψ, 0}, {ψ, 1, 0}, {0, 0, 1}} )//MatrixForm

Roll: φ
Pitch: θ

({{f_x}, {f_y}, {f_z}}) = ({{u}, {v}, {w}}) + ({{0, w, -v}, {-w, 0, u}, {v, -u, 0}}) . ({{p}, {q}, {r}}) + g ({{Sin[θ]}, {-Cos[θ] Sin[φ]}, {-Cos[θ] Cos[φ]}})

계산

({{Cos[θ], 0, Sin[θ]}, {0, 1, 0}, {-Sin[θ], 0, Cos[θ]}}) . ({{1, 0, 0}, {0, Cos[φ], -Sin[φ]}, {0, Sin[φ], Cos[φ]}})//MatrixForm

( {{Cos[θ], Sin[θ] Sin[φ], Cos[φ] Sin[θ]}, {0, Cos[φ], -Sin[φ]}, {-Sin[θ], Cos[θ] Sin[φ], Cos[θ] Cos[φ]}} )

({{0, 0, -g}}) . ({{Cos[θ], 0, Sin[θ]}, {0, 1, 0}, {-Sin[θ], 0, Cos[θ]}}) . ({{1, 0, 0}, {0, Cos[φ], -Sin[φ]}, {0, Sin[φ], Cos[φ]}})//MatrixForm

( {{g Sin[θ], -g Cos[θ] Sin[φ], -g Cos[θ] Cos[φ]}} )

( {{g Sin[θ]}, {-g Cos[θ] Sin[φ]}, {-g Cos[θ] Cos[φ]}} )

({{1, 0, 0}, {0, Cos[-φ], -Sin[-φ]}, {0, Sin[-φ], Cos[-φ]}})//MatrixForm

( {{1, 0, 0}, {0, Cos[φ], Sin[φ]}, {0, -Sin[φ], Cos[φ]}} )

yaw: φ

x = ({{φ}, {θ}, {φ}})

rotation vector: Roll: φ, Pitch: θ, yaw: ψ

({{f_x}, {f_y}, {f_z}}) = ({{u}, {v}, {w}}) + ({{0, w, -v}, {-w, 0, u}, {v, -u, 0}}) . ({{p}, {q}, {r}}) + g ({{Sin[θ]}, {-Cos[θ] Sin[φ]}, {-Cos[θ] Cos[φ]}})

({{0, 0, -g}}) . ({{Cos[θ], 0, Sin[θ]}, {0, 1, 0}, {-Sin[θ], 0, Cos[θ]}}) . ({{1, 0, 0}, {0, Cos[φ], -Sin[φ]}, {0, Sin[φ], Cos[φ]}})//MatrixForm

Tan[θ] = Sin[θ]/Cos[θ]

Sec[θ] = 1/Cos[θ]

Euler Angle

[Graphics:HTMLFiles/index_221.gif]

C_n^b = C_3^b[φ] C_2^3[θ] C_n^2[ψ]

R_zyx = R_z[ψ] R_y[θ] R_x[φ]

Derivation

RPY angles

[Graphics:HTMLFiles/index_233.gif]

R[φ] = R_z[φ] R_y[ϑ] R_x[ψ]

Euler Angle (Roll φ Overscript[x,^] _B, Pitch θ Overscript[y,^] _B, Yaw ψ Overscript[z,^] _B, ZYX) → Rotation Matrix _B^AR

_B^AR = R_z[ψ] . R_y[θ] . R_x[φ]

Inverse problem

R = ({{r_11, r_12, r_13}, {r_21, r_22, r_23}, {r_31, r_32, r_33}})

the three RPY angles are (ϑ∈ (-π/2, π/2))

φ = ArcTan[r_11, r_21]

ϑ = ArcTan[(r_32^2 + r_33^2)^(1/2), -r_31] = ArcSin[-r_31]

ψ = ArcTan[r_33, r_32]

or (ϑ∈ (π/2, 3 π/2))

φ = ArcTan[-r_11, -r_21]

ϑ = ArcTan[-(r_32^2 + r_33^2)^(1/2), -r_31] = ArcSin[-r_31]

ψ = ArcTan[-r_33, -r_32]

θ (Pitch) = π/2, Cos[θ]≈0

r_23 - r_12 = (Sin[θ] + 1) Sin[φ - ψ]

r_13 + r_22 = (Sin[θ] + 1) Cos[φ - ψ]

r_23 + r_12 = (Sin[θ] - 1) Sin[φ + ψ]

r_13 - r_22 = (Sin[θ] + 1) Cos[φ + ψ]

φ = ArcTan[r_13 - r_22, r_23 + r_12]

θ = ArcSin[-r_31]

ψ = φ - ArcTan[r_13 + r_22, r_23 - r_12]

Angle and Axis

[Graphics:HTMLFiles/index_260.gif]

R[ϑ, r] = R_z[α] R_y[β] R_z[ϑ] R_y[-β] R_z[-α]

R[ϑ, r] = R_z[α] . R_y[β] . R_z[ϑ] . R_y[-β] . R_z[-α]

Sin[α] = r_y/(r_x^2 + r_y^2)^(1/2)

Cos[α] = r_x/(r_x^2 + r_y^2)^(1/2)

Sin[β] = (r_x^2 + r_y^2)^(1/2)

Cos[β] = r_z

유도

Sin[α] = r_y/(r_x^2 + r_y^2)^(1/2)

Cos[α] = r_x/(r_x^2 + r_y^2)^(1/2)

Sin[β] = (r_x^2 + r_y^2)^(1/2)

Cos[β] = r_z

α = ArcTan[r_y/r_x] ;

β = ArcTan[(r_x^2 + r_y^2)^(1/2)/r_z] ;

r_x^2 + r_y^2 + r_z^2 = 1 ;

(1 + (r_x^2 + r_y^2)/r_z^2)^(1/2) r_z = 1 ;

Inverse problem

R = ({{r_11, r_12, r_13}, {r_21, r_22, r_23}, {r_31, r_32, r_33}})

the angle and axis of rotation are (Sin[ϑ] ≠0)

ϑ = Cos[(r_11 + r_22 + r_33 - 1)/2]^(-1)

r = 1/(2Sin[ϑ]) ({{r_32 - r_23}, {r_13 - r_31}, {r_21 - r_12}})

r_x^2 + r_y^2 + r_z^2 = 1

Angle θ and Axis r

r = ({{r_x}, {r_y}, {r_z}})

| r | = (r_x^2 + r_y^2 + r_z^2)^(1/2) = 1

Sin[α] = r_y/(r_x^2 + r_y^2)^(1/2)

Cos[α] = r_x/(r_x^2 + r_y^2)^(1/2)

Sin[β] = (r_x^2 + r_y^2)^(1/2)/(r_x^2 + r_y^2 + r_z^2)^(1/2) = (r_x^2 + r_y^2)^(1/2)

Cos[β] = r_z/(r_x^2 + r_y^2 + r_z^2)^(1/2) = r_z

α = ArcTan[r_y/r_x]

β = ArcTan[(r_x^2 + r_y^2)^(1/2)/r_z]

R[θ, r] = R_z[α] . R_y[β] . R_z[θ] . R_y[-β] . R_z[-α]

Rotation Matrix → Angle θ and Axis r

R[-θ, -r] = R[θ, r]

θ = ArcCos[(r_11 + r_22 + r_33 - 1)/2]

r = 1/(2Sin[θ]) ({{r_32 - r_23}, {r_13 - r_31}, {r_21 - r_12}})

유도

1

r_11 + r_22 + r_33

= r_x^2 (1 - Cos[θ]) + Cos[θ] + r_y^2 (1 - Cos[θ]) + Cos[θ] + r_z^2 (1 - Cos[θ]) + Cos[θ]

= (r_x^2 + r_y^2 + r_z^2) (1 - Cos[θ]) + 3Cos[θ]

= (1 - Cos[θ]) + 3Cos[θ]

= 1 + 2Cos[θ]

Cos[θ] = (r_11 + r_22 + r_33 - 1)/2

θ = ArcCos[(r_11 + r_22 + r_33 - 1)/2]

2

r_32 - r_23

= (r_yr_z (1 - Cos[θ]) + r_xSin[θ]) - (r_yr_z (1 - Cos[θ]) - r_xSin[θ])

= 2r_xSin[θ]

r_x = (r_32 - r_23)/(2Sin[θ])

3

r_13 - r_31

= (r_xr_z (1 - Cos[θ]) + r_ySin[θ]) - (r_xr_z (1 - Cos[θ]) - r_ySin[θ])

= 2r_ySin[θ]

r_y = (r_13 - r_31)/(2Sin[θ])

4

r_21 - r_12

= (r_xr_y (1 - Cos[θ]) + r_zSin[θ]) - (r_xr_y (1 - Cos[θ]) - r_zSin[θ])

= 2r_zSin[θ]

r_z = (r_21 - r_12)/(2Sin[θ])

5

θ = ArcCos[(r_11 + r_22 + r_33 - 1)/2]

Overscript[r,^] = ({{r_x}, {r_y}, {r_z}}) = 1/(2Sin[θ]) ({{r_32 - r_23}, {r_13 - r_31}, {r_21 - r_12}})

r_x^2 + r_y^2 + r_z^2 = 1

Quaternion → Rotation Matrix

Euler axis

Overscript[u,^] = ({{u_x}, {u_y}, {u_z}})

Angle

θ

Rotation Matrix

Concisely

R = I Cos[θ] + Sin[θ][u] _× + (1 - Cos[θ]) u⊗u

I = ({{1, 0, 0}, {0, 1, 0}, {0, 0, 1}})

[u] _× = ({{0, -u_z, u_y}, {u_z, 0, -u_x}, {-u_y, u_x, 0}})

u⊗u = ({{u_x^2, u_xu_y, u_xu_z}, {u_xu_y, u_y^2, u_yu_z}, {u_xu_z, u_yu_z, u_z^2}})

Quaternion Representation

A quaternion is a four-dimensional complex number

An arbitrary orientation of frame B relative to frame A

_B^AOverscript[q,^]

[Graphics:HTMLFiles/index_332.gif]

A rotation of angle θ around an axis ^AOverscript[r,^] defined in frame A

^AOverscript[r,^] = ({{r_x, r_y, r_z}})

normalized four-dimensional vector

_B^AOverscript[q,^] = ({{q_1, q_2, q_3, q_4}}) = ({{Cos[θ/2], -r_xSin[θ/2], -r_ySin[θ/2], -r_zSin[θ/2]}})

4parameter representation ρ={η,ε}

η = Cos[ϑ/2]

ε = Sin[ϑ/2] ^AOverscript[r,^]

(ϑ, ^AOverscript[r,^]) and (-ϑ, -^AOverscript[r,^]) give the same quaternion

quaternion extracted from R^(-1)=R^T

ρ^(-1) = {η, -ε}

the quaternion paramentrization obeys

q_1^2 + q_2^2 + q_3^2 + q_4^2 = 1

η^2 + ε_x^2 + ε_y^2 + ε_z^2 = 1

hypercomplex numbers {i,j,k}

a + b i + c j + d k

i^2 = j^2 = k^2 = -1

i j = -j i = k

j k = -k j = i

k i = -i k = j

2x2 Complex Matrices, The quaternion a + b i + c j + d k

({{a + b , c + d }, {-c + d, a - b}})

A quaternion conjugate

_B^AOverscript[q,^]^* = _A^BOverscript[q,^] = ({{q_1, -q_2, -q_3, -q_4}})

A quaternion product ⊗

_C^AOverscript[q,^] = _C^BOverscript[q,^] ⊗_B^AOverscript[q,^]

ρ_1 = {η_1, ε_1}, ρ_2 = {η_2, ε_2}

ρ_1 * ρ_2 = {η_1η_2 - ε_1ε_2, η_1ε_2 + η_2ε_1 + ε_1×ε_2}

Hamilton rule

a⊗b = ({{a_1, a_2, a_3, a_4}}) ⊗ ({{b_1, b_2, b_3, b_4}})

= (a_1 + a_2 i + a_3j + a_4k) (b_1 + b_2 i + b_3j + b_4k)

= a_1 b_1 + i^2 a_2 b_2 + j^2 a_3 b_3 + k^2 a_4 b_4

+i a_2 b_1 + i a_1 b_2 + j k a_4 b_3 + j k a_3 b_4

+j a_3 b_1 + i k a_4 b_2 + j a_1 b_3 + i k a_2 b_4

+k a_4 b_1 + i j a_3 b_2 + i j a_2 b_3 + k a_1 b_4

= a_1 b_1 - a_2 b_2 - a_3 b_3 - a_4 b_4

+i a_2 b_1 + i a_1 b_2 - i a_4 b_3 + i a_3 b_4

+j a_3 b_1 + j a_4 b_2 + j a_1 b_3 - j a_2 b_4

+k a_4 b_1 - k a_3 b_2 + k a_2 b_3 + k a_1 b_4

= a_1 b_1 - a_2 b_2 - a_3 b_3 - a_4 b_4

+i (a_1 b_2 + a_2 b_1 + a_3 b_4 - a_4 b_3)

+j (a_1 b_3 - a_2 b_4 + a_3 b_1 + a_4 b_2)

+k (a_1 b_4 + a_2 b_3 - a_3 b_2 + a_4 b_1)

= ({{a_1, a_2, a_3, a_4}}) . ({{b_1, b_2, b_3, b_4}, {-b_2, b_1, -b_4, b_3}, {-b_3, b_4, b_1, -b_2}, {-b_4, -b_3, b_2, b_1}})

Euler axis

Overscript[e,^] = ({{e_x}, {e_y}, {e_z}})

ε = ({{ε_x}, {ε_y}, {ε_z}})

Angle

θ

Vector's Elements

q_1 = Cos[θ/2]

q_2 = e_xSin[θ/2]

q_3 = e_ySin[θ/2]

q_4 = e_zSin[θ/2]

Inverse problem

R = ({{r_11, r_12, r_13}, {r_21, r_22, r_23}, {r_31, r_32, r_33}})

the quaternion is (η≥0)

η = 1/2 (r_11 + r_22 + r_33 + 1)^(1/2)

ε = 1/2 ({{Sign[r_32 - r_23] (r_11 - r_22 - r_33 + 1)^(1/2)}, {Sign[r_13 - r_31] (r_22 - r_33 - r_11 + 1)^(1/2)}, {Sign[r_21 - r_12] (r_33 - r_11 - r_22 + 1)^(1/2)}})

A three dimensional vector - Same vector from frame A to frame B

^Bυ = _B^AOverscript[q,^] ⊗^Aυ⊗_B^AOverscript[q,^]^*

= ({{0, ^Bυ_x, ^Bυ_y, ^Bυ_z}}) = ({{q_1, q_2, q_3, q_4}}) ⊗ ({{0, ^Aυ_x, ^Aυ_y, ^Aυ_z}}) ⊗ ({{q_1, -q_2, -q_3, -q_4}})

= ({{q_1, q_2, q_3, q_4}}) ⊗ ({{0, ^Aυ_x, ^Aυ_y, ^Aυ_z}}) ⊗ ({{q_1, -q_2, -q_3, -q_4}})

= ({{q_1, q_2, q_3, q_4}}) ⊗ ({{0, v_x, v_y, v_z}}) ⊗ ({{q_1, -q_2, -q_3, -q_4}})

0

^Bυ = _B^AOverscript[q,^] ⊗^Aυ⊗_B^AOverscript[q,^]^*

        q_1^2 + q_2^2 + q_3^2 + q_4^2 = 1

^Bυ = _A^BR . ^Aυ

Rodrigues formula

Vector

a b≠a×b

a b = a×b - a·b

x×y×z = (x·z) y - (x·y) z

a b c = (a×b - a·b) c

     = (a×b) c - (a·b) c

     = {(a×b) ×c - (a×b) ·c} - (a·b) c

     = {(a·c) b - (a·b) c - (a×b) ·c} - (a·b) c

     = (a·c) b - (a×b) ·c - 2 (a·b) c

u v u = (u·u) v - (u×v) ·u - 2 (u·v) u

     = (u·u) v - 2 (u·v) u

(x×y) ·z = (z×x) ·y = (y×z) ·x

(u×v) ·u = (u×u) ·v = (v×u) ·u = 0

1

v_rot = v Cos[θ] + (k×v) Sin[θ] + k (k·v) (1 - Cos[θ])

Given a rotation axis represented by a unit vector k and a vector v to rotate about k by the angle θ

Vector Projection of v on k

v_parallel = (k·v) k

Vector rejection of v from k

v_orthogonal = v - v_parallel = v - (k·v) k

w = k×v = k× (v_orthogonal + v_parallel) = k×v_orthogonal + k×v_parallel = k×v_orthogonal

v_orthogonal_rot = v_orthogonalCos[θ] + w Sin[θ]

= (v - (k·v) k) Cos[θ] + (k×v) Sin[θ]

v_rot = v_orthogonal_rot + v_parallel_rot

= v_orthogonal_rot + v_parallel

= (v - (k·v) k) Cos[θ] + (k×v) Sin[θ] + (k·v) k

= v Cos[θ] + (k×v) Sin[θ] + k (k·v) (1 - Cos[θ])

K = ({{0, -k_3, k_2}, {k_3, 0, -k_1}, {-k_2, k_1, 0}})

K v = k×v

v_rot = v Cos[θ] + (k×v) Sin[θ] + k (k·v) (1 - Cos[θ])

     = v Cos[θ] + K v Sin[θ] + k (k·v) (1 - Cos[θ])

     = v - v + v Cos[θ] + K v Sin[θ] + k (k·v) (1 - Cos[θ])

     = v - v (1 - Cos[θ]) + K v Sin[θ] + k (k·v) (1 - Cos[θ])

     = v + K v Sin[θ] + (k (k·v) - v) (1 - Cos[θ])

a× (b×c) = b (a·c) - c (a·b)

(k (k·v) - v) = (k (k·v) - v (k·k)) = k× (k×v) = K^2v

     = v + Sin[θ] K v + (1 - Cos[θ]) K^2v     = R v

R = I + Sin[θ] K + (1 - Cos[θ]) K^2

R = exp[θ K]

R[θ] R[φ] = R[θ + φ], R[0] = I

1

x = n· (n·x) - n× (n×x)

x^′ = n· (n·x) + Sin[α] (n×x) - Cos[α] n× (n×x)

x^′ = x + Sin[α] N x + (1 - Cos[α]) N^2x

x^′ = n· (n·x) + Sin[α] (n×x) - Cos[α] (n× (n×x))

= n· (n·x) + Sin[α] (n×x) - Cos[α] ((n·x) n - x)

= (1 - Cos[α]) n (n·x) + Sin[α] (n×x) + x Cos[α]

= x Cos[α] + (n×x) Sin[α] + n (n·x) (1 - Cos[α])

    a b = a×b - a·b

                x×y×z = (x·z) y - (x·y) z

         = {(a·c) b - (a·b) c - (a×b) ·c} - (a·b) c

                 (u×v) ·u = (u×u) ·v = (v×u) ·u = 0

         = (a·c) b - (a×b) ·c - 2 (a·b) c

    u v u = (u·u) v - (u×v) ·u - 2 (u·v) u         = (u·u) v - 2 (u·v) u

x^′ = n· (n·x) + Sin[α] (n×x) - Cos[α] (n× (n×x))

= x Cos[α] + (n×x) Sin[α] + n (n·x) (1 - Cos[α])

= x (Cos[α/2]^2 - Sin[α/2]^2) + (n×x) (2Cos[α/2] Sin[α/2]) + n (n·x) (2Sin[α/2]^2)

= x Cos[α/2]^2 + 2 (n×x) Cos[α/2] Sin[α/2] - (x (n·n) - 2n (n·x)) Sin[α/2]^2

= x Cos[α/2]^2 + (n×x + n×x) Cos[α/2] Sin[α/2] - ((n·n) x - (n×x) ·n - 2 (n·x) n) Sin[α/2]^2

= x Cos[α/2]^2 + (n×x - x×n) Cos[α/2] Sin[α/2] - (((n·n) x - (n·x) n - (n×x) ·n) - (n·x) n) Sin[α/2]^2

= x Cos[α/2]^2 + (n×x - x×n + x·n - n·x) Cos[α/2] Sin[α/2] - (((n×x) ×n - (n×x) ·n) - (n·x) n) Sin[α/2]^2

= x Cos[α/2]^2 + (n×x - n·x - (x×n - x·n)) Cos[α/2] Sin[α/2] - ((n×x) n - (n·x) n) Sin[α/2]^2

= x Cos[α/2]^2 + (n x - x n) Cos[α/2] Sin[α/2] - (n×x - n·x) n Sin[α/2]^2

= x Cos[α/2]^2 + n x Cos[α/2] Sin[α/2] - x n Cos[α/2] Sin[α/2] - n x n Sin[α/2]^2

= (Cos[α/2] + n Sin[α/2]) x (Cos[α/2] - n Sin[α/2])

= q x q^*

    q = Cos[α/2] + n Sin[α/2]

Axis/Angle → Rotation matrix→ Orientation _B^AOverscript[q,^]

Axis/Angle → Rotation Matrix _B^AR

α = ArcTan[r_y/r_x] ;

β = ArcTan[(r_x^2 + r_y^2)^(1/2)/r_z] ;

_B^AR = R[γ, Overscript[r,^]] = R_z[α] . R_y[β] . R_z[γ] . R_y[-β] . R_z[-α]

γ = ArcCos[(r_11 + r_22 + r_33 - 1)/2]

Overscript[r,^] = ({{r_x}, {r_y}, {r_z}}) = 1/(2Sin[γ]) ({{r_32 - r_23}, {r_13 - r_31}, {r_21 - r_12}})

Orientation _B^AOverscript[q,^] → Axis/Angle → Rotation Matrix _B^AR

{{Cos[γ/2] = ± (1 + Cos[γ])/2^(1/2), Sin[γ/2] = ± (1 - Cos[γ])/2^(1/2)}}

_B^AOverscript[q,^] = ({{q_1, q_2, q_3, q_4}}) = ({{Cos[γ/2], r_xSin[γ/2], r_ySin[γ/2], r_zSin[γ/2]}})

1

q_1 = Cos[γ/2] = ± (1 + Cos[γ])/2^(1/2)

q_2 = r_xSin[γ/2] = ±r_x (1 - Cos[γ])/2^(1/2)

q_3 = r_ySin[γ/2] = ±r_y (1 - Cos[γ])/2^(1/2)

q_4 = r_zSin[γ/2] = ±r_z (1 - Cos[γ])/2^(1/2)

2

q_1^2 = (1 + Cos[γ])/2

q_2^2 = r_x^2 (1 - Cos[γ])/2

q_3^2 = r_y^2 (1 - Cos[γ])/2

q_4^2 = r_z^2 (1 - Cos[γ])/2

3

Cos[γ] = 2q_1^2 - 1

r_x^2 (1 - Cos[γ]) = 2q_2^2

r_y^2 (1 - Cos[γ]) = 2q_3^2

r_z^2 (1 - Cos[γ]) = 2q_4^2

Sin[γ] = (1 - Cos[γ]^2)^(1/2) = ((1 - Cos[γ]) (1 + Cos[γ]))^(1/2) = (2q_1^2 (1 - Cos[γ]))^(1/2)

Sin[γ] = 2^(1/2) q_1 (1 - Cos[γ])^(1/2)

4

r_x = (2^(1/2) q_2)/(1 - Cos[γ])^(1/2)

r_y = (2^(1/2) q_3)/(1 - Cos[γ])^(1/2)

r_z = (2^(1/2) q_4)/(1 - Cos[γ])^(1/2)

Sin[γ] = 2^(1/2) q_1 (1 - Cos[γ])^(1/2)

5

6

({{q_1, q_2, q_3, q_4}}) = ({{η, ε_x, ε_y, ε_z}}) ;

Orientation _B^AOverscript[q,^] → Roll φOverscript[x,^] _B,Pitch θOverscript[y,^] _B,Yaw ψOverscript[z,^] _B

φ around Overscript[x,^] _B, θ around Overscript[y,^] _B, ψ around Overscript[z,^] _B

φ = ArcTan[_B^AR_33, _B^AR_32] = ArcTan[2q_1^2 - 1 + 2q_4^2, 2 (q_3q_4 - q_1q_2)]

θ = -ArcTan[(1 - _B^AR_31^2)^(1/2), _B^AR_31] = ArcSin[-_B^AR_31] = -ArcSin[2 (q_2q_4 + q_1q_3)]

ψ = ArcTan[_B^AR_11, _B^AR_12] = ArcTan[2q_1^2 - 1 + 2q_2^2, 2 (q_2q_3 - q_1q_4)]

Axis/Angle

[Graphics:HTMLFiles/index_517.gif]

A rotation of angle θ around an axis ^AOverscript[r,^] defined in frame A

^AOverscript[r,^] = ({{r_x, r_y, r_z}})

^AP = ({{^Ap_x}, {^Ap_y}, {^Ap_z}}), ^BP = ({{^Bp_x}, {^Bp_y}, {^Bp_z}})

Overscript[x,^] _A = ({{^Ax_x}, {^Ax_y}, {^Ax_z}}), Overscript[y,^] _A = ({{^Ay_x}, {^Ay_y}, {^Ay_z}}), Overscript[z,^] _A = ({{^Az_x}, {^Az_y}, {^Az_z}})

Overscript[x,^] _B = ({{^Bx_x}, {^Bx_y}, {^Bx_z}}), Overscript[y,^] _B = ({{^By_x}, {^By_y}, {^By_z}}), Overscript[z,^] _B = ({{^Bz_x}, {^Bz_y}, {^Bz_z}})

({{Overscript[x,^] _A}, {Overscript[y,^] _A}, {Overscript[z,^] _A}}), ({{Overscript[x,^] _B}, {Overscript[y,^] _B}, {Overscript[z,^] _B}})

^AP = _B^AR^BP

P = ^AP = ^Ap_xOverscript[x,^] _A + ^Ap_yOverscript[y,^] _A + ^Ap_zOverscript[z,^] _A
= ^BP = ^Bp_xOverscript[x,^] _B + ^Bp_yOverscript[y,^] _B + ^Bp_zOverscript[z,^] _B

Axis/Angle → Rotation Matrix _B^AR

α = ArcTan[r_y/r_x] ;

β = ArcTan[(r_x^2 + r_y^2)^(1/2)/r_z] ;

R[γ, r] = R_z[α] . R_y[β] . R_z[γ] . R_y[-β] . R_z[-α]

Axis/Angle →  Roll φ Overscript[x,^] _B, Pitch θ Overscript[y,^] _B, Yaw ψ Overscript[z,^] _B, ZYX

φ around Overscript[x,^] _B, θ around Overscript[y,^] _B, ψ around Overscript[z,^] _B

φ (Roll) = ArcTan[r_33, r_32] = ArcTan[r_z^2 (1 - Cos[γ]) + Cos[γ], r_yr_z (1 - Cos[γ]) + r_xSin[γ]]

θ (Pitch) = ArcSin[-r_31] = ArcSin[r_ySin[γ] - r_xr_z (1 - Cos[γ])]

ψ (Yaw) = ArcTan[r_11, R_12] = ArcTan[r_x^2 (1 - Cos[γ]) + Cos[γ], r_xr_y (1 - Cos[γ]) + r_zSin[γ]]

Axis/Angle → Quaternion _B^AOverscript[q,^]

An arbitrary orientation of frame B relative to frame A

_B^AOverscript[q,^] = ({{q_1, q_2, q_3, q_4}}) = ({{Cos[γ/2], -r_xSin[γ/2], -r_ySin[γ/2], -r_zSin[γ/2]}})

q_1^2 + q_2^2 + q_3^2 + q_4^2 = 1

Euler Angle (Roll φ Overscript[x,^] _B, Pitch θ Overscript[y,^] _B, Yaw ψ Overscript[z,^] _B, ZYX) → Rotation Matrix _B^AR

X axis (Roll) φ→R_x[φ] → {-π, π}

Y axis (Pitch) θ→R_y[θ] → {-π/2, π/2}

Z axis (Yaw) ψ→R_z[ψ] → {-π, π}

_B^AR = R_zyx = ({{r_11, r_12, r_13}, {r_21, r_22, r_23}, {r_31, r_32, r_33}})

= R_z[ψ] . R_y[θ] . R_x[φ]

Euler Angle (Roll φ Overscript[x,^] _B, Pitch θ Overscript[y,^] _B, Yaw ψ Overscript[z,^] _B, ZYX) → Axis/Angle

γ = ArcCos[(r_11 + r_22 + r_33 - 1)/2] = ArcCos[(Cos[θ] Cos[ψ] + Cos[φ] Cos[ψ] + Sin[θ] Sin[φ] Sin[ψ] + Cos[θ] Cos[φ] - 1)/2]

Euler Angle (Roll φ Overscript[x,^] _B, Pitch θ Overscript[y,^] _B, Yaw ψ Overscript[z,^] _B, ZYX) → Quaternion _B^AOverscript[q,^]

q_1 = 1/2 (r_11 + r_22 + r_33 + 1)^(1/2)

q_2 = -Sign[r_32 - r_23]/2 (r_11 - r_22 - r_33 + 1)^(1/2)

q_3 = -Sign[r_13 - r_31]/2 (r_22 - r_33 - r_11 + 1)^(1/2)

q_4 = -Sign[r_21 - r_12]/2 (r_33 - r_11 - r_22 + 1)^(1/2)

q_1 = 1/2 (r_11 + r_22 + r_33 + 1)^(1/2)

= 1/2 (1 + Cos[θ] Cos[φ] + Cos[θ] Cos[ψ] + Cos[φ] Cos[ψ] + Sin[θ] Sin[φ] Sin[ψ])^(1/2)

q_2 = -Sign[r_32 - r_23]/2 (r_11 - r_22 - r_33 + 1)^(1/2)

q_3 = -Sign[r_13 - r_31]/2 (r_22 - r_33 - r_11 + 1)^(1/2)

q_4 = -Sign[r_21 - r_12]/2 (r_33 - r_11 - r_22 + 1)^(1/2)

Quaternion _B^AOverscript[q,^] → Rotation Matrix _B^AR

_B^AOverscript[q,^] = ({{q_1, q_2, q_3, q_4}}) = ({{Cos[γ/2], -r_xSin[γ/2], -r_ySin[γ/2], -r_zSin[γ/2]}})

Cos[γ/2] = ± (1 + Cos[γ])/2^(1/2) , Sin[γ/2] = ± (1 - Cos[γ])/2^(1/2)

_B^AR = ({{r_11, r_12, r_13}, {r_21, r_22, r_23}, {r_31, r_32, r_33}})

Quaternion _B^AOverscript[q,^] → Euler Angle (Roll φ Overscript[x,^] _B, Pitch θ Overscript[y,^] _B, Yaw ψ Overscript[z,^] _B, ZYX)

φ = ArcTan[_B^AR_33, _B^AR_32] = ArcTan[2q_1^2 - 1 + 2q_4^2, 2 (q_3q_4 - q_1q_2)]

θ = ArcSin[-_B^AR_31] = -ArcSin[2 (q_2q_4 + q_1q_3)]

ψ = ArcTan[_B^AR_11, _B^AR_12] = ArcTan[2q_1^2 - 1 + 2q_2^2, 2 (q_2q_3 - q_1q_4)]

Quaternion _B^AOverscript[q,^] → Axis/Angle

1

γ = ArcCos[(r_11 + r_22 + r_33 - 1)/2]

= ArcCos[((2q_1^2 - 1 + 2q_2^2) + (2q_1^2 - 1 + 2q_3^2) + (2q_1^2 - 1 + 2q_4^2) - 1)/2]

= ArcCos[-2 + 3 q_1^2 + q_2^2 + q_3^2 + q_4^2]

= ArcCos[2 q_1^2 - 1]

2

Overscript[r,^] = ({{r_x}, {r_y}, {r_z}}) = 1/(2Sin[γ]) ({{r_32 - r_23}, {r_13 - r_31}, {r_21 - r_12}})

= 1/(2Sin[γ]) ({{2 (q_3q_4 - q_1q_2) - 2 (q_3q_4 + q_1q_2)}, {2 (q_2q_4 - q_1q_3) - 2 (q_2q_4 + q_1q_3)}, {2 (q_2q_3 - q_1q_4) - 2 (q_2q_3 + q_1q_4)}})

= 1/Sin[γ] ( {{-2 q_1 q_2}, {-2 q_1 q_3}, {-2 q_1 q_4}} )

3

q_1 = Cos[γ/2] = ± (1 + Cos[γ])/2^(1/2)

Sin[γ/2] = ± (1 - Cos[γ])/2^(1/2) = ± (1 - q_1^2)^(1/2)

Sin[γ] = 2Sin[γ/2] Cos[γ/2] = 2q_1 (1 - q_1^2)^(1/2)

4

_B^AOverscript[q,^] = ({{q_1, -r_x (1 - q_1^2)^(1/2), -r_y (1 - q_1^2)^(1/2), -r_z (1 - q_1^2)^(1/2)}})

γ = 2ArcCos[q_1] = ArcCos[2q_1^2 - 1]

5

_B^AOverscript[q,^] = ({{q_1, q_2, q_3, q_4}}) = ({{q_1, -r_x (1 - q_1^2)^(1/2), -r_y (1 - q_1^2)^(1/2), -r_z (1 - q_1^2)^(1/2)}})

γ = ArcCos[2q_1^2 - 1]

Overscript[r,^] = ({{r_x}, {r_y}, {r_z}}) = -1/(1 - q_1^2)^(1/2) ( {{q_2}, {q_3}, {q_4}})

6

γ = ArcCos[(r_11 + r_22 + r_33 - 1)/2] = ArcCos[-2 + 3 q_1^2 + q_2^2 + q_3^2 + q_4^2] = ArcCos[2 q_1^2 - 1]

Rotation Matrix _B^AR → Euler Angle (Roll φ Overscript[x,^] _B, Pitch θ Overscript[y,^] _B, Yaw ψ Overscript[z,^] _B, ZYX)

φ = ArcTan[r_33, r_32]

θ = ArcSin[-r_31]

ψ = ArcTan[r_11, r_21]

θ={-π/2,π/2}

φ (Roll) = ArcTan[r_33, r_32]

θ (Pitch) = ArcSin[-r_31]

ψ (Yaw) = ArcTan[r_11, r_21]

θ={π/2,3π/2}

φ = ArcTan[-r_33, -r_32]

θ = ArcSin[-r_31]

ψ = ArcTan[-r_11, -r_21]

θ (Pitch)=π/2, Cos[θ]≈0

r_23 + r_12 = (Sin[θ] - 1) Sin[φ + ψ]

r_13 - r_22 = (Sin[θ] - 1) Cos[φ + ψ]

r_12 - r_23 = (Sin[θ] + 1) Sin[φ - ψ]

r_13 + r_22 = (Sin[θ] + 1) Cos[φ - ψ]

φ + ψ = ArcTan[r_13 - r_22, r_23 + r_12]

θ = ArcSin[-r_31]

φ - ψ = ArcTan[r_13 + r_22, r_12 - r_23]

Rotation Matrix _B^AR → Axis/Angle

γ = ArcCos[(r_11 + r_22 + r_33 - 1)/2]

Overscript[r,^] = ({{r_x}, {r_y}, {r_z}}) = 1/(2Sin[γ]) ({{r_32 - r_23}, {r_13 - r_31}, {r_21 - r_12}})

r_x^2 + r_y^2 + r_z^2 = 1

Angle γ and Axis r

Sin[α] = r_y/(r_x^2 + r_y^2)^(1/2), Cos[α] = r_x/(r_x^2 + r_y^2)^(1/2)

Sin[β] = (r_x^2 + r_y^2)^(1/2)/(r_x^2 + r_y^2 + r_z^2)^(1/2), Cos[β] = r_z/(r_x^2 + r_y^2 + r_z^2)^(1/2)

Rotation matrix ↔ Euler axis/angles

Angle

γ = ArcCos[(r_11 + r_22 + r_33 - 1)/2]

Euler axis

Overscript[r,^] = ({{r_x}, {r_y}, {r_z}})

r_x = (r_32 - r_23)/(2 Sin[γ])

r_y = (r_13 - r_31)/(2 Sin[γ])

r_z = (r_21 - r_12)/(2 Sin[γ])

Rotation - concisely

R = I . Cos[θ] + Sin[θ] u_× + (1 - Cos[θ]) u⊗u

I = ({{1, 0, 0}, {0, 1, 0}, {0, 0, 1}})

u⊗u = ({{u_x^2, u_xu_y, u_xu_z}, {u_xu_y, u_y^2, u_yu_z}, {u_xu_z, u_yu_z, u_z^2}})

u_× = ({{0, -u_z, u_y}, {u_z, 0, -u_x}, {-u_y, u_x, 0}})

Example

( {{Ax Cos[θ] - Ay Sin[θ]}, {Ay Cos[θ] + Ax Sin[θ]}, {Az}} )

{{Ax*Cos[θ] + Az*Sin[θ]*Subscript[u, y] - (-1 + Cos[θ])*Subscript[u, x]*
    (Ax*Subscript[u, x] + Ay*Subscript[u, y]) -
   (Ay*Sin[θ] + Az*(-1 + Cos[θ])*Subscript[u, x])*Subscript[u, z]},
{Ay*Cos[θ] - Subscript[u, x]*(Az*Sin[θ] + Ax*(-1 + Cos[θ])*Subscript[u, y]) +
   Ax*Sin[θ]*Subscript[u, z] - (-1 + Cos[θ])*Subscript[u, y]*
    (Ay*Subscript[u, y] + Az*Subscript[u, z])},
{Az*Cos[θ] + Ay*Sin[θ]*Subscript[u, x] - Ax*Sin[θ]*Subscript[u, y] -
   (-1 + Cos[θ])*(Ax*Subscript[u, x] + Ay*Subscript[u, y])*Subscript[u, z] -
   Az*(-1 + Cos[θ])*Subscript[u, z]^2}}

R_x[φ_] :=//MatrixForm

R_y[θ_] := ({{Cos[θ], 0, Sin[θ]}, {0, 1, 0}, {-Sin[θ], 0, Cos[θ]}})//MatrixForm

R_z[ψ_] := ({{Cos[ψ], -Sin[ψ], 0}, {Sin[ψ], Cos[ψ], 0}, {0, 0, 1}})//MatrixForm

({{Cos[p], 0, Sin[p]}, {0, 1, 0}, {-Sin[p], 0, Cos[p]}}) . ({{1, 0, 0}, {0, Cos[r], -Sin[r]}, {0, Sin[r], Cos[r]}}) . ({{Vx}, {Vy}, {Vz}})

{{Vx Cos[p] + Vz Cos[r] Sin[p] + Vy Sin[p] Sin[r]}, {Vy Cos[r] - Vz Sin[r]}, {Vz Cos[p] Cos[r] - Vx Sin[p] + Vy Cos[p] Sin[r]}}

({{1, 0, 0}, {0, Cos[r], -Sin[r]}, {0, Sin[r], Cos[r]}}) . ({{Ux}, {Uy}, {Uz}})

{{Ux}, {Uy Cos[r] - Uz Sin[r]}, {Uz Cos[r] + Uy Sin[r]}}

Rotation Matrix _B^AR → Quaternion _B^AOverscript[q,^]

1

_B^AOverscript[q,^] = ({{q_1, q_2, q_3, q_4}})

q_1^2 + q_2^2 + q_3^2 + q_4^2 = 1

2

r_11 + r_22 + r_33 + 1

= (2q_1^2 - 1 + 2q_2^2) + (2q_1^2 - 1 + 2q_3^2) + (2q_1^2 - 1 + 2q_4^2) + 1

= 2 (-1 + 2 q_1^2 + q_1^2 + q_2^2 + q_3^2 + q_4^2) = 2 (-1 + 2 q_1^2 + 1)

= 4 q_1^2

r_11 - r_22 - r_33 + 1

= (2q_1^2 - 1 + 2q_2^2) - (2q_1^2 - 1 + 2q_3^2) - (2q_1^2 - 1 + 2q_4^2) + 1

= -2 (-1 + q_1^2 - q_2^2 + q_3^2 + q_4^2) = -2 (-1 - 2q_2^2 + q_1^2 + q_2^2 + q_3^2 + q_4^2) = -2 (-1 - 2q_2^2 + 1)

= 4q_2^2

-r_11 + r_22 - r_33 + 1

= -(2q_1^2 - 1 + 2q_2^2) + (2q_1^2 - 1 + 2q_3^2) - (2q_1^2 - 1 + 2q_4^2) + 1

= -2 (-1 + q_1^2 + q_2^2 - q_3^2 + q_4^2) = -2 (-1 - 2q_3^2 + q_1^2 + q_2^2 + q_3^2 + q_4^2) = -2 (-1 - 2q_3^2 + 1)

= 4q_3^2

-r_11 - r_22 + r_33 + 1

= -(2q_1^2 - 1 + 2q_2^2) - (2q_1^2 - 1 + 2q_3^2) + (2q_1^2 - 1 + 2q_4^2) + 1

= -2 (-1 + q_1^2 + q_2^2 + q_3^2 - q_4^2) = -2 (-1 - 2a_4^2 + q_1^2 + q_2^2 + q_3^2 + q_4^2) = -2 (-1 - 2a_4^2 + 1)

= 4q_4^2

3

q_1 = 1/2 (r_11 + r_22 + r_33 + 1)^(1/2)

q_2 = -Sign[r_32 - r_23]/2 (r_11 - r_22 - r_33 + 1)^(1/2)

q_3 = -Sign[r_13 - r_31]/2 (r_22 - r_33 - r_11 + 1)^(1/2)

q_4 = -Sign[r_21 - r_12]/2 (r_33 - r_11 - r_22 + 1)^(1/2)

Calibration

_B^AR = R_z[ψ] . R_y[θ] . R_x[φ]

_C^AR = _C^BR . _B^AR ;

({{ra_11, ra_12, ra_13}, {ra_21, ra_22, ra_23}, {ra_31, ra_32, ra_33}}) . ({{rb_11, rb_12, rb_13}, {rb_21, rb_22, rb_23}, {rb_31, rb_32, rb_33}}) //MatrixForm

Created by Mathematica  (June 26, 2015) Valid XHTML 1.1!