I'm trying to develop a control for a quadrotor, but I'm struggling with position and velocity stabilization.
I believe that I have an issue with position and velocity states of the system.
Maybe someone has already done full state system. Unfortunately, I've found only stabilization of quaternion and angular velocity in articles.
I've attached my version of the system.
Is there any articles about rigid body full state integration and linearization in terms of quaternions?. Available from: https://www.researchgate.net/post/Is_there_any_articles_about_rigid_body_full_state_integration_and_linearization_in_terms_of_quaternions [accessed Jul 14, 2017].
Here
$\eta$ - position in Earth coordinate system
$\upsilon$ - velocity in Body frame
$\omega$ - angular velocity
$\lambda$ - quaternion
\begin{cases} \dot{\vec{\eta}}=\vec{\lambda}^{-1}\cdot\upsilon\cdot\vec{\lambda} & ,\\ \dot{\vec{\upsilon}}=\vec{\lambda}\cdot\begin{pmatrix}0\\ 0\\ 0\\ -g \end{pmatrix}\cdot\vec{\lambda}^{-1}+\frac{1}{m}\cdot F & ,\\ \dot{\vec{\omega}}=\begin{pmatrix}\frac{\tau_{1}}{I_{x}}\\ \frac{\tau_{2}}{I_{y}}\\ \frac{\tau_{3}}{I_{z}} \end{pmatrix}-\begin{pmatrix}\frac{I_{z}-I_{y}}{I_{x}}\dot{\theta}\dot{\psi}\\ \frac{I_{x}-I_{z}}{I_{y}}\dot{\phi}\dot{\psi}\\ \frac{I_{y}-I_{x}}{I_{z}}\dot{\phi}\dot{\theta} \end{pmatrix} & ,\\ \dot{\vec{\lambda}}=\frac{1}{2}\vec{\lambda}\circ\vec{\omega} & . \end{cases}
or detailed
\begin{cases} \dot{\eta}_{n}=\frac{1}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}((\lambda_{0}^{2}+\lambda_{1}^{2}-\lambda_{2}^{2}-\lambda_{3}^{2})v_{x}+(2\lambda_{0}\lambda_{3}+2\lambda_{1}\lambda_{2})v_{y}+(-2\lambda_{0}\lambda_{2}+2\lambda_{1}\lambda_{3})v_{z}) & ,\\ \dot{\eta}_{e}=\frac{1}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}((-2\lambda_{0}\lambda_{3}+2\lambda_{1}\lambda_{2})v_{x}+(\lambda_{0}^{2}-\lambda_{1}^{2}+\lambda_{2}^{2}-\lambda_{3}^{2})v_{y}+(2\lambda_{0}\lambda_{1}+2\lambda_{3}\lambda_{2})v_{z}) & ,\\ \dot{\eta}_{h}=\frac{1}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}((2\lambda_{0}\lambda_{2}+2\lambda_{1}\lambda_{3})v_{x}+(-2\lambda_{0}\lambda_{1}+2\lambda_{3}\lambda_{2})v_{y}+(\lambda_{0}^{2}-\lambda_{1}^{2}-\lambda_{2}^{2}+\lambda_{3}^{2})v_{z}) & ,\\ \dot{v}_{x}=\frac{2g}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}(-\lambda_{1}\lambda_{3}-\lambda_{0}\lambda_{2}) & ,\\ \dot{v}_{y}=\frac{2g}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}(\lambda_{0}\lambda_{1}-\lambda_{2}\lambda_{3}) & ,\\ \dot{v}_{z}=\frac{-g}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}(\lambda_{0}^{2}-\lambda_{1}^{2}-\lambda_{2}^{2}+\lambda_{3}^{2})+\frac{1}{m}F & ,\\ \ddot{\phi}=\frac{1}{I_{x}}\left[\tau_{1}-(I_{z}-I_{y})\dot{\theta}\dot{\psi}\right] & ,\\ \ddot{\theta}=\frac{1}{I_{y}}\left[\tau_{2}-(I_{x}-I_{z})\dot{\phi}\dot{\psi}\right] & ,\\ \ddot{\psi}=\frac{1}{I_{z}}\left[\tau_{3}-(I_{y}-I_{x})\dot{\phi}\dot{\theta}\right] & ,\\ \dot{\lambda_{0}}=\frac{1}{2}[-\lambda_{1}\dot{\phi}-\lambda_{2}\dot{\theta}-\lambda_{3}\dot{\psi}] & ,\\ \dot{\lambda_{1}}=\frac{1}{2}[\lambda_{0}\dot{\phi}-\lambda_{3}\dot{\theta}+\lambda_{2}\dot{\psi}] & ,\\ \dot{\lambda_{2}}=\frac{1}{2}[\lambda_{3}\dot{\phi}+\lambda_{0}\dot{\theta}-\lambda_{1}\dot{\psi}] & ,\\ \dot{\lambda_{3}}=\frac{1}{2}[-\lambda_{2}\dot{\phi}+\lambda_{1}\dot{\theta}+\lambda_{0}\dot{\psi}] & . \end{cases}