# Design and Implementation of Model Predictive Control for a Gyroscopic Inverted Pendulum
## 2 Dynamic System Modeling
### Nonlinear Dynamic Model  
Figure 3b shows the definitions of the coordinate systems that are required to describe the system's motions. The lower-case letters \(b\), \(g_{i}\), and \(f_{i}\), where \(i=1,\,2\), present the centres of mass of five bodies: the inverted pendulum, two gimbal frames, and two flywheels, respectively. Four Society of Automotive Engineers (SAE)-standard coordinate systems are used in the model: an inertial frame \(\Gamma_{o}(\mathbf{I},\mathbf{J},\mathbf{K})\) that is fixed on the ground, a reference frame \(\Gamma_{b}(\mathbf{i}_{b},\mathbf{j}_{b},\mathbf{k}_{b})\) that is mounted on the inverted pendulum at point O, and the two frames \(\Gamma_{g1}(\mathbf{i}_{g1},\mathbf{j}_{g1},\mathbf{k}_{g1})\) and \(\Gamma_{g2}(\mathbf{i}_{g2},\mathbf{j}_{g2},\mathbf{k}_{g2})\) that are placed on the gimbal's revolute joints \(G_{1}\) and \(G_{2}\) which move with the gimbal frames. The coordinate \(\Gamma_{b}\) rotates about the \(\mathbf{I}\) axis of \(\Gamma_{o}\) through an angle \(\theta\). The transformation between each coordinate system is defined by:  
\[\Gamma_{o}\rightarrow\Gamma_{b}:\,\mathbf{R}_{b}=\left[ \begin{array}{ccc}1& 0&0\\ 0&\cos\theta&\sin\theta\\ 0&-\sin\theta&\cos\theta\end{array} \right]; \tag{4}\]  
and the coordinate \(\Gamma_{g1}\) is obtained by rotating \(\Gamma_{b}\) about its \(\mathbf{j}_{b}\) axis through an angle \(\alpha_{1}\):  
\[\Gamma_{b}\rightarrow\Gamma_{g1}:\,\mathbf{R}_{g1}=\left[ \begin{array}{ccc} \cos\alpha_{1}&0&-\sin\alpha_{1}\\ 0&1&0\\ \sin\alpha_{1}&0&\cos\alpha_{1}\end{array} \right]. \tag{5}\]  
Similarly, the coordinate \(\Gamma_{g2}\) is obtained by rotating through an angle \(\alpha_{2}\):  
\[\Gamma_{b}\rightarrow\Gamma_{g2}:\,\mathbf{R}_{g2}=\left[ \begin{array}{ccc} \cos\alpha_{2}&0&-\sin\alpha_{2}\\ 0&1&0\\ \sin\alpha_{2}&0&\cos\alpha_{2}\end{array} \right]. \tag{6}\]  
The position of the inverted pendulum's centre of mass of \(b\) in the coordinate system \(\Gamma_{b}\) is:  
\[\mathbf{r}_{b}=\left[ \begin{array}{ccc}x_{b}&y_{b}&z_{b}\end{array} \right]^ {T}; \tag{7}\]  
the position of the gimbal joint \(G_{i}\) in \(\Gamma_{b}\) is:  
Figure 3: The gyroscopic inverted pendulum system: (**a**) physical model; (**b**) definitions of the coordinate systems. \[\mathbf{r}_{Gi}=\left[ \begin{array}{cc}x_{Gi}&y_{Gi}&z_{Gi}\end{array} \right]^{T}\! ;\,i=1,2. \tag{8}\]  
The position of the gimbal frame's centre of mass of \(g_{i}\) and \(f_{i}\) in \(\Gamma_{gi}\) is:  
\[\mathbf{r}_{gi}=\left[ \begin{array}{cc}x_{gi}&y_{gi}&z_{gi}\end{array} \right] ^{T}\! ; \tag{9}\]  
\[\mathbf{r}_{fi}=\left[ \begin{array}{cc}x_{fi}&y_{fi}&z_{fi}\end{array} \right] ^{T}\!;\,i=1,2. \tag{10}\]  
The position of these points in \(\Gamma_{o}\) are obtained by:  
\[ \begin{array}{l}\mathbf{r}_{ob}=\mathbf{R}_{p}^{T}\mathbf{r}_{b};\\ \mathbf{r}_{ogi}=\mathbf{R}_{p}^{T}(\mathbf{r}_{Gi}+\mathbf{R}_{gi}^{T} \mathbf{r}_{gi});\\ \mathbf{r}_{ofi}=\mathbf{R}_{b}^{T}(\mathbf{r}_{Gi}+\mathbf{R}_{gi}^{T} \mathbf{r}_{fi});\,i=1,2.\end{array}  \tag{11}\]  
The angular velocity of each body is calculated as:  
\[ \begin{array}{l}\mathbf{\omega}_{b}=\left[\begin{array}{cc}\dot{\theta}& 0&0\end{array}\right]^{T}\!;\\ \mathbf{\omega}_{gi}=\mathbf{R}_{gi}\mathbf{\omega}_{b}+\left[\begin{array}{ cc}0&\dot{\alpha}_{i}&0\end{array}\right]^{T}\!;\\ \mathbf{\omega}_{fi}=\mathbf{\omega}_{gi}+\left[\begin{array}{cc}0&0&\dot{ \phi}_{i}\end{array}\right]^{T}\!;\,i=1,2.\end{array}  \tag{12}\]  
The velocity of each body is calculated as:  
\[ \begin{array}{l}\mathbf{v}_{b}=\mathbf{\omega}_{b}\times\mathbf{r}_{b};\\ \mathbf{v}_{gi}=\mathbf{\omega}_{b}\times\mathbf{r}_{Gi}+\mathbf{R}_{2}^{T}( \mathbf{\omega}_{gi}\times\mathbf{r}_{gi});\\ \mathbf{v}_{fi}=\mathbf{\omega}_{b}\times\mathbf{r}_{Gi}+\mathbf{R}_{2}^{T}( \mathbf{\omega}_{gi}\times\mathbf{r}_{fi});\,i=1,2.\end{array}  \tag{13}\]  
From Equations (12) and (13), the kinetic energy of the system is obtained by:  
\[