Desenvolvimento de um Veículo Aéreo Não-Tripulado - LARA ...

∂q b n
∂ω b ib
=
⎡
⎤
α x q 0 + β x Σ q0 − γq 1 α y q 0 + β y Σ q0 − γq 2 α z q 0 + β z Σ q0 − γq 3
α x q 1 + β x Σ q1 + γq 0 α y q 1 + β y Σ q1 − γq 3 α z q 1 + β z Σ q1 + γq 2
⎢
⎣α x q 2 + β x Σ q2 + γq 3 α y q 2 + β y Σ q2 + γq 0 α z q 2 + β z Σ q2 − γq 1
⎥
⎦
α x q 3 + β x Σ q3 − γq 2 α y q 3 + β y Σ q3 + γq 1 α z q 3 + β z Σ q3 + γq 0
(5.67a)
∂v n
∂q b n
= 2Φ∆T
∂v n
∂b f
= −Θ∆T
∂v n
= Θ∆T
∂f b
(5.67b)
∂r n
∂q b n
∂e pseudo
∂q b n
= 2Φ ∆T 2
2
[
= − q 0
||qn||
b
∂r n
= −Θ ∆T 2
∂b f 2
− q 1
||q b n||
− q 2
||q b n||
∂r n
2
∂f b = Θ∆T 2
]
− q 3
||qn||
b
(5.67c)
(5.67d)
α i = − sen(d/2) δ i ∆T
2d
(5.68a)
β i = [− d 2cos(d/2) + sen(d/2)]
d 3 δ i ∆T (5.68b)
γ = sen(d/2) ∆T
d
(5.68c)
Σ q0 = ( δ x q 1 + δ y q 2 + δ z q 3 ) (5.68d)
Σ q1 = (−δ x q 0 + δ y q 3 − δ z q 2 )
Σ q2 = (−δ x q 3 − δ y q 0 + δ z q 1 )
(5.68e)
(5.68f)
Σ q3 = ( δ x q 2 − δ y q 1 − δ z q 0 ) (5.68g)
(5.68h)
⎡
⎤
(q 0 f x − q 3 f y + q 2 f z ) (q 1 f x + q 2 f y + q 3 f z ) (−q 2 f x + q 1 f y + q 0 f z ) (−q 3 f x − q 0 f y + q 1 f z )
Φ = ⎢
⎣(q 3 f x + q 0 f y − q 1 f z ) (q 2 f x − q 1 f y − q 0 f z ) ( q 1 f x + q 2 f y + q 3 f z ) ( q 0 f x − q 3 f y + q 2 f z ) ⎥
⎦
(q 2 f x − q 1 f y + q 0 f z ) (q 3 f x + q 0 f y − q 1 f z ) (−q 0 f x + q 3 f y − q 2 f z ) ( q 1 f x + q 2 f y + q 3 f z )
⎡
(q0 2 + q2 1 − q2 2 − q2 3 ) (2q ⎤
1q 2 − 2q 0 q 3 ) (2q 1 q 3 + 2q 0 q 2 )
Θ = ⎢
⎣ (2q 1 q 2 + 2q 0 q 3 ) (q0 2 − q2 1 + q2 2 − q2 3 ) (2q 2q 3 − 2q 0 q 1 ) ⎥
⎦
(2q 1 q 3 − 2q 0 q 2 ) (2q 2 q 3 + 2q 0 q 1 ) (q0 2 − q2 1 − q2 2 + q2 3 )
(5.68i)
(5.68j)
110