% Dedução matematica
% Restart geral: limpa variáveis antigas, limpa a memória, fecha todas as
% janelas e limpa todos os comandos usados no 'workspace' (área de trabalho).
clear all; close all; clc;
% Definição dos símbolos
% - Variáveis de estados (X e X.):
% - Posições:
x = sym('x','real');
y = sym('y','real');
z = sym('z','real');
Pos = [x y z]';
% - Velocidades (v):
vx = sym('vx','real');
vy = sym('vy','real');
vz = sym('vy','real');
V = [vx vy vz]';
% - Acelerações (a):
ax = sym('ax','real');
ay = sym('ay','real');
az = sym('az','real');
Vponto = [ax ay az]';
% - Angulos:
psi = sym('psi','real');
theta = sym('theta','real');
phi = sym('phi','real'); 
Ang = [psi theta phi]';
% - Velocidades angulares (w):
wpsi = sym('wpsi','real');
wtheta = sym('wtheta','real');
wphi = sym('wphi','real');
Wang = [wpsi wtheta wphi]';
% - Acelerações angulares (aw):
awpsi = sym('awpsi','real');
awtheta = sym('awtheta','real');
awphi = sym('awphi','real');
Wangponto = [awpsi awtheta awphi]';
% - X
X = [Pos' V' Ang' Wang']';
% - Xponto
Xponto = [V' Vponto' Wang' Wangponto']';
% - Variáveis de entradas (U):
% - Velocidades angulares do motor da esquerda:
wlp = sym('wlp','real');
% - Velocidades angulares do motor da direita:
wrp = sym('wrp','real');
% - Velocidades angulares do motor da cauda:
wbp = sym('wbp','real');
% - Angulo de inclinação dos rotores frontais:
deltalp = sym('deltalp','real');
deltarp = sym('deltarp','real');
% - Matriz U:
U = [wlp wrp wbp deltalp deltarp]';
% - Constantes:
% - Velocidade angular de inclinação do rotores frontais
% (velocidade nominal dos servomotores)
wdelta = sym('wdelta','real');
wdelta = [0 -1 0]'*wdelta;
% - Constante de conversão força - velocidade angular dos rotores
klp = sym('klp','real');
krp = sym('krp','real');
kbp = sym('kbp','real');
% - Elementos da matriz de inércia J:
Jxx = sym('Jxx','real');
Jxy = sym('Jxy','real');
Jxz = sym('Jxz','real');
Jyx = sym('Jyx','real');
Jyy = sym('Jyy','real');
Jyz = sym('Jyz','real');
Jzx = sym('Jzx','real');
Jzy = sym('Jzy','real');
Jzz = sym('Jzz','real');
% - Matriz de inécia J:
J = [Jxx Jxy Jxz

 Jyx Jyy Jyz
 Jzx Jzy Jzz];
% - Massa do modelo (m):
m = sym('m','real');
% - Força da gravidade (g):
g = sym('g','real');
% - Vetor de distância do rotor da esquerda ao cg:
rlpx = sym('rlpx','real');
rlpy = sym('rlpy','real');
rlpz = sym('rlpz','real');
rlp = [rlpx, rlpy, rlpz]';
% - Vetor de distância do rotor da direita ao cg:
rrpx = sym('rrpx','real');
rrpy = sym('rrpy','real');
rrpz = sym('rrpz','real');
rrp = [rrpx, rrpy, rrpz]';
% - Vetor de distância do rotor da cauda ao cg:
rbpx = sym('rbpx','real');
rbpy = sym('rbpy','real');
rbpz = sym('rbpz','real');
rbp = [rbpx, rbpy, rbpz]';
% Rotações
% Matriz de rotação de roll:
Rx = [1 0 0
 0 cos(psi) sin(psi)
 0 -sin(psi) cos(psi)];
% Matriz de rotação de pitch:
Ry = [cos(theta) 0 -sin(theta)
 0 1 0 sin(theta) 0 cos(theta)];
% Matriz de rotação de yaw:
Rz = [ cos(phi) sin(phi) 0 -sin(phi) cos(phi) 0 0 0 1];
% Matriz de rotação completa (R):
R = Rz*Ry*Rx;
% Somatório das Forças
% Definição da força dos rotores (Fprop):
Fpropdir = [ sin(deltalp) sin(deltarp) 0 0 0 0 -cos(deltalp) -cos(deltarp) 1];% Matriz de direção das forças
Fpropabs = [klp*wlp^2*sign(wlp) krp*wrp^2*sign(wrp) kbp*wbp^2*sign(wbp)]'; % Matriz de módulo das forças
Fprop = Fpropdir*Fpropabs;
% Definição da força da gravidade (Fg):
Fg = R*[0 0 1]'*m*g;
% Matriz das forças (F):
F = Fprop + Fg;
% Somatório dos Momentos
% Definição do momento do rotor da esquerda (Mlp):
Mlpdir = cross(rlp, Fpropdir(:,1));
% Definição do momento do rotor da direita (Mrp):
Mrpdir = cross(rrp, Fpropdir(:,2));
% Definição do momento do rotor da cauda (Mbp):
Mbpdir = cross(rbp, Fpropdir(:,3));
% Definição da matriz dos momentos dos rotores (Mprop):
Mdir = [Mlpdir Mrpdir Mbpdir];
Mabs = Fpropabs;
Mprop = Mdir*Mabs;
% Definição da matriz do momento giroscópico do rotor da esquerda (Mgirolp):
Mgirolp = J*cross(wdelta,Fpropdir(:,1))*wlp;
% Definição da matriz do momento giroscópico do rotor da direita (Mgirorp):
Mgirorp = J*cross(wdelta,Fpropdir(:,2))*wrp;
% Definição da matriz do momento giroscópico do rotor da cauda (Mgirobp):
Mgirobp = J*cross(zeros(size(Fpropdir(:,1))),Fpropdir(:,3))*wbp;
% Definição da matriz do momento giroscópico do rotor da cauda (Mgirobp):
Mgiro = Mgirolp+Mgirorp+Mgirobp;
% Matriz dos momentos (M):
M = Mprop + Mgiro;
% Aplicação das Leis de Newton-Euler - Forças
% F = mVponto+cross(W,mV)
% Vponto = (1/m)*[F-cross(W,mV)]
Vponto2 = (1/m).*(F-cross(Ang,m*V));
[Vponto Vponto2]
display('========================================')
% Aplicação das Leis de Newton-Euler - Momentos
% M = J*Wponto+cross(W,JW)
% Wponto = inv(J)*[M-cross(W,JW)]
Wangponto2 = inv(J)*(M-cross(Wang,J*Wang));
[Wangponto Wangponto2]
display('========================================')
% Matriz X. Completa
Xponto2 = [Xponto(1:3)' Vponto2' Xponto(7:9)' Wangponto2']'
display('========================================')
wdelta = -2*pi; % em [rad/s]
% wdelta = [0 -1 0]'*wdelta; % em forma vetorial
% Matriz Jacobiana (A,B,C,D)
A = jacobian(Xponto2,X)
B = jacobian(Xponto2,U)
C = eye(size(A))
D = zeros(length(A),length(U))