# LMIs in Control/Click here to continue/Applications of Linear systems/LMI-based State Feedback Design for Quadcopter Optimal path control and Tracking
## Introduction

An LMI-based state feedback approach that ensures optimum path tracking and improved steady state performance of a quadrotor in both translational and rotational movements.

## Quadcopter Dynamics

The motion of Quad Copter in 6DOF is controlled by varying the rpm of four rotors individually, thereby changing the vertical, horizontal and rotational forces.
* ASSUMPTIONS:
* The structure is symmetric, thus the inertia matrices are diagonal.
* The center of mass corresponds to the origin of the physical coordinate system.
* A quadcopter is a rigid body.

## State Space Representation

    ${\begin{aligned}{\dot {x}}(t)&=Ax(t)+Bu(t)\\y(t)&=Cx(t)+Du(t)\\\end{aligned}}$
where x(t) is state vector, y(t) is output vector and u(t) is Input or control vector.
* A is the system matrix
* B is the input matrix
* C is the output matrix
* D is the feed forward matrix
${\dot {x}}(t)={\frac {d}{dt}}x(t)$

## Quadcopter modelling with 6 degree of freedom

* REQUIRED 12 STATES:
| parameter | description |
| x | position along x axis |
| y | position along y axis |
| z | position along z axis(height) |
| x' | velocity along x axis |
| y' | velocity along y axis |
| z' | velocity along z axis |
| Φ | Roll angle |
| θ | pitch angle |
| ψ | yaw angle |
| Φ' | Roll rate |
| θ' | pitch rate |
| ψ' | yaw rate |
The state vector x is $x^{T}=$ ${\begin{bmatrix}x&y&z&x'&y'&z'&\phi &\theta &\psi &\phi '&\theta '&\psi '\end{bmatrix}}$
The Input matrix u is, $u^{T}=$${\begin{bmatrix}U1&U2&U3&U4\end{bmatrix}}$, where
* U1 is the Total Upward Force on the quadrotor along z-axis ( T-mg)
* U2 is the Pitch Torque (about x-axis)
* U3 is the Roll Torque (about y-axis)
* U4 is the Yaw Torque (about z-axis)
The Output matrix y is $y^{T}=$${\begin{bmatrix}x&y&z&\phi &\theta &\psi \end{bmatrix}}$
The State differential equations written in matrix form as,
${\begin{bmatrix}x'\\y'\\z'\\x''\\y''\\z''\\\phi '\\\theta '\\\psi '\\\phi ''\\\theta ''\\\psi ''\\\end{bmatrix}}$ = ${\begin{bmatrix}0&0&0&1&0&0&0&0&0&0&0\\0&0&0&0&1&0&0&0&0&0&0\\0&0&0&0&0&1&0&0&0&0&0\\0&0&0&0&0&0&0&-g&0&0&0\\0&0&0&0&0&0&g&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&1&0&0\\0&0&0&0&0&0&0&0&0&1&0\\0&0&0&0&0&0&0&0&0&0&1\\0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0\\\end{bmatrix}}$ ${\begin{bmatrix}x\\y\\z\\x'\\y'\\z'\\\phi \\\theta \\\psi \\\phi '\\\theta '\\\psi '\\\end{bmatrix}}$ + ${\begin{bmatrix}0&0&0&0\\0&0&0&0\\0&0&0&0\\0&0&0&0\\{\frac {1}{m}}&0&0&0\\0&0&0&0\\0&0&0&0\\0&0&0&0\\0&0&0&0\\0&{\frac {1}{l_{x}}}&0&0\\0&0&{\frac {1}{l_{y}}}&0\\0&0&0&{\frac {1}{l_{z}}}\\\end{bmatrix}}$${\begin{bmatrix}U1\\U2\\U3\\U4\end{bmatrix}}$
The above martices represents the equation $x'=Ax+Bu$
${\begin{bmatrix}x\\y\\z\\\phi \\\theta \\\psi \\\end{bmatrix}}$=${\begin{bmatrix}1&0&0&0&0&0&0&0&0&0&0\\0&1&0&0&0&0&0&0&0&0&0\\0&0&1&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&1&0&0&0&0\\0&0&0&0&0&0&0&1&0&0&0\\0&0&0&0&0&0&0&0&1&0&0\\\end{bmatrix}}$${\begin{bmatrix}x\\y\\z\\x'\\y'\\z'\\\phi \\\theta \\\psi \\\phi '\\\theta '\\\psi '\\\end{bmatrix}}$+${\begin{bmatrix}0&0&0&0\\0&0&0&0\\0&0&0&0\\0&0&0&0\\\end{bmatrix}}$${\begin{bmatrix}U1\\U2\\U3\\U4\end{bmatrix}}$
The above martices represents the equation $y=Cx+Du$

## LMI for stability Analysis

${\begin{bmatrix}PA^{T}-W^{T}B^{T}+AP-BW+\mu ^{2}\delta &-PA^{T}+W^{T}B^{T}-DP&I\\PD^{T}-AP+BW&D^{T}P+PD&-P\\I&-P&-\delta I\\\end{bmatrix}}<0$
* $K=WP^{-}1$
Solving the above LMI yields the unknown coefficients of the feedback control. The system will be then asymptotically stable and path track will be achieved.

## LPV Attitude State Feedback Controller Design

To design a $H\infty$ LPV feedback control scheme for the altitude/attitude stabilization of the quadrotor aircraft,
* Output $y={\begin{bmatrix}\phi &\theta &\psi &\mathbb {Z} \end{bmatrix}}^{T}$ of the system must track r =${\begin{bmatrix}\phi _{ref}&\theta _{ref}&\psi _{ref}&\mathbb {Z} _{ref}\end{bmatrix}}^{T}$, a reference trajectory.
* To achieve the objective outputs of the integrator are considered as extra state variables $x_{e}={\begin{bmatrix}x_{\phi }&x_{\theta }&x_{\psi }&x_{Z}\end{bmatrix}}^{T}$ as
$x_{\phi }=\int _{0}^{t}e_{\phi }(\delta )d\delta \,dx,e_{\phi }=\phi _{ref}-\phi$
$x_{\theta }=\int _{0}^{t}e_{\theta }(\delta )d\delta \,dx,e_{\theta }=\theta _{ref}-\theta$
$x_{\psi }=\int _{0}^{t}e_{\psi }(\delta )d\delta \,dx,e_{\psi }=\psi _{ref}-\psi$
$x_{z}=\int _{0}^{t}e_{z}(\delta )d\delta \,dx,e_{z}=z_{ref}-z$
In this case the error signal e=y-r ,for the outputs $U_{1},U_{2},U_{3},U_{4}$,the weight functions $W_{ui},i=1,2,3,4$are added to the system. The system matrices of weight functions are $A_{ui},B_{ui},C_{ui}andD_{ui}.$
The dynamic of all the weight functions $W_{u1},W_{u2},W_{u3},W_{u4}$ can be constituted as,
    ${\begin{aligned}{\dot {x}}_{u}=A_{u}x_{u}+B_{u}u\\y_{u}=C_{u}x_{u}+D_{u}u\end{aligned}}$
where $x_{u}={\begin{bmatrix}x_{u1}&x_{u2}&x_{u3}&x_{u4}\end{bmatrix}}^{T}$ is the state,$u={\begin{bmatrix}U_{1}&U_{2}&U_{3}&U_{4}\end{bmatrix}}^{T}$ represents the input $y_{u}={\begin{bmatrix}Z_{1}&Z_{2}&Z_{3}&Z_{4}\end{bmatrix}}^{T}$
* The system matrices of the weight function can be deducted as,
$\Delta _{u}={\begin{bmatrix}\Delta _{u1}&0&0&0\\0&\Delta _{u2}&0&0\\0&0&\Delta _{u3}\\0&0&0&\Delta _{u4}\end{bmatrix}}$,$\Delta \in {A,B,C,D}$
$w={\begin{bmatrix}r&d\end{bmatrix}}^{T}$,$z={\begin{bmatrix}y_{u}&e\end{bmatrix}}^{T}$,and ${\bar {x}}={\begin{bmatrix}x&x_{e}&x_{u}\end{bmatrix}}^{T}$ are the exogenous input and exogenous output respectively.
* The system differential equations with augumented ststes and weight functions are,
    ${\begin{aligned}{\dot {\bar {x}}}=\sum _{i=1}^{16}\mu _{i}({\bar {A}}_{i}{\bar {x}}+{\bar {B}}1_{i}w+{\bar {B}}_{2}u)\\z=C_{\bar {x}}+D11w+D12u\end{aligned}}$
where,
${\bar {A}}_{i}={\begin{bmatrix}{\bar {A}}_{i}&0&0&0\\-C&0&0\\0&0&A_{u}\end{bmatrix}}$;
${\bar {B}}_{1}={\begin{bmatrix}0&{\bar {E}}_{i}\\-I_{4}&0\\0&0\end{bmatrix}}$
${\bar {B}}_{2}={\begin{bmatrix}{\bar {B}}_{i}\\0\\B_{u}\end{bmatrix}}$
$C_{1}={\begin{bmatrix}0&0&C_{u}\\C&0&0\end{bmatrix}}$
$D_{11}={\begin{bmatrix}0&0\\-I_{4}&0\end{bmatrix}}$
$D_{12}={\begin{bmatrix}D_{u}\\0\end{bmatrix}}$
Making the closed loop system,
$dot{\bar {x}}=\sum _{i=1}^{16}\mu _{i}(({\bar {A}}_{i}+{\bar {B}}_{2}ki){\bar {x}}(t)+{\bar {B}}_{1}w)$

## LMI for H∞ optimal state-feedback

$X=X^{T}>0$
${\begin{bmatrix}He({\bar {A}}_{i}X+{\bar {B}}_{2}Y_{i}&(*)^{T}&(*)^{T}\\{\bar {B}}_{1i}^{T}&-\gamma I&(*)^{T}\\{\bar {C}}_{1i}X+D_{12}Y_{i}&{\bar {D}}_{11}&-\gamma I\end{bmatrix}}<0$
$He({\bar {A}}_{i}X+{\bar {B}}_{2}Y_{i}+2\alpha X<0$ ; i=1 to 16
By solving the LMI,the optimal H\inf state feed-back controller with the smallest attenuation level \gamma >0 for the attitude/altitude subsystem of the mass-varying quadcopter is $K(\rho )=\sum _{i=1}^{16}\mu _{i}Y_{i}X^{-}1$

## Conclusion

This LMIs can be used to analyze the state feedback control and path tracking of a quadcopter.

## Implementation

This LMI can be used in a problem and can be solved using the solvers like Yalmip,sedumi,gurobi etc,.