

In [5]:

    
%%HTML
<LINK rel=stylesheet type="text/css" href="files/defense.css">



In [6]:

    
from IPython.display import IFrame
import TSatPyViz
from TSatPy import State
%load_ext autoreload
%autoreload 2









    



The autoreload extension is already loaded. To reload it, use:
  %reload_ext autoreload

TableSat IA

Master's Thesis

Development of a Modular Application for Analyzing Observer-Based Control Systems for NASA's Spin-Stabilized MMS Mission Spacecraft

Math + Control Theory + Coding = years and years of fun

Daniel R. Couture

University of New Hampshire Mechanical Engineering

Prof. May-Win Thein

June 5th, 2014

NASA Goddard Space Flight Center

@MathYourLife

github.com/MathYourLife

vimeo.com/search?q=mathyourlife

Thanks

TableSat IA

Physical model to capture and quantify boom dynamics.

Analytical and Experimental ADCS

State Measurement

TableSat IA Experimental Boom Dynamics Blockers

Truster (Fan)
System Mass
Magnetometer Noise

4 Matlab ADCS Versions

Object-oriented classes
Actuator Logic
Run-time visualizations
- PID controller
- EKF
Matlab issues
- More complex coding = More Matlab "monkey patching"
- Simulink doesn't play nice with experimental setups
- Not open source ($, community, resources, ...)
- Debugging is painful

Final Python ADCS Vesion

Feature Set

Concurrent Estimation/Control Algorithms
Run-time interface
Variable System Clock
Adaptive Step Algorithms
Modular Design
Source Agnostic Control System
Open Source - MIT License

Quaternion Additive Correction

Quaternon Propagation:

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

Issues in:

State Error
Luenberger Gains
PID State Estimators
PID Controllers
Sliding Mode Observers
Sliding Mode Controllers
Kalman Filters
Extended Kalman Filters
Integrals / Derivatives
...



In [7]:

    
TSatPyViz.dq_adjust()









    



/usr/lib/python3/dist-packages/gi/overrides/__init__.py:92: Warning: Source ID 2 was not found when attempting to remove it
  return fn(*args, **kwargs)

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

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 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*

Theta Multiplier with Quaternion Vector Balancing

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.

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

Luenberger Observer

$$ \begin{aligned} \boldsymbol{\hat{q}}(t_{k+1}) &= \boldsymbol{\psi}(\boldsymbol{q}_e(t_{k}), K) \otimes f(\boldsymbol{\hat{q}}(t_{k})) \\ \text{where } K &= \text{scalar gain} \end{aligned} $$



In [8]:

    
q = State.Quaternion(vector=[1, 1, 0], radians=1)
TSatPyViz.show_tmqvb(q)









    



/usr/lib/python3/dist-packages/gi/overrides/__init__.py:92: Warning: Source ID 571 was not found when attempting to remove it
  return fn(*args, **kwargs)

PID State Estimator

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

Adaptive Time Step Quaternion Integral

$$ \boldsymbol{q}_{ei}(t_k) = \boldsymbol{\psi}(\boldsymbol{q}_e(t_{k}), \Delta t_{k}) \otimes \boldsymbol{q}_{ei}(t_{k-1}) $$

Adaptive Time Step Quaternion Derivative

$$ \boldsymbol{q}_{ed}(t_k) = \boldsymbol{\psi}\left(\boldsymbol{q}_e(t_{k-1})^* \otimes \boldsymbol{q}_e(t_{k}), \frac{1}{\Delta t_{k}}\right) $$



In [9]:

    
TSatPyViz.show_pid()









    



/usr/lib/python3/dist-packages/gi/overrides/__init__.py:92: Warning: Source ID 1167 was not found when attempting to remove it
  return fn(*args, **kwargs)

Quaternion Nutation Control

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 an Attitude into Rotation and Nutation Quaternions

$$ \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 [10]:

    
from TSatPy.State import Quaternion

yaw = 0.2
pitch = 0.1
print("Creating an attitude with %g rad yaw and %g rad pitch.\n" % (yaw, pitch))

q = Quaternion([0,1,0], radians=pitch) * Quaternion([0,0,1], radians=yaw)
print("Estimator reports an attitude quaternion of:")
print("q:     %s\n" % (q))

q_r, q_n = q.decompose()
print("Controller decomposes the attitude and ignores the rotational component")
print("q_r:   %s" % (q_r))
print("q_n:   %s" % (q_n))









    



Creating an attitude with 0.2 rad yaw and 0.1 rad pitch.

Estimator reports an attitude quaternion of:
q:     <Quaternion [0.00498959 -0.0497295 -0.0997087], 0.993761>

Controller decomposes the attitude and ignores the rotational component
q_r:   <Quaternion [0 0 -0.0998334], 0.995004>
q_n:   <Quaternion [-0 0.0499792 0], -0.99875>

Quaternion Attitude Control (Nutation Control)

Section	Given	Problem
Controller	Attitude error $\boldsymbol{q}_e$	Calculate nutation correction $\boldsymbol{M}$

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

The proportional moment correction term is

$$ \begin{align} \boldsymbol{M} &= \left[- 2k \cdot \cos^{-1} (q_{0e}) \right] \boldsymbol{\hat{e}}_e \\ \text{Where } &= \boldsymbol{\hat{e}} \text{ is the Euler axis } \boldsymbol{v}/| \boldsymbol{v}|\end{align} $$

PID Controller

$$ \boldsymbol{M} = \left[- 2K_{p} \cos^{-1} (q_{0e}) \right] \boldsymbol{\hat{e}}_e + \left[- 2K_{i} \cos^{-1} (q_{0ei}) \right] \boldsymbol{\hat{e}}_{ei} + \left[- 2K_{d} \cos^{-1} (q_{0ed}) \right] \boldsymbol{\hat{e}}_{ed} $$

SMC

$$ \begin{aligned} \boldsymbol{M} &= \left[- 2L \cos^{-1} (q_{0e}) \right] \boldsymbol{\hat{e}}_e + \left[ K sat \left( \frac{-2\cos^{-1} (q_{0e})}{S_q} \right) \right] \boldsymbol{\hat{e}}_e \\ \end{aligned} $$

Future Work

Quantify benefits to $\boldsymbol{\psi}(\boldsymbol{q}, K)$ over traditional matrix control methods
EKF with scalar gain and $\boldsymbol{\psi}(\boldsymbol{q}, K)$
Extend functionality of python system visualization
Apply ADCS to TableSat $N$ and other spin stabilized systems
More research into observer based controllers with $\boldsymbol{\psi}(\boldsymbol{q}, K)$ and quaternion decomposition
Release TSatPy to https://github.com/MathYourLife under MIT open source license
Estimator / Controller scheduling
Improve unit test coverage for TSatPy
Coarse Sun Sensors sensor improvements

Summary

Windows/Matlab boo
Linux/Python yay
TSat IA
- thrusters :(
- CSS :)
- TAM need better accuracy
Matlab plotting library (tPlot)
Robust ADCS in Python
$\boldsymbol{\psi}(\boldsymbol{q}, K)$
Quaternion decomposition $\boldsymbol{q} = \boldsymbol{q}_n \otimes \boldsymbol{q}_r$
$\boldsymbol{\psi}(\boldsymbol{q}, K)$-based PID state estimator, SMO, PID controller, SMC
$\boldsymbol{\psi}(\boldsymbol{q}, K)$-based observer-based controller (SMO+SMC) with $\boldsymbol{q}_n$