%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);
%Se obtinen las funciones de transferencia a partir de las matrices del sistema
sist_lon=ss(Alon, Blon, Clon, Dlon); %Longitudinal
sist_lat=ss(Alat, Blat, Clat, Dlat); %Lateral
h=Alt; %m
h_ref=-5; %m
psi_ref=5; %deg
Va=0.9977*u+0.0477*w; %m/s
Va_ref=+2; %m/s
C_aut_lon=[ 0 0 0 0 1 0
0.9977 0.0477 0 0 0 0];
C_aut_lat=[0 0 0 0 1];
A_aut_lon=[zeros(2,2) C_aut_lon
zeros(6,2) Alon];
B_aut_lon=[zeros(2,2)
Blon];
%Cálculo de la matriz de ganancia optima para el movimiento longitudinal
K_aut_lon=lqr(A_aut_lon,B_aut_lon,eye(8), 1*eye(2));
K_aut_lon2=place(A_aut_lon,B_aut_lon,[-7, -1.8+1.6i,-1.8-1.6i,-1.1+1.1i,-1.1-1.1i,-0.9, -0.18+0.15i,-0.18-0.15i]);
A_aut_lat=[zeros(1,1) C_aut_lat
zeros(5,1) Alat];
B_aut_lat=[zeros(1,2)
Blat];
%Cálculo de la matriz de ganancia optima para el movimiento lateral
K_aut_lat=lqr(A_aut_lat,B_aut_lat,eye(6), 1*eye(2));
K_aut_lat2=place(A_aut_lat,B_aut_lat,[-10, -3+2.5i,-3-2.5i,-0.95, -0.15+0.15i,-0.15-0.15i]);
e-REdING. Biblioteca de la Escuela Superior de Ingenieros de Sevilla.
