User:Margav06/sandbox/Click here to continue/Applications of Linear systems/LMI for H2/Hinf Polytopic Controller for Robot Arm on a Quadrotor

The System:


\begin{align} \dot x(t)&=A x(t)+B_1 w(t)+B_2 u(t) \\ z(t)&=C_1 x(t)+D_{11} w(t)+D_{12} u(t)\\ y(t)&=C_2 x(t)+D_{21} w(t)+D_{22} u(t)\\ \dot x_K(t)&=A_K x_K(t)+B_K y(t)\\ u(t)&=C_K x_K (t)+D_K y(t)\\ \end{align} $$

The Optimization Problem:
Given a state space system of

\begin{align} \dot x(t)&=A x(t)+B_1 w(t)+B_2 u(t) \\ z(t)&=C_1 x(t)+D_{11} w(t)+D_{12} u(t)\\ y(t)&=C_2 x(t)+D_{21} w(t)+D_{22} u(t)\\ \dot x_K(t)=A_K x_K(t)+B_K y(t)\\ u(t)&=C_Kx_K(t)+D_Ky(t)\end{align}$$

where $$A_K,$$ ,$$B_K,$$,$$C_K,$$ and $$D_K,$$ form the K matrix as defined in below. This, therefore, means that the Regulator system can be re-written as:

\begin{bmatrix} \dot x(t)\\

z_1(t)\\z_2(t)\\

y(t) \end{bmatrix} = \begin{bmatrix} \begin{array}{c|c c|c} A&{B}&0&{B}\\ C&D&0&D\\ 0&0&0&I\\ C&D&I&D \end{array} \end{bmatrix} \begin{bmatrix} \dot x(t)\\ w_1(t)\\w_2(t)\\ u(t) \end{bmatrix} $$ With the above 9-matrix representation in mind, the we can now derive the controller needed for solving the problem, which in turn will be accomplished through the use of LMI's. Firstly, we will be taking our $$H_2$$/$$H_\infty$$state-feedback control and make some modifications to it. More specifically, since the focus is modeling for worst-case scenario of a given parameter, we will be modifying the LMI's such that the mixed $$H_2$$/$$H_\infty$$ controller is polytopic.

The LMI:
$$H_2$$/$$H_\infty$$ Polytopic Controller for Quadrotor with Robotic Arm.

Recall that from the 9-matrix framework, $$w_1(t)$$ and $$ {w_2}(t)$$ represent our process and sensor noises respectively and $$u(t)$$ represents our input channel. Suppose we were interested in modeling noise across all three of these channels. Then the best way to model uncertainty across all three cases would be modifying the $$D$$ matrix to $$D_i$$, where ($$i=1,..,k$$ parameters, $$D_i=nI$$, and $$n$$ is a constant noise value). This, in turn results in our $$D_{11}$$-$$D_{22}$$ matrices to be modifified to $$D_{11,i}$$-$$D_{22,i}$$

Using the LMI's given for optimal $$H_2$$/$$H_\infty$$-optimal state-feedback controller from Peet Lecture 11 as reference, our resulting polytopic LMI becomes:

$$ \min\limits_{\gamma_1,\gamma_2,X_1,Y_1,Z,A_n,B_n,C_n,D_n}$$$$ \gamma^2_1$$+$$\gamma^2_2$$

\begin{align} \begin{bmatrix} AA_i&AB^T_i&AC^T_i\\ AB_i&BB_i&BC^T_i\\ AC_i&BC_i&-I \end{bmatrix}&<0\\ \begin{bmatrix} AA_i&AB^T_i&AC^T_i&AD^T_i\\ AB_i&BB_i&BC^T_i&BD^T_i\\ AC_i&BC_i&-I&CD^T_i\\ AD_i&BD_i&CD_i&{-\gamma^2_{2}}I \end{bmatrix}&<0 \\ \begin{bmatrix} Y_1&I&AD^T_i\\ I&X_1&BD^T_i\\ AD_i&BD_i&Z \end{bmatrix}&>0 \\ \end{align}$$ CD=0

$$trace(Z)<\gamma^2_1$$

where i=1,..,k,$$||S(K,P)||_{H_2}$$&$$<\gamma_1$$ and $$||S(K,P)||_{H_\infty}<\gamma_2$$ and:

$$ \begin{align} AA_i=A Y_1+ Y_1 A^T + B_2 C_n+ C^T_n B^T_2\\ AB_i=A^T+A_n+[B_2 D_n C_2]^T\\ AC_i=[B_1+B_2 D_n D_{21,i}]^T\\ AD_i=C_1 Y_1+D_{12,i}C_n\\ BB_i=X_1 A+A^T X_1+B_n C_2+C^T_2 B^T_n\\ BC_i=[X_1 B_1+B_n D_{21,i}]^T\\ BD_i=C_1+D_{12,i} D_n C_2\\ CD_i=D_{11,i}+D_{12,i} D_n D_{21,i} \end{align} $$

After solving for both the optimal $$H_2$$ and $$H_\infty$$ gain ratios as well as $${X_1},{Y_1},Z,{A_n},{B_n},{C_n},{D_n}$$, we can then construct our worst-case scenario controller by setting our $$D$$ matrix (and consequently our $${D_{11}},{D_{12}},{D_{21}},{D_{22}}$$ matrices) to the highest $$n$$value. This results in the controller:
 * $$\begin{align}

K=\begin{bmatrix}\begin{array}{c|c}{A_K}&{B_K}\\ \hline {C_K}&{D_K}\\\end{array}\end{bmatrix}\end{align} $$ which is constructed by setting:

\begin{align} &{D_K}=(I+D_{K_2}D_{22})^{-1}{D_{K_2}}\\ &{B_K}={B_{K_2}}(I+D_{22}D_{K})\\ &{C_K}=(I-D_{K}D_{22}){C_{K_2}}\\ &{A_K}={A_{K_2}}-{B_K}(I-D_{22}D_{K})^{-1}D_{22}{C_K} \end{align} $$

where:

\begin{align} &{X_2}{Y^T_2}=I-{X_1}{Y_1}\\ &\begin{bmatrix}\begin{array}{c|c}{A_{K_2}}&{B_{K_2}}\\ \hline {C_{K_2}}&{D_{K_2}}\end{array}\end{bmatrix}= \begin{bmatrix}{X_2}&{X_1}{B_2}\\0&I\end{bmatrix}^{-1} \begin{bmatrix}{A_n}-{X_1}A{Y_1}&{B_n}\\{C_n}&{D_n}\end{bmatrix} \begin{bmatrix}Y^T_2&0\\{C_2}{Y_1}&I\end{bmatrix} \end{align} $$

Conclusion:
The LMI is feasible and the resulting controller is found to be stable under normal noise disturbances for all states.