%Definimos los parámetros necesarios para el modelo del Navion

%Entradas
Elevator = -0.0304;
Aileron = 0.0036;
Rudder = 0.0475;
Throttle = 0.5189;

%Estados
u =   61.26;        %m/s
v =   2.97;         %m/s
w =   2.93;         %m/s
p =  0.00;          %deg/s
q =  0.00;          %deg/s
r =  0.00;          %deg/s
phi =   2.49;       %deg
theta = 2.86;       %deg
psi = 0.00;         %deg
Alt = 5217.00;      %m
Fuel =   194.00;    %kg
Engine =   3943;    %rot/min

%Pasamos de ángulos de Euler a quaternions
q0=cos(phi/2*pi/180)*cos(theta/2*pi/180)*cos(psi/2*pi/180)+sin(phi/2*pi/180)*sin(theta/2*pi/180)*sin(psi/2*pi/180);
q1=sin(phi/2*pi/180)*cos(theta/2*pi/180)*cos(psi/2*pi/180)-cos(phi/2*pi/180)*sin(theta/2*pi/180)*sin(psi/2*pi/180);
q2=cos(phi/2*pi/180)*sin(theta/2*pi/180)*cos(psi/2*pi/180)+sin(phi/2*pi/180)*cos(theta/2*pi/180)*sin(psi/2*pi/180);
q3=cos(phi/2*pi/180)*cos(theta/2*pi/180)*sin(psi/2*pi/180)-sin(phi/2*pi/180)*sin(theta/2*pi/180)*cos(psi/2*pi/180);

%Outputs
Airspeed =  61.40;      %m/s
Sideslip =   2.77;      %deg
AOA =  2.74;            %deg
Bank =   2.49;          %deg
Pitch =  2.86;          %deg
Heading = 0.00;         %deg
Altitude =  5217.00;    %m
    


% Longitudinal Dynamics
% -----------------------
% State vector: x = [u w q theta h Omega]
% Input vector: u = [elevator throttle]
% Output vector: y = [Va alpha q theta h]

% Matriz de estado:
Alon = [-0.0524    0.1817   -2.9123    -9.7752    -0.0001    0.0088
        -0.2499   -1.4256    60.2129   -0.4873     0.0011    0
         0.0017   -0.1113   -1.4062     0         -0.0000    0.0005
         0         0         0.9991     0          0         0
         0.0498   -0.9978    0          61.3335    0         0
         14.7454   0.7055    0          0          0.0456   -6.4606];

% Matriz de control 
Blon = [ 0.1216    0
        -6.8960    0
        -9.2110    0
         0         0
         0         0
         0      -79.3522];
         
% Matriz de observación: 
Clon = [ 0.9977    0.0477     0         0         0          0
        -0.0008    0.0163     0         0         0          0
         0         0          1.0000    0         0          0
         0         0          0         1.0000    0          0
         0         0          0         0         1.0000     0];
     
Dlon = zeros(5,2);



%  Lateral-directional Dynamics
% ------------------------------
%   State vector: x = [v p r phi psi]
%   Input vector: u = [aileron rudder]
%  Output vector: y = [beta p r phi psi]

% Matriz de estado:
Alat = [-0.1919    2.9307   -61.2573    9.7660    0
        -0.1930   -5.6982    1.4278     0         0
         0.0513   -0.4099   -0.4683     0         0
         0         1.0000    0.0499     0.0000    0
         0         0         1.0003     0.0000    0];


 % Matriz de control     
 Blat = [ 0         3.0453
         -22.3861   17.4654
         -0.8581   -3.0245
          0            0
          0            0];

% Matriz de observación: 
 Clat = [0.0163    0         0         0         0
         0         1.0000    0         0         0
         0         0         1.0000    0         0
         0         0         0         1.0000    0
         0         0         0         0         1.0000];
     
 Dlat = zeros(5,2);
 
 %Cálculo de la matriz de ganancia óptima del observador
 L=lqr(Alon', Clon', eye(6), eye(5));
 L=L';
 Llat=lqr(Alat', Clat', eye(5), eye(5));
 Llat=Llat';