

In [1]:

    
import time
import sys

Lessons Learned

MATLAB - advanced programming features, but still not a full programming language
Python - Easy and powerful
Quaternions over Euler Angles
- If your process normalizes a quaternion, it's doing it wrong >.<

The Cool Bits

Quaternion 101

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]$

Rotational Quaternion Definition

$$ \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} $$

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 doesn't work with quaternions?

Luenberger Gains
PID State Estimators
PID Controllers
Sliding Mode Observers
Sliding Mode Controllers
Kalman Filters
Extended Kalman Filters
Any Linear Operation

$$ \begin{align} \boldsymbol{\hat{x}(k+1)} &= f(\boldsymbol{\hat{x}}(k)) + \boldsymbol{K}(\boldsymbol{\hat{x}}(k) - \boldsymbol{x}(k)) \\ \\ \\ \boldsymbol{M}(k) &= \boldsymbol{G}(\boldsymbol{\hat{x}}(k) - \boldsymbol{x}_d) \end{align} $$

Valid Quaternion Operations

Valid Quaternion Operation $\iff$ Never need to re-normalize

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 Propagation

Nikolas Trawny and Stergios I. Roumeliotis

Indirect Kalman Filter for 3D Attitude Estimation A Tutorial for Quaternion Algebra

Taylor Series Expansion:

$$ \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}) \right] - \boldsymbol{\Omega} \left[\boldsymbol{\omega}(t_{k}) \right] \boldsymbol{\Omega} \left[\boldsymbol{\omega}(t_{k+1}) \right] \Big) \end{aligned} $$

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

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 [2]:

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

# 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])

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)

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

State Estimators

PID

$$ \begin{align} \boldsymbol{\hat{q}}(t_{k+1}) &= f(\boldsymbol{\hat{q}}(t_{k})) + K_{qp} \boldsymbol{q}_{e}(t_k) + K_{qi} \sum^k_0 \boldsymbol{q}_{e}(t_k) + K_{qd}( \boldsymbol{q}_{e}(t_{k-1}) - \boldsymbol{q}_{e}(t_k))\\ \boldsymbol{\hat{q}}(t_{k+1}) &= \boldsymbol{\psi}\left(\boldsymbol{q}_{ed}(t_k), K_{qd}\right) \otimes \boldsymbol{\psi}\big(\boldsymbol{q}_{ei}(t_k), K_{qi}\big) \otimes \boldsymbol{\psi}(\boldsymbol{q}_e(t_{k}), K_{qp}) \otimes f(\boldsymbol{\hat{q}}(t_{k})) \end{align} $$

Sliding Mode Observer

$$ \begin{align} \boldsymbol{\hat{q}}(t_{k+1}) &= f(\boldsymbol{q}(t_k)) + \boldsymbol{L} \boldsymbol{q}_{e}(t_k) + \boldsymbol{K}\boldsymbol{1}_s \big(\boldsymbol{q}_{e}(t_k) \big) \\ \boldsymbol{\hat{q}}(t_{k+1}) &= \boldsymbol{\psi} (\boldsymbol{1}_s\big(\boldsymbol{q}_{e}(t_k)\big), K) \otimes \boldsymbol{\psi}(\boldsymbol{q}_e(t_{k}), L) \otimes f(\boldsymbol{q}(t_k)) \\ \end{align} $$

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} \\ q_1 \boldsymbol{i} + q_2 \boldsymbol{j} + q_3 \boldsymbol{k} + q_0 &= (q_{1n} \boldsymbol{i} + q_{2n} \boldsymbol{j} + q_{3n} \boldsymbol{k} + q_{0n}) \otimes (q_{1r} \boldsymbol{i} + q_{2r} \boldsymbol{j} + q_{3r} \boldsymbol{k} + q_{0r}) \\ \\ \text{where } & \\ \\ \boldsymbol{q}_r &= \begin{bmatrix} 0 \\ 0 \\ q_{3r} = \frac{q_3}{q_{0n}} \\ q_{0r} = \frac{q_0}{q_{0n}} \end{bmatrix}, \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} \\ Q &= \frac{q_{0}q_{1} - q_{2}q_{3}}{q_{0}q_{2} + q_{1}q_{3}} \end{aligned} $$



In [3]:

    
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>

Nutation Control

Section	Given	Problem
Controller	Nutation $\boldsymbol{q}_n$	Calculate nutation correction $\boldsymbol{M}$

The proportional moment correction term is

$$ \boldsymbol{M}_{q} = \left[- 2k \cdot \cos^{-1} (q_{0n}) \right] \boldsymbol{\hat{e}}_n $$

Where $\boldsymbol{\hat{e}}$ is the Euler axis

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



In [ ]: