

In [1]:

    
import time
import sys

The Math

Body Rate Propagation

Euler's Moment Equations

$$ \begin{align} \dot{\omega}_{x}(t_{k}) & = \frac{1}{I_x} \left[ M_1(t_{k}) - (I_z - I_y) \omega_{y}(t_k) \omega_{z}(t_k) \right] \\ \dot{\omega}_{y}(t_{k}) & = \frac{1}{I_y} \left[ M_2(t_{k}) - (I_x - I_z) \omega_{x}(t_k) \omega_{z}(t_k) \right] \\ \dot{\omega}_{z}(t_{k}) & = \frac{1}{I_z} \left[ M_3(t_{k}) - (I_y - I_x) \omega_{x}(t_k) \omega_{y}(t_k) \right] \end{align} $$

Big Assumptions

Rigid Body
Measurement Noise
Frictionless
Parameter Uncertainty
Center of Rotation = Center of Mass
Actual Moment = Desired Moment

Angular Position Propagation

Quaternon Propagation

$$ \begin{align} \boldsymbol{\dot{q}} &= \frac{1}{2} \boldsymbol{q} \otimes \boldsymbol{\omega}\\ \boldsymbol{q}(k+1) &= \boldsymbol{q}(k) + \boldsymbol{\dot{q}}(k) \Delta t \end{align} $$

What is a Quaternion?

$$ \boldsymbol{q} = \begin{bmatrix} q_1 \\ q_2 \\ q_3 \\ q_0 \end{bmatrix} = \begin{bmatrix} \hat{\boldsymbol{e}} \sin \left( \frac{-\theta}{2} \right) \\ \cos \left( \frac{-\theta}{2} \right) \end{bmatrix} $$

Starting Position

$$x = X, y = Y, \text{ and } z = Z$$

Final Position

$-161$ degree rotation about $\boldsymbol{\hat{e}} = [-0.38 \ -0.07 \ 0.92]$

Valid Quaternion Operations

Operation	Does
$$ \boldsymbol{q} = \boldsymbol{a} \otimes \boldsymbol{b} = \boldsymbol{a}_v b_0 + \boldsymbol{b}_v a_0 + \boldsymbol{a}_v \times \boldsymbol{b}_v + a_0 b_0 - \boldsymbol{a}_v \cdot \boldsymbol{b}_v $$	"Add" rotation $\boldsymbol{a}$ onto rotation $\boldsymbol{b}$
$$\boldsymbol{q}_e = \boldsymbol{q}^* \otimes \boldsymbol{\hat{q}}$$	Gives the rotation that will move $\boldsymbol{\hat{q}}$ to $\boldsymbol{q}$

Quaternion Problems

Section	Given	Problem
Propagation	Position $\boldsymbol{q}(k)$ and velocity $\boldsymbol{\omega}(k)$	Predict $\boldsymbol{q}(k+1)$
Estimator/Controller	Where we are $\boldsymbol{\hat{q}}$ and where we should be $\boldsymbol{q}, \boldsymbol{q}_d$	Calculate $k(\boldsymbol{q}^* \otimes \boldsymbol{\hat{q}})$ where $0 < k < 1$
Controller	Estimated attitude $\boldsymbol{\hat{q}}$	Quantify nutation $\boldsymbol{q}_n$ where $\boldsymbol{\hat{q}} = \boldsymbol{q}_n \otimes \boldsymbol{q}_r$
	Nutation $\boldsymbol{q}_n$	Calculate nutation correction $\boldsymbol{M}$

Quaternion Problems

Section	Given	Problem
Propagation	Position $\boldsymbol{q}(k)$ and velocity $\boldsymbol{\omega}(k)$	Predict $\boldsymbol{q}(k+1)$

$$ \begin{aligned} \boldsymbol{q}(t_{k+1}) &= ( \boldsymbol{A} + \boldsymbol{B} ) \boldsymbol{q}(t_{k}) \\ \text{where } \boldsymbol{A} &= \exp \left( \frac{\Delta t_{k+1}}{2} \boldsymbol{\Omega} \left[ \boldsymbol{\bar{\omega}}(t_{k+1}) \right] \right)\\ \boldsymbol{B} &= \frac{1}{48} \Delta t_{k+1}^2 \Big( \boldsymbol{\Omega} \left[\boldsymbol{\omega}(t_{k+1}) \right] \boldsymbol{\Omega} \left[\boldsymbol{\omega}(t_{k})

Nikolas Trawny and Stergios I. Roumeliotis - Indirect Kalman Filter for 3D Attitude Estimation A Tutorial for Quaternion Algebra*

\right] - \boldsymbol{\Omega} \left[\boldsymbol{\omega}(t_{k}) \right] \boldsymbol{\Omega} \left[\boldsymbol{\omega}(t_{k+1}) \right] \Big) \end{aligned} $$

Quaternion Problems

Section	Given	Problem
Estimator/Controller	Where we are $\boldsymbol{\hat{q}}$ and where we should be $\boldsymbol{q}, \boldsymbol{q}_d$	Calculate $k(\boldsymbol{q}^* \otimes \boldsymbol{\hat{q}})$ where $0 < k < 1$

Requirements for High Integrity

Adhere to the unit rotational quaternion restriction. (i.e. NEVER re-normalize)

Do not modify the direction of the $\boldsymbol{\hat{e}}$ axis

Scale $\theta$ (not $q_0$) so a $4^o$ error can be intuitively scaled to down to a $1^o$ adjustment with a selected gain value of 0.25.

</ul>

Theta Multiplier with Quaternion Vector Balancing (c)?

$$ \begin{aligned} \boldsymbol{\psi}(\boldsymbol{q}, k) &= \begin{bmatrix} \boldsymbol{v} / \gamma \\ \cos ( k \cdot \cos^{-1} (q_0)) \end{bmatrix} \\ \text{where } \gamma &= \sqrt{\frac{\boldsymbol{v} \bullet \boldsymbol{v}}{\sin^2 ( k \cdot \cos^{-1} (q_0))}} \end{aligned} $$

Prove It!



In [84]:

    
from TSatPy.State import Quaternion
from TSatPy.StateOperator import QuaternionGain

def test(q_e, gain):
    e_new, r_new = (QuaternionGain(gain) * q_e).to_rotation()
    print_args = (gain*100, r_new, e_new[0,0], e_new[1,0], e_new[2,0])
    sys.stdout.write("%d%% a %s radian rotation about [%g, %g, %g]^T\n" % print_args)
    sys.stdout.flush()
    time.sleep(0.2)

# Pick any random rotational error here
q_e = Quaternion([1,-2,4], radians=2.3)
e, r = q_e.to_rotation()

print "Error quaternion is a %s radian rotation about [%g, %g, %g]^T\n" % (r, e[0,0], e[1,0], e[2,0])

N = 10
null = [test(q_e, gain/float(N)) for gain in xrange(1, N+1)]









    



Error quaternion is a 2.3 radian rotation about [-0.218218, 0.436436, -0.872872]^T

10% a 0.23 radian rotation about [-0.218218, 0.436436, -0.872872]^T
20% a 0.46 radian rotation about [-0.218218, 0.436436, -0.872872]^T
30% a 0.69 radian rotation about [-0.218218, 0.436436, -0.872872]^T
40% a 0.92 radian rotation about [-0.218218, 0.436436, -0.872872]^T
50% a 1.15 radian rotation about [-0.218218, 0.436436, -0.872872]^T
60% a 1.38 radian rotation about [-0.218218, 0.436436, -0.872872]^T
70% a 1.61 radian rotation about [-0.218218, 0.436436, -0.872872]^T
80% a 1.84 radian rotation about [-0.218218, 0.436436, -0.872872]^T
90% a 2.07 radian rotation about [-0.218218, 0.436436, -0.872872]^T
100% a 2.3 radian rotation about [-0.218218, 0.436436, -0.872872]^T

Quaternion Problems

Section	Given	Problem
Controller	Estimated attitude $\boldsymbol{\hat{q}}$	Quantify nutation $\boldsymbol{q}_n$ where $\boldsymbol{\hat{q}} = \boldsymbol{q}_n \otimes \boldsymbol{q}_r$

Quaternion $\boldsymbol{\hat{q}}$ defines the attitude as:

Controller should only care about nutation $\boldsymbol{q}_n$:

Decompose a Quaternion

$$ \begin{aligned} \boldsymbol{q} &= \boldsymbol{q_n} \otimes \boldsymbol{q_r} \\ \text{where } \boldsymbol{q}_n &= \begin{bmatrix} q_{1n} = Q \cdot q_{2n} \\ q_{2n} = \sqrt{ \frac{1 - q_3^2 - q_0^2}{Q^2 + 1} } \\ 0 \\ q_{0n} = \sqrt{q_3^2 + q_0^2} \end{bmatrix} , \boldsymbol{q}_r = \begin{bmatrix} 0 \\ 0 \\ q_{3r} = \frac{q_3}{q_{0n}} \\ q_{0r} = \frac{q_0}{q_{0n}} \end{bmatrix} \\ Q &= \frac{q_{0}q_{1} - q_{2}q_{3}}{q_{0}q_{2} + q_{1}q_{3}} \end{aligned} $$

Prove It!



In [96]:

    
from TSatPy.State import Quaternion

yaw = Quaternion([0,0,1], radians=0.2)
pitch = Quaternion([0,1,0], radians=0.1)
print("yaw:   %s" % (yaw))
print("pitch: %s\n" % (pitch))

q = pitch * yaw
print("q:     %s\n" % (q))


q_r, q_n = q.decompose()
print("q_r:   %s" % (q_r))
print("q_n:   %s" % (q_n))









    



yaw:   <Quaternion [-0 -0 -0.0998334], 0.995004>
pitch: <Quaternion [-0 -0.0499792 -0], 0.99875>

q:     <Quaternion [0.00498959 -0.0497295 -0.0997087], 0.993761>

q_r:   <Quaternion [0 0 -0.0998334], 0.995004>
q_n:   <Quaternion [-0 0.0499792 0], -0.99875>



In [ ]: