% Controle de uma aeronave com rotores inclináveis
% Limpa todas as variáveis e esvazia a memória. Fecha todas as janelas
% abertas e limpa todos os comandos utilizados no 'workspace'.
clear all; close all; clc;
% DEFINIÇÕES
% Definição de variáveis, constantes, entradas, etc.
% - Constantes (DADOS):
% - Velocidade angular de inclinação do rotores laterais
wdelta = -2*pi; % em [rad/s]
% - Constantes de conversão força - velocidade angular dos rotores
klp = 1;
krp = 1;
kbp = 1;
Ks=[klp;krp;kbp];
% - Elementos da matriz de inércia J (obtidos através da modelagem do sistema pelo
software SolidWorks):
Jxx = 1e-9*57365230.29;
Jxy = 1e-9*7372.76;
Jxz = 1e-9*-3639.40;
Jyx = 1e-9*7372.76;
Jyy = 1e-9*104669206.42;
Jyz = 1e-9*1000159.15;
Jzx = 1e-9*-3639.40;
Jzy = 1e-9*1000159.15;
Jzz = 1e-9*64929661.23;
J=[Jxx Jxy Jxz;Jyx Jyy Jyz;Jzx Jzy Jzz];
% - Massa total do modelo (m):
m = 2230.89*1e-3; % em [kg];
% - Força da gravidade (g):
g = 9.81; % em [m/s^2]
% - Vetor de distância do rotor da esquerda ao cg:
rlpx = -21.83*0.001; % em [m]
rlpy = 348.09*0.001; % em [m]
rlpz = -220.97*0.001; % em [m]
rlp = [rlpx, rlpy, rlpz]'; % em [m]
% - Vetor de distância do rotor da direita ao cg:
rrpx = -21.83*0.001; % em [m]
rrpy = -348.09*0.001; % em [m]
rrpz = -220.97*0.001; % em [m]
rrp = [rrpx, rrpy, rrpz]'; % em [m]
% - Vetor de distância do rotor da cauda ao cg:
rbpx = -480.91*0.001; % em [m]
rbpy = 0*0.001; % em [m]
rbpz = -46.07*0.001; % em [m]
rbp = [rbpx, rbpy, rbpz]'; % em [m]
% - Condições iniciais de X:
X0 = zeros(12,1); % as posições de X são referentes às seguintes variáveis
 % x = 1, y = 2, z = 3, vx = 4, vy = 5, vz = 6,
 % psi = 7, theta = 8, phi = 9, wpsi = 10
 % wtheta = 11, wphi = 12
% - Entradas (INPUTS):
% - Velocidades angulares do motor da esquerda:
wlp = 100;
% - Velocidades angulares do motor da direita:
wrp = 100;
% - Velocidades angulares do motor da cauda:
wbp = 100;
% - Angulo de inclinação dos rotores frontais:
deltalp = 0; % em [graus]
deltarp = 0; % em [graus]
deltalp = (pi/180)*deltalp; % em [rad]
deltarp = (pi/180)*deltarp; % em [rad]
% - Matriz U:
U = [wlp wrp wbp deltalp deltarp]; % as posições de U são referentes às seguintes variáveis
 % wlp = 1, wrp = 2, wbp = 3
 % deltalp = 4, deltarp = 5
phi=0;
theta=0;
psi=0;
wphi=0;
wtheta=0;
wpsi=0;
vx=0;
vy=0;
vz=8.33;
A =[ 0 0 0 1 0 0 0
0 0
0
0
0;
0 0 0 0 1 0 0
0 0
0
0
0;
0 0 0 0 0 1 0
0 0
0
0
0;
0 0 0 0 phi -theta g*(cos(psi)*sin(phi) + cos(phi)*sin(psi)*sin(theta)) -(m*vz
+ g*m*cos(phi)*cos(psi)*cos(theta))/m (m*vy + g*m*(cos(phi)*sin(psi) +
cos(psi)*sin(phi)*sin(theta)))/m
0
0
0;
0 0 0 -phi 0 psi (m*vz + g*m*(cos(phi)*cos(psi) - sin(phi)*sin(psi)*sin(theta)))/m
g*cos(psi)*cos(theta)*sin(phi) -(m*vx + g*m*(sin(phi)*sin(psi) -
cos(phi)*cos(psi)*sin(theta)))/m
0
0
0;
0 0 0 theta -psi 0 -(m*vy + g*m*cos(theta)*sin(psi))/m
(m*vx - g*m*cos(psi)*sin(theta))/m 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
0
1
0;
0 0 0 0 0 0 0
0 0
0
0
1;
0 0 0 0 0 0 0
0 0 ((Jyy*Jzz - Jyz*Jzy)*(Jyx*wphi -
Jzx*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jxy*Jyz - Jxz*Jyy)*(Jyz*wphi + 2*Jyx*wpsi - Jxx*wtheta +
Jyy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jxy*Jzz - Jxz*Jzy)*(Jzz*wphi - Jxx*wphi + 2*Jzx*wpsi +
Jzy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) ((Jxy*Jzz - Jxz*Jzy)*(Jxy*wphi - Jzy*wpsi))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jxy*Jyz - Jxz*Jyy)*(Jxz*wphi
+ Jxx*wpsi - Jyy*wpsi + 2*Jxy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jyy*Jzz - Jyz*Jzy)*(Jzz*wphi - Jyy*wphi +
Jzx*wpsi + 2*Jzy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx +
Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) ((Jxy*Jzz - Jxz*Jzy)*(2*Jxz*wphi + Jxx*wpsi - Jzz*wpsi +
Jxy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jxy*Jyz - Jxz*Jyy)*(Jyz*wpsi - Jxz*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jyy*Jzz -
Jyz*Jzy)*(2*Jyz*wphi + Jyx*wpsi + Jyy*wtheta - Jzz*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx);
0 0 0 0 0 0 0
0 0 ((Jxx*Jyz - Jxz*Jyx)*(Jyz*wphi +
2*Jyx*wpsi - Jxx*wtheta + Jyy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jyx*Jzz - Jyz*Jzx)*(Jyx*wphi -
Jzx*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) + ((Jxx*Jzz - Jxz*Jzx)*(Jzz*wphi - Jxx*wphi + 2*Jzx*wpsi +
Jzy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) ((Jyx*Jzz - Jyz*Jzx)*(Jzz*wphi - Jyy*wphi + Jzx*wpsi +
2*Jzy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jxx*Jyz - Jxz*Jyx)*(Jxz*wphi + Jxx*wpsi - Jyy*wpsi +
2*Jxy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jxx*Jzz - Jxz*Jzx)*(Jxy*wphi - Jzy*wpsi))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) ((Jxx*Jyz - Jxz*Jyx)*(Jyz*wpsi -
Jxz*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jxx*Jzz - Jxz*Jzx)*(2*Jxz*wphi + Jxx*wpsi - Jzz*wpsi +
Jxy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jyx*Jzz - Jyz*Jzx)*(2*Jyz*wphi + Jyx*wpsi + Jyy*wtheta -
Jzz*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx);
0 0 0 0 0 0 0
0 0 ((Jyx*Jzy - Jyy*Jzx)*(Jyx*wphi -
Jzx*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jxx*Jyy - Jxy*Jyx)*(Jyz*wphi + 2*Jyx*wpsi - Jxx*wtheta +
Jyy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jxx*Jzy - Jxy*Jzx)*(Jzz*wphi - Jxx*wphi + 2*Jzx*wpsi +
Jzy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) ((Jxx*Jzy - Jxy*Jzx)*(Jxy*wphi - Jzy*wpsi))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jxx*Jyy - Jxy*Jyx)*(Jxz*wphi
+ Jxx*wpsi - Jyy*wpsi + 2*Jxy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jyx*Jzy - Jyy*Jzx)*(Jzz*wphi - Jyy*wphi +
Jzx*wpsi + 2*Jzy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx +
Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) ((Jxx*Jzy - Jxy*Jzx)*(2*Jxz*wphi + Jxx*wpsi - Jzz*wpsi +
Jxy*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) - ((Jxx*Jyy - Jxy*Jyx)*(Jyz*wpsi - Jxz*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jyx*Jzy -
Jyy*Jzx)*(2*Jyz*wphi + Jyx*wpsi + Jyy*wtheta - Jzz*wtheta))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx);]
B =[0 0 0 0 0;
 0 0 0 0 0;
 0 0 0 0 0;
 ((2*klp*wlp*sin(deltalp)*sign(wlp) + 2*klp*wlp^2*sin(deltalp)*dirac(wlp))/m)
((2*krp*wrp*sin(deltarp)*sign(wrp) + 2*krp*wrp^2*sin(deltarp)*dirac(wrp))/m) 0
((klp*wlp^2*cos(deltalp)*sign(wlp))/m) ((krp*wrp^2*cos(deltarp)*sign(wrp))/m);
 0 0 0 0 0;
 (-(2*klp*wlp^2*cos(deltalp)*dirac(wlp) + 2*klp*wlp*cos(deltalp)*sign(wlp))/m) (-
(2*krp*wrp^2*cos(deltarp)*dirac(wrp) + 2*krp*wrp*cos(deltarp)*sign(wrp))/m)
((2*kbp*wbp*sign(wbp) + 2*kbp*wbp^2*dirac(wbp))/m)
((klp*wlp^2*sin(deltalp)*sign(wlp))/m) ((krp*wrp^2*sin(deltarp)*sign(wrp))/m);
 0 0 0 0 0;
 0 0 0 0 0;
 0 0 0 0 0;
 ((Jyy*Jzz - Jyz*Jzy)*(Jxx*wdelta*cos(deltalp) + Jxz*wdelta*sin(deltalp) -
2*klp*rlpy*wlp*cos(deltalp)*sign(wlp) -
2*klp*rlpy*wlp^2*cos(deltalp)*dirac(wlp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxy*Jzz - Jxz*Jzy)*(Jyx*wdelta*cos(deltalp) +
Jyz*wdelta*sin(deltalp) + 2*klp*wlp^2*dirac(wlp)*(rlpx*cos(deltalp) + rlpz*sin(deltalp)) +
2*klp*wlp*sign(wlp)*(rlpx*cos(deltalp) + rlpz*sin(deltalp))))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jxy*Jyz -
Jxz*Jyy)*(Jzx*wdelta*cos(deltalp) + Jzz*wdelta*sin(deltalp) -
2*klp*rlpy*wlp*sin(deltalp)*sign(wlp) -
2*klp*rlpy*wlp^2*sin(deltalp)*dirac(wlp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx), ((Jyy*Jzz - Jyz*Jzy)*(Jxx*wdelta*cos(deltarp) +
Jxz*wdelta*sin(deltarp) - 2*krp*rrpy*wrp*cos(deltarp)*sign(wrp) -
2*krp*rrpy*wrp^2*cos(deltarp)*dirac(wrp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxy*Jzz - Jxz*Jzy)*(Jyx*wdelta*cos(deltarp) + 
Jyz*wdelta*sin(deltarp) + 2*krp*wrp^2*dirac(wrp)*(rrpx*cos(deltarp) + rrpz*sin(deltarp))
+ 2*krp*wrp*sign(wrp)*(rrpx*cos(deltarp) + rrpz*sin(deltarp))))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jxy*Jyz -
Jxz*Jyy)*(Jzx*wdelta*cos(deltarp) + Jzz*wdelta*sin(deltarp) -
2*krp*rrpy*wrp*sin(deltarp)*sign(wrp) -
2*krp*rrpy*wrp^2*sin(deltarp)*dirac(wrp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx), ((Jxy*Jzz -
Jxz*Jzy)*(2*kbp*rbpx*wbp*sign(wbp) + 2*kbp*rbpx*wbp^2*dirac(wbp)))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jyy*Jzz -
Jyz*Jzy)*(2*kbp*rbpy*wbp*sign(wbp) + 2*kbp*rbpy*wbp^2*dirac(wbp)))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx),
((wlp*(Jzz*wdelta*cos(deltalp) - Jzx*wdelta*sin(deltalp)) -
klp*rlpy*wlp^2*cos(deltalp)*sign(wlp))*(Jxy*Jyz - Jxz*Jyy))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jyy*Jzz -
Jyz*Jzy)*(wlp*(Jxz*wdelta*cos(deltalp) - Jxx*wdelta*sin(deltalp)) +
klp*rlpy*wlp^2*sin(deltalp)*sign(wlp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxy*Jzz -
Jxz*Jzy)*(wlp*(Jyz*wdelta*cos(deltalp) - Jyx*wdelta*sin(deltalp)) +
klp*wlp^2*sign(wlp)*(rlpz*cos(deltalp) - rlpx*sin(deltalp))))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx), ((wrp*(Jzz*wdelta*cos(deltarp) -
Jzx*wdelta*sin(deltarp)) - krp*rrpy*wrp^2*cos(deltarp)*sign(wrp))*(Jxy*Jyz -
Jxz*Jyy))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) + ((Jyy*Jzz - Jyz*Jzy)*(wrp*(Jxz*wdelta*cos(deltarp) -
Jxx*wdelta*sin(deltarp)) + krp*rrpy*wrp^2*sin(deltarp)*sign(wrp)))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxy*Jzz -
Jxz*Jzy)*(wrp*(Jyz*wdelta*cos(deltarp) - Jyx*wdelta*sin(deltarp)) +
krp*wrp^2*sign(wrp)*(rrpz*cos(deltarp) - rrpx*sin(deltarp))))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx);
 (((Jxx*Jzz - Jxz*Jzx)*(Jyx*wdelta*cos(deltalp) + Jyz*wdelta*sin(deltalp) +
2*klp*wlp^2*dirac(wlp)*(rlpx*cos(deltalp) + rlpz*sin(deltalp)) +
2*klp*wlp*sign(wlp)*(rlpx*cos(deltalp) + rlpz*sin(deltalp))))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jyx*Jzz -
Jyz*Jzx)*(Jxx*wdelta*cos(deltalp) + Jxz*wdelta*sin(deltalp) -
2*klp*rlpy*wlp*cos(deltalp)*sign(wlp) -
2*klp*rlpy*wlp^2*cos(deltalp)*dirac(wlp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxx*Jyz - Jxz*Jyx)*(Jzx*wdelta*cos(deltalp) +
Jzz*wdelta*sin(deltalp) - 2*klp*rlpy*wlp*sin(deltalp)*sign(wlp) -
2*klp*rlpy*wlp^2*sin(deltalp)*dirac(wlp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx)) (((Jxx*Jzz - Jxz*Jzx)*(Jyx*wdelta*cos(deltarp) +
Jyz*wdelta*sin(deltarp) + 2*krp*wrp^2*dirac(wrp)*(rrpx*cos(deltarp) + rrpz*sin(deltarp))
+ 2*krp*wrp*sign(wrp)*(rrpx*cos(deltarp) + rrpz*sin(deltarp))))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jyx*Jzz -
Jyz*Jzx)*(Jxx*wdelta*cos(deltarp) + Jxz*wdelta*sin(deltarp) -
2*krp*rrpy*wrp*cos(deltarp)*sign(wrp) -
2*krp*rrpy*wrp^2*cos(deltarp)*dirac(wrp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxx*Jyz - Jxz*Jyx)*(Jzx*wdelta*cos(deltarp) +
Jzz*wdelta*sin(deltarp) - 2*krp*rrpy*wrp*sin(deltarp)*sign(wrp) -
2*krp*rrpy*wrp^2*sin(deltarp)*dirac(wrp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx)) (- ((Jxx*Jzz -
Jxz*Jzx)*(2*kbp*rbpx*wbp*sign(wbp) + 2*kbp*rbpx*wbp^2*dirac(wbp)))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jyx*Jzz -
Jyz*Jzx)*(2*kbp*rbpy*wbp*sign(wbp) + 2*kbp*rbpy*wbp^2*dirac(wbp)))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx)) (((Jxx*Jzz -
Jxz*Jzx)*(wlp*(Jyz*wdelta*cos(deltalp) - Jyx*wdelta*sin(deltalp)) +
klp*wlp^2*sign(wlp)*(rlpz*cos(deltalp) - rlpx*sin(deltalp))))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jyx*Jzz -
Jyz*Jzx)*(wlp*(Jxz*wdelta*cos(deltalp) - Jxx*wdelta*sin(deltalp)) + 
klp*rlpy*wlp^2*sin(deltalp)*sign(wlp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((wlp*(Jzz*wdelta*cos(deltalp) -
Jzx*wdelta*sin(deltalp)) - klp*rlpy*wlp^2*cos(deltalp)*sign(wlp))*(Jxx*Jyz -
Jxz*Jyx))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx)) (((Jxx*Jzz - Jxz*Jzx)*(wrp*(Jyz*wdelta*cos(deltarp) -
Jyx*wdelta*sin(deltarp)) + krp*wrp^2*sign(wrp)*(rrpz*cos(deltarp) -
rrpx*sin(deltarp))))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy
- Jxz*Jyy*Jzx) - ((Jyx*Jzz - Jyz*Jzx)*(wrp*(Jxz*wdelta*cos(deltarp) -
Jxx*wdelta*sin(deltarp)) + krp*rrpy*wrp^2*sin(deltarp)*sign(wrp)))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) -
((wrp*(Jzz*wdelta*cos(deltarp) - Jzx*wdelta*sin(deltarp)) -
krp*rrpy*wrp^2*cos(deltarp)*sign(wrp))*(Jxx*Jyz - Jxz*Jyx))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx));
 (((Jyx*Jzy - Jyy*Jzx)*(Jxx*wdelta*cos(deltalp) + Jxz*wdelta*sin(deltalp) -
2*klp*rlpy*wlp*cos(deltalp)*sign(wlp) -
2*klp*rlpy*wlp^2*cos(deltalp)*dirac(wlp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxx*Jzy - Jxy*Jzx)*(Jyx*wdelta*cos(deltalp) +
Jyz*wdelta*sin(deltalp) + 2*klp*wlp^2*dirac(wlp)*(rlpx*cos(deltalp) + rlpz*sin(deltalp)) +
2*klp*wlp*sign(wlp)*(rlpx*cos(deltalp) + rlpz*sin(deltalp))))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jxx*Jyy -
Jxy*Jyx)*(Jzx*wdelta*cos(deltalp) + Jzz*wdelta*sin(deltalp) -
2*klp*rlpy*wlp*sin(deltalp)*sign(wlp) -
2*klp*rlpy*wlp^2*sin(deltalp)*dirac(wlp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx)) (((Jyx*Jzy - Jyy*Jzx)*(Jxx*wdelta*cos(deltarp)
+ Jxz*wdelta*sin(deltarp) - 2*krp*rrpy*wrp*cos(deltarp)*sign(wrp) -
2*krp*rrpy*wrp^2*cos(deltarp)*dirac(wrp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxx*Jzy - Jxy*Jzx)*(Jyx*wdelta*cos(deltarp) +
Jyz*wdelta*sin(deltarp) + 2*krp*wrp^2*dirac(wrp)*(rrpx*cos(deltarp) + rrpz*sin(deltarp))
+ 2*krp*wrp*sign(wrp)*(rrpx*cos(deltarp) + rrpz*sin(deltarp))))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jxx*Jyy -
Jxy*Jyx)*(Jzx*wdelta*cos(deltarp) + Jzz*wdelta*sin(deltarp) -
2*krp*rrpy*wrp*sin(deltarp)*sign(wrp) -
2*krp*rrpy*wrp^2*sin(deltarp)*dirac(wrp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx)) (((Jxx*Jzy -
Jxy*Jzx)*(2*kbp*rbpx*wbp*sign(wbp) + 2*kbp*rbpx*wbp^2*dirac(wbp)))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jyx*Jzy -
Jyy*Jzx)*(2*kbp*rbpy*wbp*sign(wbp) + 2*kbp*rbpy*wbp^2*dirac(wbp)))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx))
(((wlp*(Jzz*wdelta*cos(deltalp) - Jzx*wdelta*sin(deltalp)) -
klp*rlpy*wlp^2*cos(deltalp)*sign(wlp))*(Jxx*Jyy - Jxy*Jyx))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) + ((Jyx*Jzy -
Jyy*Jzx)*(wlp*(Jxz*wdelta*cos(deltalp) - Jxx*wdelta*sin(deltalp)) +
klp*rlpy*wlp^2*sin(deltalp)*sign(wlp)))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz +
Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxx*Jzy -
Jxy*Jzx)*(wlp*(Jyz*wdelta*cos(deltalp) - Jyx*wdelta*sin(deltalp)) +
klp*wlp^2*sign(wlp)*(rlpz*cos(deltalp) - rlpx*sin(deltalp))))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx)) (((wrp*(Jzz*wdelta*cos(deltarp) -
Jzx*wdelta*sin(deltarp)) - krp*rrpy*wrp^2*cos(deltarp)*sign(wrp))*(Jxx*Jyy -
Jxy*Jyx))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy -
Jxz*Jyy*Jzx) + ((Jyx*Jzy - Jyy*Jzx)*(wrp*(Jxz*wdelta*cos(deltarp) -
Jxx*wdelta*sin(deltarp)) + krp*rrpy*wrp^2*sin(deltarp)*sign(wrp)))/(Jxx*Jyy*Jzz -
Jxx*Jyz*Jzy - Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx) - ((Jxx*Jzy -
Jxy*Jzx)*(wrp*(Jyz*wdelta*cos(deltarp) - Jyx*wdelta*sin(deltarp)) +
krp*wrp^2*sign(wrp)*(rrpz*cos(deltarp) - rrpx*sin(deltarp))))/(Jxx*Jyy*Jzz - Jxx*Jyz*Jzy -
Jxy*Jyx*Jzz + Jxy*Jyz*Jzx + Jxz*Jyx*Jzy - Jxz*Jyy*Jzx));]
 
C = eye(size(A))
D = zeros(length(A),length(U))
CO = ctrb(A,B) % Matriz de controlabilidade %
Posto = rank(CO) % teste posto %
OB = obsv(A,C) % Matriz de observabilidade %
observa = rank(OB) % teste posto %
% Resposta ao degrau unitário em malha aberta
 sys=ss(A,B,C,D);
 figure
 step(sys);
% Valores propostos para os novos polos do sistema realimentado %
 p= [-1+3i -1-3i -3 -4 -5 -6 -7 -8 -9 -10 -11 -12]
% Matriz K de ganhos obtida através do comando place para os polos propostos %
 K = place(A,B,p)
% Matrizes Q e R Realimentação de estados com LQR %
Q = [1 0 0 0 0 0 0 0 0 0 0 0;
 0 1 0 0 0 0 0 0 0 0 0 0;
 0 0 1 0 0 0 0 0 0 0 0 0;
 0 0 0 1 0 0 0 0 0 0 0 0;
 0 0 0 0 1 0 0 0 0 0 0 0;
 0 0 0 0 0 1 0 0 0 0 0 0;
 0 0 0 0 0 0 1 0 0 0 0 0;
 0 0 0 0 0 0 0 1 0 0 0 0;
 0 0 0 0 0 0 0 0 1 0 0 0;
 0 0 0 0 0 0 0 0 0 1 0 0;
 0 0 0 0 0 0 0 0 0 0 1 0;
 0 0 0 0 0 0 0 0 0 0 0 1;]
R = [1000 0 0 0 0;
 0 1000 0 0 0;
 0 0 1000 0 0;
 0 0 0 1000 0;
 0 0 0 0 1000;]
% Realimentação de estados com LQR %
[K2,S,e] = lqr(sys,Q,R,0)
K = K
K2 = K2
 A1=A-B*K2;
 sys1= ss(A1,B,C,D);
 step(sys1)
 A2=A-B*K2;
 sys2= ss(A2,B,C,D);
 step(sys2) 