System Dynamics Model

Derivation

Given the Quaternion, $q_k$, we can solve for its derivative using the definition of a derivative and other properties as shown below:

$$ \dot{q} = \frac{1}{2} q \otimes \omega = \frac{1}{2} S (\omega_k ) q_k = \frac{1}{2} S(q_k) \omega_k $$

$$$$

$$ \dot{q} = \lim_{\Delta \to t} \frac{q_\mathrm{k+1} - q_k}{\Delta t} $$

$$$$

$$ \dot{q} = \lim_{\Delta \to t} \frac{q_\mathrm{k+1} - q_k}{\Delta t} = \frac{1}{2} S(w_k - b_w)q_k = \frac{1}{2} S(q_k)w_k - \frac{1}{2}S(q_k)b_w $$

$$$$

$$ \dot{q} = q_\mathrm{k+1} - q_k = \frac{\Delta t}{2} S(q_k)w_k - \frac{\Delta t}{2} S(q_k)b_w $$

$$$$

$$ q_\mathrm{k+1} = q_k + \frac{\Delta t}{2} S(q_k)w_k - \frac{\Delta t}{2} S(q_k)b_w $$

Or, re-written as by subtracting 1 from $k$, and shuffling variables:

$$ q_\mathrm{k} = q_\mathrm{k-1} - \frac{\Delta t}{2} S(q_\mathrm{k-1})b_w + \frac{\Delta t}{2} S(q_\mathrm{k-1})w_k $$

The last equation can be re-written in terms of matrices:

$$ x_k^{-} = \begin{bmatrix} I_\mathrm{4x4} & -\frac{T}{2}S(q_\mathrm{k-1}) \\[0.5em] 0_\mathrm{3x4} & I_\mathrm{3x3} \\[0.5em] \end{bmatrix} x_{k-1} + \begin{bmatrix} \frac{T}{2}S(q) \\[0.5em] 0_\mathrm{3x3} \\[0.5em] \end{bmatrix} u_{k-1} $$

$$$$

$$ x^-_k = \begin{bmatrix} q_0 \\[0.5em] q_1 \\[0.5em] q_2 \\[0.5em] q_3 \\[0.5em] b_x \\[0.5em] b_y \\[0.5em] b_z \\[0.5em] \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 & -\frac{T}{2}(-q_1) & -\frac{T}{2}(-q_2) & -\frac{T}{2}(-q_3) \\[0.5em] 0 & 1 & 0 & 0 & -\frac{T}{2}(\phantom{-}q_0) & -\frac{T}{2}(-q_3) & -\frac{T}{2}(\phantom{-}q_2) \\[0.5em] 0 & 0 & 1 & 0 & -\frac{T}{2}(\phantom{-}q_3) & -\frac{T}{2}(\phantom{-}q_0) & -\frac{T}{2}(-q_1) \\[0.5em] 0 & 0 & 0 & 1 & -\frac{T}{2}(-q_2) & -\frac{T}{2}(\phantom{-}q_1) & -\frac{T}{2}(\phantom{-}q_0) \\[0.5em] 0 & 0 & 0 & 0 & 1 & 0 & 0 \\[0.5em] 0 & 0 & 0 & 0 & 0 & 1 & 0 \\[0.5em] 0 & 0 & 0 & 0 & 0 & 0 & 1 \\[0.5em] \end{bmatrix}_{k-1} \begin{bmatrix} q_0 \\[0.5em] q_1 \\[0.5em] q_2 \\[0.5em] q_3 \\[0.5em] b_x \\[0.5em] b_y \\[0.5em] b_z \\[0.5em] \end{bmatrix}_{k-1} + \begin{bmatrix} -q_1 & -q_2 & -q_3 \\[0.5em] \phantom{-}q_0 & -q_3 & \phantom{-}q_2 \\[0.5em] \phantom{-}q_3 & \phantom{-}q_0 & -q_1 \\[0.5em] -q_2 & \phantom{-}q_1 & \phantom{-}q_0 \\[0.5em] 0 & 0 & 0 \\[0.5em] 0 & 0 & 0 \\[0.5em] 0 & 0 & 0 \\[0.5em] \end{bmatrix}_{k-1} \begin{bmatrix} w_x \\[0.5em] w_y \\[0.5em] w_z \\[0.5em] \end{bmatrix}_{k-1} $$