| Symbol | Meaning |
|---|---|
| state vector | |
| system input | |
| vehicle position in NED | |
| vehicle attitude quaternion | |
| vehicle velocity in world frame | |
| vehicle angular velocity FRD | |
| actuator force | |
| actuator torque | |
| vehicle mass | |
| moment of inertia | |
| state-error | |
| ${\pmb{q}{err}}{3 \times 1}$ | attitude error in quaternion vector representation (the vector part of $\pmb{q}{now} \pmb{q}{ref}^{-1}$) |
| running stage weighting matrix for |
|
| terminal stage weighting matrix for |
|
| running stage weighting matrix for |
$$ \pmb{x}=\begin{bmatrix} \pmb{p}{3\times1}\ \pmb{q}{4\times1}\ W\pmb{v}{3\times1}\ B\pmb{\omega}{3\times1}\ \end{bmatrix}_{13\times1}
\pmb{u}=\begin{bmatrix} _B\dot{\pmb{f}a}{3\times1}\ B\dot{\pmb{\tau}a}{3\times1} \end{bmatrix}{6\times1}
\pmb{e}=\begin{bmatrix}
\pmb{p}{err}\
\pmb{q}{err}\
W\pmb{v}{err}\
B\pmb{\omega}{err}\
\end{bmatrix}
\
\
\dot{\pmb{x}}=\begin{bmatrix}
_W\pmb{v}\
\frac{1}{2} \pmb{q} \begin{bmatrix}0 \ _B\pmb{\omega} \end{bmatrix}\
\frac{rotate(_B{\pmb{f}_a},\pmb{q})}{m}\ +\ \pmb{g}\
\pmb{J}^{-1}(_B{\pmb{\tau}_a}\ -\ _B\pmb{\omega} \times (\pmb{J}_B\pmb{\omega}))\
\end{bmatrix}
$$
$$ \min_U\ \Sigma_{k=0}^{N-1}(\norm{\pmb{e}k}^2{\pmb{Q}}\ +\ \norm{\pmb{u}k}^2{\pmb{R}})\ +\ \norm{\pmb{e}N}^2{\pmb{Q}_N} \ [0, 0, -40, -20, -20, -20]^T \le \pmb{u}_k \le [0, 0, 0, 20, 20, 20]^T\ [-10, -10, -10, -60, -60, -60] \le [_W\pmb{v},_B\pmb{\omega}]_k \le [10, 10, 10, 60, 60, 60] $$