├── AddLegend.m ├── Copy_3_of_L1AC_NAVION_Latest3.slx ├── Copy_3_of_L1AC_NAVION_Latest3.slx.r2017a ├── Copy_3_of_L1AC_NAVION_Latest3.slxc ├── Copy_3_of_L1AC_NAVION_Latest3_temp.slx ├── FullScreen.m ├── HideFig.m ├── INIT.m ├── INIT_temp.m ├── LinkAll.m ├── MONTECARLO.m ├── NewFigureHide.m ├── PLOTTING_SCRIPT.m ├── PLOTTING_SCRIPT2.m ├── PLOTTING_SCRIPT3.m ├── PLOTTING_SCRIPT4.m ├── PLOT_ALL.m ├── POLE_PLACEMENT.m ├── PrintAll2PDF.m ├── README.md ├── RemoveLegend.m ├── SETUP.m ├── SETUP_MONTECARLO.m ├── SVM.fig ├── SVM1.emf ├── SVM2.emf ├── SVM2.fig ├── SVM3.emf ├── SVM3.fig ├── SVM_8_9.m ├── SaveFig.m ├── TEST_NAVION_LAT.m ├── TEST_NAVION_LONG.m ├── TRIM.m ├── UnDockAll.m ├── docknewfig.m ├── estrai.m ├── f_NAVION.m ├── f_NAVION_TEMP.m ├── funzione_di_costo.m ├── holdALL.m ├── linearizza.m ├── min objective.fig ├── objective funtion model.fig ├── phi.emf ├── phi.fig ├── plot_good.m ├── ploteveryMONTECARLO.m ├── ploteverything.m ├── psi.emf ├── psi.fig ├── run_control.m ├── svm.mat ├── theta.emf ├── theta.fig └── wind2body.m /AddLegend.m: -------------------------------------------------------------------------------- 1 | function AddLegend(N_MAX) 2 | % Add legends as Nominal + N_MAX 3 | FIG = get(0,'Children'); 4 | 5 | stringa2 = ['legend(''','NOMINAL','''',',']; 6 | 7 | for kk =1:N_MAX 8 | stringa2 = [ stringa2 ,'''','DATA _ ', num2str(kk),'''',',']; 9 | end 10 | 11 | stringa2(end) = ')'; 12 | 13 | 14 | for kk = max(size(FIG)):-1:1 15 | % figure(FIG(kk)) 16 | set(groot, 'CurrentFigure', FIG(kk)); 17 | 18 | eval(stringa2); 19 | end 20 | -------------------------------------------------------------------------------- /Copy_3_of_L1AC_NAVION_Latest3.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/Copy_3_of_L1AC_NAVION_Latest3.slx -------------------------------------------------------------------------------- /Copy_3_of_L1AC_NAVION_Latest3.slx.r2017a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/Copy_3_of_L1AC_NAVION_Latest3.slx.r2017a -------------------------------------------------------------------------------- /Copy_3_of_L1AC_NAVION_Latest3.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/Copy_3_of_L1AC_NAVION_Latest3.slxc -------------------------------------------------------------------------------- /Copy_3_of_L1AC_NAVION_Latest3_temp.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/Copy_3_of_L1AC_NAVION_Latest3_temp.slx -------------------------------------------------------------------------------- /FullScreen.m: -------------------------------------------------------------------------------- 1 | function [] = FullScreen() 2 | % FullScreen() Set all figure to full screen automatically 3 | FIG = get(0,'Children'); 4 | 5 | for kk = max(size(FIG)):-1:1 6 | figure(FIG(kk)); 7 | set(FIG(kk),'units','normalized','outerposition',[0 0 1 1]); 8 | end 9 | 10 | end 11 | -------------------------------------------------------------------------------- /HideFig.m: -------------------------------------------------------------------------------- 1 | function HideFig(input) 2 | %% HideFig Hides all the figure to speed-up plotting 3 | % if function HideFig('on') set figures visible 4 | 5 | FIG = get(0,'Children'); 6 | if nargin==1 && strcmp(input,'on') 7 | set(FIG,'Visible','on'); 8 | else 9 | set(FIG,'Visible','off'); 10 | end 11 | end 12 | -------------------------------------------------------------------------------- /INIT.m: -------------------------------------------------------------------------------- 1 | % Initialize data for trim and simulation. 2 | % This script setup the nominal parameter of the aircraft dynamics 3 | % state_dot = f(state,input) 4 | 5 | global g rho c b S CL_0 CL_alpha CL_eq CL_de CD_0 AR e CY_beta CY_dr ... 6 | Tmax Cl_beta Cl_p Cl_r Cl_da Cl_dr Cm_0 Cm_alpha Cm_eq Cm_de ... 7 | Cn_beta Cn_p Cn_r Cn_da Cn_dr m Ixx Iyy Izz 8 | 9 | rho = 1.225; 10 | g = 9.81; 11 | m = 0.4; %[kg] 12 | Ixx = 915.06*(1e-5); %kg*m2 13 | Iyy = 422.48*(1e-5); %kg*m2 14 | Izz = 1337.5*(1e-5); %kg*m2 15 | Ixy = -166.07*(1e-5); %kg*m2 16 | Ixz = -0.0001*(1e-5); %kg*m2 17 | Iyz = 0; 18 | 19 | S = 0.138; %[m^2] 20 | b = 1.22; %[m] 21 | c = 0.29; %[m] 22 | T_W_ratio = 1.2; 23 | Tmax = m * g * T_W_ratio; %we guess TMAX 33 % OF the mass 24 | 25 | % LONGITUDINAL 26 | 27 | CL_alpha = 4.44; 28 | CDalpha = 0.33; 29 | Cm_alpha = - 0.683; 30 | CLalpha_dot = 0; 31 | Cmalpha_dot = - 4.36; 32 | CLq = 3.8; 33 | Cmq = - 9.96; 34 | CL_de = 0.355; 35 | Cm_de = - 0.923; 36 | 37 | CL_eq = CLalpha_dot + CLq; 38 | Cm_eq = Cmalpha_dot + Cmq; 39 | 40 | % CL = 0.41; %pag 400 Aircraft control and simulation Brian L.Stevin 41 | % CD = 0.05; 42 | CL = 1; %pag 400 Aircraft control and simulation Brian L.Stevin 43 | CD = 0.1; 44 | % To inverse engineering some parameters 45 | % CL=CL0+CLalpha*alpha+CLde*de 46 | % Cm=Cm0+Cmalpha*alpha+Cmde*de 47 | % CD=CD0+CD_alpha*alpha; 48 | 49 | % x0=zeros(5,1) 50 | % x=fsolve(@equilibriumULTRALIGHT,x0) % if ran gives us line 40; 51 | % alpha=x(1),de=x(2),CL0=x(3),CD0=x(4),Cm0=x(5), 52 | %Cm0=0.1325,de=0.0053,CL0=0.5411,CD0=0.0363; 53 | alpha = 0.0889;de = - 0.0256; 54 | CL_0 = 0.0242; CD_0 = 0.0206; Cm_0 = 0.0371; %reversed engineered parameters 55 | %%LATERAL 56 | CY_beta = - .564; 57 | Cl_beta = - 0.074; 58 | Cn_beta = 0.071; 59 | Cl_p = - .41; 60 | Cn_p = - .0575; 61 | Cl_r = 0.107; 62 | Cn_r = - .125; 63 | Cl_da = 0.1342; % -0.134; %%TO BE CHECKED OUT 64 | Cn_da = - 0.00346; %%TO BE CHECKED OUT 65 | CY_dr = 0.157; %%TO BE CHECKED OUT 66 | Cl_dr = 0.118; %%TO BE CHECKED OUT 67 | Cn_dr = - .0717; %%TO BE CHECKED OUT 68 | 69 | %% 70 | AR = 6.04; e = 0.8; 71 | %% Store all the aircraft parameters 72 | PARAMETERS_AC = [g rho*8/8 c b S CL_0 CL_alpha CL_eq CL_de CD_0 AR e CY_beta CY_dr ... 73 | Tmax Cl_beta Cl_p Cl_r Cl_da Cl_dr Cm_0 Cm_alpha Cm_eq Cm_de ... 74 | Cn_beta Cn_p Cn_r Cn_da Cn_dr m Ixx Iyy Izz]; 75 | -------------------------------------------------------------------------------- /INIT_temp.m: -------------------------------------------------------------------------------- 1 | % Initialize data for trim and simulation. 2 | % This script setup the nominal parameter of the aircraft dynamics 3 | % state_dot = f(state,input) 4 | 5 | global g rho c b S CL_0 CL_alpha CL_eq CL_de CD_0 AR e CY_beta CY_dr ... 6 | Tmax Cl_beta Cl_p Cl_r Cl_da Cl_dr Cm_0 Cm_alpha Cm_eq Cm_de ... 7 | Cn_beta Cn_p Cn_r Cn_da Cn_dr m Ixx Iyy Izz 8 | 9 | rho = 1.225; 10 | g = 9.81; 11 | % m = 2750 * 0.453592; %[kg] 12 | m = 0.4; 13 | % Ixx = 1048 * 1.35581795; %[kg m^2] cannot use num2cell simulink 14 | % Iyy = 3000 * 1.35581795; %[kg m^2] 15 | % Izz = 3530 * 1.35581795; %[kg m^2] 16 | % Ixz = 0; %[kg m^2] 17 | % Iyz = 0; %[kg m^2] 18 | % Ixy = 0; %[kg m^2] 19 | Ixx = 915.06*1e-6; %kg*m2 20 | Iyy = 422.48*1e-6; %kg*m2 21 | Izz = 1337.5*1e-6; %kg*m2 22 | Ixy = -166.07*1e-6; %kg*m2 23 | Ixz = -0.0001*1e-6; %kg*m2 24 | Iyz = 0; 25 | % 26 | % S = 184 * 0.3048 ^ 2; %[m^2] 27 | % b = 33.4 * 0.3048; %[m] 28 | % c = 5.7 * 0.3048; %[m] 29 | S = 0.138; %[m^2] 30 | b = 1.22; %[m] 31 | c = 0.29; %[m] 32 | % T_W_ratio = 0.33; 33 | T_W_ratio = 1.5; 34 | Tmax = m * g * T_W_ratio; %we guess TMAX 33 % OF the mass 35 | 36 | % LONGITUDINAL 37 | 38 | CL_alpha = 4.44; 39 | CDalpha = 0.33; 40 | Cm_alpha = - 0.683; 41 | CLalpha_dot = 0; 42 | Cmalpha_dot = - 4.36; 43 | CLq = 3.8; 44 | Cmq = - 9.96; 45 | CL_de = 0.355; 46 | Cm_de = - 0.923; 47 | 48 | CL_eq = CLalpha_dot + CLq; 49 | Cm_eq = Cmalpha_dot + Cmq; 50 | 51 | CL = 0.41; %pag 400 Aircraft control and simulation Brian L.Stevin 52 | CD = 0.05; 53 | % To inverse engineering some parameters 54 | % CL=CL0+CLalpha*alpha+CLde*de 55 | % Cm=Cm0+Cmalpha*alpha+Cmde*de 56 | % CD=CD0+CD_alpha*alpha; 57 | 58 | % x0=zeros(5,1) 59 | % x=fsolve(@equilibriumULTRALIGHT,x0) % if ran gives us line 40; 60 | % alpha=x(1),de=x(2),CL0=x(3),CD0=x(4),Cm0=x(5), 61 | %Cm0=0.1325,de=0.0053,CL0=0.5411,CD0=0.0363; 62 | alpha = 0.0889;de = - 0.0256; 63 | CL_0 = 0.0242; CD_0 = 0.0206; Cm_0 = 0.0371; %reversed engineered parameters 64 | %%LATERAL 65 | CY_beta = - .564; 66 | Cl_beta = - 0.074; 67 | Cn_beta = 0.071; 68 | Cl_p = - .41; 69 | Cn_p = - .0575; 70 | Cl_r = 0.107; 71 | Cn_r = - .125; 72 | Cl_da = 0.1342; % -0.134; %%TO BE CHECKED OUT 73 | Cn_da = - 0.00346; %%TO BE CHECKED OUT 74 | CY_dr = 0.157; %%TO BE CHECKED OUT 75 | Cl_dr = 0.118; %%TO BE CHECKED OUT 76 | Cn_dr = - .0717; %%TO BE CHECKED OUT 77 | 78 | %% 79 | AR = 6.04; e = 0.8; 80 | %% Store all the aircraft parameters 81 | PARAMETERS_AC = [g rho*8/8 c b S CL_0 CL_alpha CL_eq CL_de CD_0 AR e CY_beta CY_dr ... 82 | Tmax Cl_beta Cl_p Cl_r Cl_da Cl_dr Cm_0 Cm_alpha Cm_eq Cm_de ... 83 | Cn_beta Cn_p Cn_r Cn_da Cn_dr m Ixx Iyy Izz]; 84 | -------------------------------------------------------------------------------- /LinkAll.m: -------------------------------------------------------------------------------- 1 | function LinkAll() 2 | % LinkAll() links all the figure in the x-axis 3 | FIG = get(0,'Children'); % Find all the figures automatically 4 | MAX_FIG = length(FIG); % Find max number of figures 5 | All_Axes = []; 6 | for kk = 1 : MAX_FIG 7 | N_Axes = length( FIG(kk).Children ); 8 | All_Axes_now =FIG(kk).Children'; % get all the axes of the kk figure 9 | All_Axes = [ All_Axes , All_Axes_now ]; % add all the axes all together 10 | end 11 | linkaxes(All_Axes,'x'); 12 | end 13 | -------------------------------------------------------------------------------- /MONTECARLO.m: -------------------------------------------------------------------------------- 1 | %% This script runs many simulations to check the robustness of the 2 | % L1 Adaptive control. It is divided in different parts: 3 | 4 | %%%%~~~~~~~~~~~~~~Last changes~~~~~~~~~~~~~~~~~~~~~~~%%%%% 5 | 6 | % modified longitudinal K of the adaptive control feedback in 7 | % TestNavionLong 8 | 9 | %%%%~~~~~~~~~~~~~~ Part 1 :Clean Up ~~~~~~~~~~~~~~~~~~~~~~~%%%%% 10 | 11 | tic, 12 | close all, 13 | clear , warning off 14 | restoredefaultpath % be sure to do not have something loaded with the same names 15 | 16 | %%%%~~~~~~~~~~~~~~ Part 2 : Run Nominal case ~~~~~~~~~~~~~~~~~~~~~~~%%%%% 17 | 18 | addpath( genpath( pwd )) % Add current Folder to the path, otherwise would not find some function and files 19 | MONTECARLO_EXE = 1; % Only simulate the Nominal parameter set if MONTECARLO_EXE = 0, otherwise execute the MONTECARLO simulations 20 | OUTPUT_DISTURBANCE = 0; % Add sinusoidal to input/output 21 | INPUT_DISTURBANCE = 0; % Add sinusoidal to input/output 3deg ==> 0.5 22 | VARIANCE = diag( [ones( 12 , 1 )] )*1e-9; % if you want to test it againt noisy measurements increase it adequately 1e-8 is low enough to be No Noise Case 23 | MAX_RUN = 2; % How many runs do you want to perform 24 | UncertantiesParameters = 25/100; % Parameter variation in percentage for all the dynamic parameters except gravity!!! 25 | InputCrossCoupling = 10/100; % CrossCoupling in the inputs, usually very small, it can be for example Thrust effect on the yaw/roll due to propeller torque effect 5% is an extreme value 26 | ThrustReduction = 20/100; % Max Thrust Reduction in percentage 27 | CmAlphaIncert = 0.8; % If == 0 takes the uncertanties of the parameters, otherwise custom one defined 28 | Ts = 0.01; % Controller frequency 100Hz=0.01s, Simulation step is Ts/10 (CTRL+E in Simulink file) 29 | 30 | % Set the simulation time Programmatically 31 | model_name = dir('*.slx'); 32 | model_name = model_name.name ; model_name = strrep( model_name , '.slx' , '' ); 33 | open(model_name) 34 | set_param( model_name , 'FixedStep' , 'Ts/10'); 35 | pause( 1 ) 36 | blockName = [ model_name , '/Reference Generator Variation/Step' ]; 37 | % DeltaSpeed = 10; 38 | DeltaSpeed = 5; 39 | 40 | % Set the delta Speed Programmatically 41 | set_param( blockName , 'after' , num2str( DeltaSpeed ) ); 42 | pause(1) 43 | save_system 44 | close_system 45 | 46 | % Run nominal case 47 | % V_trim = 30; % define trim speed 48 | V_trim = 40; % define trim speed 49 | % V_trim = 70; % define trim speed 50 | % target_speed = 70; % define target speed (not needed used only for debugging once) 51 | 52 | %%%%%%~~~~~~~~ Run Nominal Parameter simulation and setup of the L1AC control system :) !!! ~~~~~~~~~~~~~~~~~ 53 | SETUP 54 | % docknewfig 55 | % LinkAll 56 | %%%%~~~~~~~~~~~~~~ Part 3 : Run Montecarlo only if MONTECARLO_EXE == 1 ~~~~~~~~~~~~~~~~~~~~~~~%%%%% 57 | try 58 | if MONTECARLO_EXE==1 59 | ACTUATOR_NOMINAL = ACTUATOR_TRUE; % save nominal input history [de da dr dt] 60 | BODY_Variables_NOMINAL = BODY_Variables; % save nominal output history [u v w p q r phi theta psi x y z] 61 | 62 | pause(.1) 63 | warning off; % stop annoying warning error messages 64 | 65 | % max_figure = findall( 0 , 'type' , 'figure' ); % find all open figure 66 | % max_figure = max( size (max_figure) ); 67 | % 68 | % % print "nominal" in all the figures 69 | % for ii = 1 : max_figure 70 | % figure( ii ) 71 | % legend( 'NOMINAL' ) 72 | % end 73 | % 74 | % FIG = get( 0 , 'Children' ); % find all the figures and axes 75 | % set ( FIG , 'Visible' , 'off'); % set it invisible so we speed things up 76 | 77 | PARAMETERS_AC_old = PARAMETERS_AC; % old aircraft parameter, NOMINAL stored an on top add the parameter uncertanties 78 | omega_true = eye(4); % omega_NOMINAL 79 | kk = 0; % index of the failed parameter 80 | 81 | PARAMETRI_SIMULATI = zeros(33 , MAX_RUN); % Preallocate the parameter 82 | PARAMETRI_SIMULATI_FAIL_INDEX = zeros(10 , 1); 83 | 84 | % initialize the RUNs where the variable are stored 85 | 86 | ACTUATOR_RUNs( MAX_RUN ) = ACTUATOR_TRUE; 87 | BODY_Variables_RUNs( MAX_RUN ) = BODY_Variables; 88 | cc1 = waitbar( 0 / (MAX_RUN), ' simulation running ' ); % show a progressingbar saying the simulation is running 89 | 90 | for ii = 1 : MAX_RUN % Run simulation with different PARAMETERS_AC +-30% of the nominal , Cmalpha can be even unstable!! and it recovers 91 | 92 | omega_true = eye(4); 93 | % uncertainties in the Parameter describing the aircraft dynamic 94 | 95 | PARAMETERS_AC_MONTECARLO = PARAMETERS_AC_old + UncertantiesParameters * PARAMETERS_AC_old .*rand(size(PARAMETERS_AC)).*sign(randn(size(PARAMETERS_AC))); 96 | PARAMETERS_AC_MONTECARLO(1) = PARAMETERS_AC_old(1); % gravity g does not change 97 | PARAMETERS_AC_MONTECARLO(2) = 1.225 -1.5* 1.225 * UncertantiesParameters * rand(1); % density varies only in negative sense 98 | 99 | % commented: just here to explain some considerations related 100 | % to some past experiments 101 | % SIGN = 1; 102 | % PARAMETERS_AC_MONTECARLO(9) = rand(1).*SIGN; %CLde; the sign would change omega too much and goes outside the assumption of omega diagonally dominant and does not change its sign 103 | % PARAMETERS_AC_MONTECARLO(24) = rand(1)*SIGN; %Cmde 104 | 105 | if CmAlphaIncert ~= 0 106 | PARAMETERS_AC_MONTECARLO(22) = CmAlphaIncert * rand(1).*sign(randn(1)); % Cmalpha also unstable, otherwise as UncertantiesParameter perturbation as above 107 | end 108 | 109 | % Thrust Reduction 110 | 111 | PARAMETERS_AC_MONTECARLO(15) = abs(1-abs(ThrustReduction* ( 0 + 1 * rand(1) ) ) ) * Tmax; % maximum thrust available reduced up to 30% 112 | % CrossCoupling is added to de da dr --------dt influence not considered 113 | 114 | Omega_inputCrossCoupling = zeros(size(omega_true)); 115 | Omega_inputCrossCoupling(1:end-1,1:end-1) = randn(3,3) * InputCrossCoupling; 116 | % Omega_inputCrossCoupling(1:end-1,end) = InputCrossCoupling/10 * randn(size(Omega_inputCrossCoupling(1:end-1,end))); 117 | % Omega_inputCrossCoupling(end,1:end-1) = InputCrossCoupling/10 * randn(size(Omega_inputCrossCoupling(end,1:end-1))); 118 | % Omega_inputCrossCoupling(end,end) = randn(1,1) * InputCrossCoupling; 119 | % OMEGA_MONTECARLO = omega_true+randn(4,4)*InputCrossCoupling; % add cross coupling up to <> per control channel 120 | OMEGA_MONTECARLO = omega_true+Omega_inputCrossCoupling; % Add cross coupling to omega_true 121 | 122 | % correct/override the thrust to be at minimum equal to = 123 | % = T_W_ratio*(1-ThrustReduction)~ 70% MAX THRUST 124 | mass = PARAMETERS_AC_MONTECARLO(30); 125 | Tmax = mass * g * T_W_ratio; % TMAX T_W_ratio of the mass 126 | PARAMETERS_AC_MONTECARLO(15) = Tmax * (1 - ThrustReduction * abs(rand(1))); 127 | 128 | SETUP_MONTECARLO; % RUN the simulation simulation with the new data/parameter 129 | 130 | % ~~~~~~~~~~~~Store all the actuator and body history~~~~~~~~~~~~~~~~~% 131 | ACTUATOR_RUNs(ii) = ACTUATOR_TRUE; 132 | BODY_Variables_RUNs(ii) = BODY_Variables; 133 | PARAMETRI_SIMULATI(:,ii)=PARAMETERS_AC_MONTECARLO ; % store all the parameters 134 | 135 | waitbar(ii/MAX_RUN,cc1); % update the waitbar 136 | 137 | if max(time)<199 % if the simulation aborted store the parameter that triggered the singularities 138 | kk=kk+1 % add the counter 139 | PARAMETRI_SIMULATI_FAIL(:,kk) = PARAMETERS_AC_MONTECARLO; % store the failure one separately 140 | end 141 | end 142 | 143 | close(cc1); % close the waitbar 144 | 145 | %%%%~~~~~~~~~~~~~~ Part 4 : Save the simulations data ~~~~~~~~~~~~~~~~~~~~~~~%%%%% 146 | 147 | BODY_Variables_RUNs(MAX_RUN+1)=BODY_Variables_NOMINAL; % store at the end the body variable NOMINAL 148 | ACTUATOR_RUNs(MAX_RUN+1) = ACTUATOR_NOMINAL; 149 | PARAMETRI_SIMULATI(:,MAX_RUN+1) = PARAMETERS_AC_old; 150 | %% save all the time hystory in v7.3 because otherwise could crash!!! Do not know why 151 | NOME = datestr( now ) ; 152 | NOME = strrep( NOME , ':' , '_' ); 153 | save( NOME ,'ThrustReduction','InputCrossCoupling','UncertantiesParameters','BODY_Variables_RUNs','ACTUATOR_RUNs','PARAMETRI_SIMULATI','Reference_Altitude','CmAlphaIncert','-v7.3'); 154 | %% Clear and Re Load the latest file 155 | clear all , close all , clear force all % to clean everything and then reload what you have just saved 156 | warning off 157 | NOME = dir('*.mat*'); % load the latest most recent mat file 158 | NOME = NOME(end).name; 159 | load(NOME); 160 | 161 | %% %%~~~~~~~~~~~~~~ Part 5 : Plot all simulations data and parameters ~~~~~~~~~~~~~~~~~~~~~~~%%%%% 162 | 163 | % Label of all parameters 164 | LABEL_NEXT = {'g', 'rho', 'c', 'b', 'S', 'CL_0', 'CL_{alpha}', 'CL_{eq}', 'CL_{de}', 'CD_0', 'AR', 'e', 'CY_{beta}', 'CY_{dr}', ... 165 | 'T_{max}', 'Cl_{beta}', 'Cl_p', 'Cl_r', 'Cl_{da}', 'Cl_{dr}', 'Cm_0', 'Cm_{alpha}', 'Cm_{eq}', 'Cm_{de}' ... 166 | ,'Cn_{beta}', 'Cn_p', 'Cn_r' ,'Cn_{da}' ,'Cn_{dr}' ,'m', 'Ixx', 'Iyy', 'Izz' }; 167 | 168 | MAX_RUN = max( size ( BODY_Variables_RUNs )) - 1; % BODY_Variables_RUNs is a time series of 1x(MAX_RUN -1) variable run 169 | 170 | % PLOT_ALL % state and control variable 171 | % LinkAll % link all the figures axes so when you zoom in and out in x direction, you zoom all the figure with time as horizontal axes 172 | 173 | % Plot all the parameters !!! No need for link them all 174 | MAX_PARAM = 33; % Total number of parameters 175 | % for ii = 1:MAX_PARAM % for each parameter 176 | % 177 | % h = figure; % create new figure after the first plots 178 | % FIG = get(0,'Children'); % find all the figures at this moment 179 | % set(FIG,'Visible','off'); % plot in invisible mode 180 | % 181 | % for kk = 1:MAX_RUN+1 % for each simulation plot the "ii" parameter 182 | % plot( kk-1, PARAMETRI_SIMULATI( ii , kk ), '^' , 'LineWidth' , 2 ), hold on, grid on,grid minor 183 | % title (LABEL_NEXT{ii}) % set the title as the name of the parameter 184 | % end 185 | % set(h,'Name',LABEL_NEXT{ii},'NumberTitle','off') % change name of the figure itself not only the title!!! 186 | % title (LABEL_NEXT{ii}) 187 | % end 188 | % AddLegend(MAX_RUN) 189 | % clear 190 | % FIG = get(0,'Children'); % find all figure 191 | % set(FIG,'Visible','off'); % make all figure invisible 192 | % docknewfig 193 | % UnDockAll 194 | % FullScreen % set all figure full screen automatically and make them visible 195 | % pause(1) 196 | 197 | % PrintAll2PDF 198 | 199 | % RemoveLegend 200 | % docknewfig 201 | % SaveFig % save figs in eps format 202 | 203 | end 204 | end 205 | %% Print back the value used for the simulation 206 | % ThrustReduction 207 | % InputCrossCoupling 208 | % UncertantiesParameters 209 | % CmAlphaIncert 210 | % Finished: play music 211 | toc 212 | load handel % play aleluia song to wake me up 213 | sound( y , Fs ) 214 | -------------------------------------------------------------------------------- /NewFigureHide.m: -------------------------------------------------------------------------------- 1 | function NewFigureHide() 2 | h = figure; 3 | set(h, 'Visible', 'off'); 4 | end 5 | -------------------------------------------------------------------------------- /PLOTTING_SCRIPT.m: -------------------------------------------------------------------------------- 1 | % This script plot all the simulation data contained in BODY_Variables that is contained inside the simulink file 2 | time = BODY_Variables.Time; % take the Data 3 | if max(time)>0.5 % if has not crasched assign variable and plot 4 | u = BODY_Variables.Data(:,1); 5 | v = BODY_Variables.Data(:,2); 6 | w = BODY_Variables.Data(:,3); 7 | p = BODY_Variables.Data(:,4); 8 | q = BODY_Variables.Data(:,5); 9 | r = BODY_Variables.Data(:,6); 10 | phi = BODY_Variables.Data(:,7)*180/pi; 11 | theta = BODY_Variables.Data(:,8)*180/pi; 12 | psi = BODY_Variables.Data(:,9)*180/pi; 13 | x = BODY_Variables.Data(:,10); 14 | y = BODY_Variables.Data(:,11); 15 | h = BODY_Variables.Data(:,12); 16 | 17 | de = ACTUATOR_TRUE.Data(:,1)*180/pi; 18 | da = ACTUATOR_TRUE.Data(:,2)*180/pi; 19 | dr = ACTUATOR_TRUE.Data(:,3)*180/pi; 20 | dt = ACTUATOR_TRUE.Data(:,4); 21 | 22 | %% Data are saved in body coordinates 23 | V = sqrt(u.^2+v.^2+w.^2); 24 | alpha = atan(w./V)*180/pi; 25 | beta = asin(v./V)*180/pi; 26 | 27 | %% 28 | if ~exist('ETICHETTA') % first time it runs. later on is defined and created 29 | for kk = 1 : 15 30 | figure(kk); 31 | set(groot,'CurrentFigure', kk); 32 | end 33 | end 34 | 35 | % plot everything !!! 36 | ETICHETTA = {'V [m/sec]','\alpha [\circ]','\beta [\circ]','p [\circ/sec]','q[\circ/sec]','r[\circ/sec]','phi[\circ]','theta[\circ]','psi[\circ]','x [m]','y [m]','h [m]','de [\circ]','da [\circ]','dr [\circ]','dt [\circ]'}; 37 | kk = 1; 38 | set(groot,'CurrentFigure', kk); 39 | plot(time,BODY_Variables.Data(:,kk)); 40 | % plot(time,ones(size(time))*sqrt(sum(target_point(1:3)))), 41 | grid on,hold on 42 | xlabel('Time [sec]') 43 | ylabel(ETICHETTA(kk)) 44 | 45 | kk = 2; 46 | set(groot,'CurrentFigure', kk) 47 | plot(time,alpha),grid on,hold on 48 | xlabel('Time [sec]') 49 | ylabel(ETICHETTA(kk)) 50 | 51 | kk = 3; 52 | set(groot,'CurrentFigure', kk) 53 | plot(time,beta),grid on,hold on 54 | xlabel('Time [sec]') 55 | ylabel(ETICHETTA(kk)) 56 | 57 | for kk = 4:6 58 | 59 | set(groot,'CurrentFigure', kk) 60 | plot(time,BODY_Variables.Data(:,kk)*180/pi),grid on,hold on 61 | xlabel('Time [sec]') 62 | ylabel(ETICHETTA(kk)) 63 | 64 | end 65 | 66 | for kk = 7:9 67 | 68 | set(groot,'CurrentFigure', kk) 69 | plot(time,BODY_Variables.Data(:,kk)*180/pi),grid on,hold on 70 | xlabel('Time [sec]') 71 | ylabel(ETICHETTA(kk)) 72 | 73 | end 74 | 75 | for kk = 10:12 76 | 77 | set(groot,'CurrentFigure', kk) 78 | plot(time,BODY_Variables.Data(:,kk)),grid on,hold on 79 | xlabel('Time [sec]') 80 | ylabel(ETICHETTA(kk)) 81 | 82 | end 83 | end 84 | -------------------------------------------------------------------------------- /PLOTTING_SCRIPT2.m: -------------------------------------------------------------------------------- 1 | % This script plots V alpha beta p q r phi theta and psi, with 2 | % custom position to maximize the space and look 3 | % Plot everything in the same 3x3 Plot 4 | kk=13; 5 | set(groot, 'CurrentFigure', kk) 6 | 7 | 8 | pos1 = [0.04 0.71 0.27 0.25]; % [left bottom width height] 9 | subplot('Position',pos1) 10 | 11 | hold on,grid on 12 | plot(time,V) 13 | xlabel('Time [s]','FontSize', 14) 14 | ylabel('V [m/sec]','FontSize', 14) 15 | 16 | 17 | pos2 = [0.04 0.38 0.27 0.25]; % [left bottom width height] 18 | subplot('Position',pos2) 19 | hold on,grid on 20 | plot(time,alpha) 21 | xlabel('Time [s]','FontSize', 14) 22 | ylabel('\alpha [\circ]','FontSize', 14) 23 | 24 | pos3 = [0.04 0.06 0.27 0.25]; % [left bottom width height] 25 | subplot('Position',pos3) 26 | hold on,grid on 27 | plot(time,beta) 28 | xlabel('Time [s]','FontSize', 14) 29 | ylabel('\beta [\circ]','FontSize', 14) 30 | 31 | pos4 = [0.37 0.71 0.27 0.25]; % [left bottom width height] 32 | subplot('Position',pos4) 33 | hold on,grid on 34 | plot(time,p*180/pi) 35 | xlabel('Time [s]','FontSize', 14) 36 | ylabel('p [\circ/sec]','FontSize', 14) 37 | 38 | pos5 = [0.37 0.38 0.27 0.25]; % [left bottom width height] 39 | subplot('Position',pos5) 40 | hold on,grid on 41 | plot(time,q*180/pi) 42 | xlabel('Time [s]','FontSize', 14) 43 | ylabel('q [\circ/sec]','FontSize', 14) 44 | 45 | pos6 = [0.37 0.06 0.27 0.25]; % [left bottom width height] 46 | subplot('Position',pos6) 47 | hold on,grid on 48 | plot(time,r*180/pi) 49 | xlabel('Time [s]','FontSize', 14) 50 | ylabel('r [\circ/sec]','FontSize', 14) 51 | 52 | pos7 = [0.7 0.71 0.27 0.25]; % [left bottom width height] 53 | subplot('Position',pos7) 54 | hold on,grid on 55 | plot(time,phi) 56 | xlabel('Time [s]','FontSize', 14) 57 | ylabel('\phi [\circ]','FontSize', 14) 58 | 59 | pos8 = [0.7 0.38 0.27 0.25]; % [left bottom width height] 60 | subplot('Position',pos8) 61 | hold on,grid on 62 | plot(time,theta) 63 | xlabel('Time [s]','FontSize', 14) 64 | ylabel('\theta [\circ]','FontSize', 14) 65 | 66 | pos9 = [0.7 0.06 0.27 0.25]; % [left bottom width height] 67 | subplot('Position',pos9) 68 | hold on,grid on 69 | plot(time,psi) 70 | xlabel('Time [s]','FontSize', 14) 71 | ylabel('\psi [\circ]','FontSize', 14) 72 | -------------------------------------------------------------------------------- /PLOTTING_SCRIPT3.m: -------------------------------------------------------------------------------- 1 | % This script plots all the commands: 2 | % de : elevator 3 | % da : aileron 4 | % dr : rudder 5 | % dt : thrust 6 | 7 | % in figure 15 Plot all the input in the same windows [de da dr dt] 8 | kk = 15; 9 | set(groot, 'CurrentFigure', kk) 10 | pos1 = [0.04 0.75 .95 0.22]; % [left bottom width height] 11 | subplot('Position',pos1) 12 | hold on 13 | plot(time,de),grid on,hold on 14 | set(gca,'XTickLabel','') 15 | ylabel(ETICHETTA(end-3)) 16 | 17 | pos2 = [0.04 0.52 .95 0.2]; % [left bottom width height] 18 | subplot('Position',pos2) 19 | hold on 20 | plot(time,da),grid on,hold on 21 | set(gca,'XTickLabel','') 22 | ylabel(ETICHETTA(end-2)) 23 | 24 | pos3 = [0.04 0.29 .95 0.2]; % [left bottom width height] 25 | subplot('Position',pos3) 26 | hold on 27 | plot(time,dr),grid on,hold on 28 | set(gca,'XTickLabel','') 29 | ylabel(ETICHETTA(end-1)) 30 | 31 | pos4 = [0.04 0.06 .95 0.2]; % [left bottom width height] 32 | subplot('Position',pos4) 33 | xlabel('Time') 34 | plot(time,dt),grid on,hold on 35 | 36 | xlabel('Time [sec]') 37 | ylabel(ETICHETTA(end)) 38 | -------------------------------------------------------------------------------- /PLOTTING_SCRIPT4.m: -------------------------------------------------------------------------------- 1 | % This script plot in Fig 14 the Altitude vs Time 2 | set(groot, 'CurrentFigure', 14); 3 | hold on,grid on; 4 | plot(time,h); 5 | 6 | xlabel('Time [sec]'); 7 | ylabel('Altitude [m]'); 8 | -------------------------------------------------------------------------------- /PLOT_ALL.m: -------------------------------------------------------------------------------- 1 | %% Plot all the variable 2 | close all 3 | pause(1) 4 | fprintf(' \n ') 5 | set( 0 , 'DefaultFigureVisible', 'off'); 6 | 7 | set( groot , 'DefaultFigureVisible', 'off'); 8 | cc = waitbar( 1/(MAX_RUN+1) ,'Plotting everything'); 9 | 10 | for ll=1:MAX_RUN+1 11 | BODY_Variables = BODY_Variables_RUNs(ll); 12 | ACTUATOR_TRUE = ACTUATOR_RUNs(ll); 13 | PLOTTING_SCRIPT % plot everything in a different figure 14 | if max(time)>199 15 | PLOTTING_SCRIPT2 % plot time variable all in one figure [V alpha beta p q r phi theta psi] 16 | PLOTTING_SCRIPT3 % plot input 17 | PLOTTING_SCRIPT4 % plot altitude 18 | end 19 | waitbar(ll/(MAX_RUN+1),cc); 20 | end 21 | 22 | set(groot, 'CurrentFigure', 12); 23 | try 24 | plot(time,-Reference_Altitude.data,'k','Linewidth',2) %% Reference_Altitude = -h Ref 25 | end 26 | % AddLegend(MAX_RUN); 27 | Legenda = get(gca,'legend'); 28 | 29 | try 30 | LegendaNew = [Legenda.String ,'Reference']; 31 | legend(LegendaNew); 32 | end 33 | 34 | HideFig; 35 | close(cc) % close the waitbar 36 | -------------------------------------------------------------------------------- /POLE_PLACEMENT.m: -------------------------------------------------------------------------------- 1 | % this script execute the linearization of the model and the pole placement 2 | % design 3 | step_dxdu = 1e-9; 4 | 5 | % Take the Jacobian matrix derivative numercally (could be done symbolically for better precision) 6 | [ A , B ]= linearizza ( @f_NAVION , stato_body(1:12) , trim_input , step_dxdu); 7 | 8 | % sort out the matrix from the linearization 9 | [ A_LONG , A_LATERAL , B_LONG , B_LATERAL , A_LONG_aug , B_LONG_aug , A_LATERAL_aug , B_LATERAL_aug ] = estrai( A , B ); 10 | 11 | 12 | %% Computes the eigenvalues of A Longitudinal and Longitudinal 13 | ... augumented (augumented not used by L1) 14 | 15 | EIG_A_LONG=eig(A_LONG); 16 | EIG_A_LONG_aug=eig(A_LONG_aug); 17 | figure(1) 18 | subplot(2,3,1) 19 | plot(real(EIG_A_LONG_aug),imag(EIG_A_LONG_aug),'or','Linewidth',2);grid on,hold on 20 | plot(real(EIG_A_LONG),imag(EIG_A_LONG),'xgr','Linewidth',2); 21 | 22 | title('Longitudinal Dynamics') 23 | legend('A_{LONG_{aug}}','A_{LONG}') 24 | 25 | %% computes the eigenvalues of A lateral and lateral augumented 26 | 27 | EIG_A_LATERAL = eig(A_LATERAL); 28 | EIG_A_LATERAL_aug = eig(A_LATERAL_aug); 29 | 30 | subplot(2,3,4) 31 | plot(real(EIG_A_LATERAL_aug),imag(EIG_A_LATERAL_aug),'or','Linewidth',2); 32 | grid on,hold on 33 | plot(real(EIG_A_LATERAL),imag(EIG_A_LATERAL),'xgr','Linewidth',2); 34 | 35 | title('Lateral Dynamics') 36 | legend('A_{LAT_{aug}}','A_{LAT}') 37 | 38 | %% computes the gain matrices for longitudinal dynamics and lateral dynamic OK 39 | pole_TARGET_LONG = EIG_A_LONG + [-2.5 -2.5 -2.5 -2.5]'; % shift all the eigenvalue to the LHS 40 | K_LONG = place (A_LONG , B_LONG , pole_TARGET_LONG); 41 | 42 | subplot(2,3,2) 43 | plot ( real(EIG_A_LONG) , imag(EIG_A_LONG) ,' xgr' , 'Linewidth' , 2); 44 | grid on,hold on 45 | plot( eig (A_LONG - B_LONG*K_LONG), 'k+' , 'Linewidth' , 2); 46 | hold on , grid on 47 | legend('A_{LONG}','A_{LONG} K') 48 | title('Longitudinal Dynamics with state feedback') 49 | 50 | pole_TARGET_LAT = [-1.5 -1.2 -1.4 -1]'; %05/05/2018 Good also at V_trim = 40 Best tradeOff so far 51 | 52 | K_LAT = place(A_LATERAL,B_LATERAL,pole_TARGET_LAT); 53 | 54 | subplot(2,3,5) 55 | plot ( real ( EIG_A_LATERAL ) , 0 , 'xgr' , 'Linewidth' , 2); 56 | grid on , hold on 57 | plot( eig ( A_LATERAL - B_LATERAL * K_LAT ),0, 'k+' , 'Linewidth' , 2); 58 | hold on , grid on 59 | legend('A_{LAT}','A_{LAT} K') 60 | title('Lateral Dynamics with state feedback') 61 | % h = figure(1); 62 | 63 | % fprintf('Closing Window ...'), 64 | % pause(1),fprintf('...'), 65 | % pause(1),fprintf('... \n') 66 | % pause(1) 67 | %close(h) 68 | clc 69 | -------------------------------------------------------------------------------- /PrintAll2PDF.m: -------------------------------------------------------------------------------- 1 | function [] = PrintAll2PDF() 2 | %PRINTALL2PDF Summary of this function goes here 3 | % Saves all figures in a PDF 4 | PrevFolder = pwd; 5 | if isempty(dir('Immagini')) 6 | mkdir('Immagini') 7 | cd('Immagini') 8 | else 9 | cd('Immagini') 10 | end 11 | NOMI = dir('*.pdf') 12 | if ~isempty(NOMI) 13 | delete(NOMI.name); 14 | end 15 | FullScreen 16 | FIG = get(0,'children'); 17 | 18 | NmaxFig = length(FIG); 19 | 20 | for kk = 1:NmaxFig 21 | h = figure(FIG(kk)) ; 22 | NOME = h.Name; 23 | set(h,'PaperOrientation','landscape'); 24 | set(h,'PaperUnits','normalized'); 25 | set(h,'PaperPosition', [0 0 1 1]); 26 | 27 | print (NOME,'-dpdf','-painters', '-r1600') 28 | end 29 | cd(PrevFolder); 30 | end 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LIAC-Flight-Control-System 2 | This is a projet of ornithopter flight control system used L1 adaptive control, designed by Matlab/Simulink.This work based on https://github.com/LozioAlce/L1_AC. 3 | -------------------------------------------------------------------------------- /RemoveLegend.m: -------------------------------------------------------------------------------- 1 | function []=RemoveLegend() 2 | 3 | FIG = get( 0 , 'Children' ); 4 | max_kk = max(size( FIG )); 5 | 6 | for kk =1 :max_kk 7 | h = figure( kk ); 8 | set( h , 'visible' , 'off' ); 9 | legend( gca , 'off' ); 10 | end 11 | 12 | end 13 | -------------------------------------------------------------------------------- /SETUP.m: -------------------------------------------------------------------------------- 1 | % This script runs everything is needed to setup the A/C and Controls 2 | clc 3 | 4 | INIT % set parameters of the A/C: Navion 5 | 6 | % Define some condition where you want the a/c to be trimmed at (V_trim) 7 | alpha_trim = 0; 8 | beta_trim = 0; 9 | 10 | TRIM % trim the aircraft 11 | %% Define the control laws: Lateral and Longitudinal separately 12 | POLE_PLACEMENT % define the nominal closed loop by pole placement 13 | 14 | TEST_NAVION_LONG % AC LONGITUDINAL L1 AC design 15 | TEST_NAVION_LAT % AC LATERAL L1 AC design 16 | 17 | %% Setup some initial condition X0 and target conditions 18 | x0 = stato_trim_body; %% Initial condition = Trim Conditions 19 | x0(12) = 10; %% initial Altitude 20 | x0(11) = 0; %% initial East Component (position) 21 | x0(9)=-0.0*pi/180; %% Initial Psi 22 | 23 | target_point = stato_trim_body; %% where we want to trim the A/C (can be different than x0) 24 | target_point(12) = 0; %% altitude we want to reach 25 | target_point(9) = -45*pi/180; %% Psi Target: target coincides with psi of the reference path 26 | try 27 | target_point(1) = target_speed; % If it is defined elsewhere , inside MONTECARLO.m use that one otherwise go on with default/ as initial/trim condition to reach (see the simulink model) 28 | end 29 | 30 | %% 31 | %close all 32 | Ts_delay = 1*Ts; %Ts = 0.01 Nominally .It used in simulation in ... 33 | ... the controller Zero-Order-Hold to represent delay 34 | 35 | %% NOMINAL CASE 36 | 37 | omega_true = eye(4); % can add for example mismatches representing crosscoupling +0.05.*rand(4,4) 38 | PARAMETERS_AC(2) = rho*7/8; % nominal density 7/8 1.225 39 | PARAMETERS_AC(2) = rho*8/8; % test 1st may 40 | PARAMETERS_AC_old = PARAMETERS_AC; 41 | 42 | sim(model_name) %Name of the file that run the simulation 43 | hold on 44 | 45 | PLOTTING_SCRIPT % plot state variable 46 | PLOTTING_SCRIPT2 % plot state variable in a custom subplot 47 | PLOTTING_SCRIPT3 % plot input history 48 | PLOTTING_SCRIPT4 49 | 50 | % figure(14) , hold on , grid on 51 | % plot( time , h ) % plot altitude vs time 52 | -------------------------------------------------------------------------------- /SETUP_MONTECARLO.m: -------------------------------------------------------------------------------- 1 | %% Robustness to parameter uncertanties 2 | 3 | PARAMETERS_AC = PARAMETERS_AC_MONTECARLO; % to re use the other script 4 | omega_true = OMEGA_MONTECARLO; 5 | 6 | sim (model_name) 7 | hold on 8 | -------------------------------------------------------------------------------- /SVM.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/SVM.fig -------------------------------------------------------------------------------- /SVM1.emf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/SVM1.emf -------------------------------------------------------------------------------- /SVM2.emf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/SVM2.emf -------------------------------------------------------------------------------- /SVM2.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/SVM2.fig -------------------------------------------------------------------------------- /SVM3.emf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/SVM3.emf -------------------------------------------------------------------------------- /SVM3.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/SVM3.fig -------------------------------------------------------------------------------- /SVM_8_9.m: -------------------------------------------------------------------------------- 1 | target_point_all=[target_point_8_SUCCESS;target_point_7_SUCCESS]; 2 | target_point_all2=[target_point_8_FAIL;target_point_7_FAIL]; 3 | target_point_all=double([target_point_all,target_point_all2]'); 4 | Y=double([linspace(1,1,length(target_point_8_SUCCESS)),linspace(0,0,length(target_point_8_FAIL))]'); 5 | close all; 6 | % figure; 7 | % subplot(1,2,1); 8 | % plot(target_point_all(1:length(target_point_8_SUCCESS),1),target_point_all(1:length(target_point_8_SUCCESS),2),'m+');hold on; 9 | % plot(target_point_all(length(target_point_8_SUCCESS):end,1),target_point_all(length(target_point_8_SUCCESS):end,2),'c*'); 10 | % subplot(1,2,2); 11 | % model = svmtrain(Y(1:400),target_point_all(1:400,:),'-s 0 -c 10 -t 1 -g 1 -r 1 -d 3 showplot'); 12 | % [prelabel,accuracy,decision_values]=svmpredict(Y(401:500),target_point_all(401:500,:),model); 13 | % C = svmclassify(model,target_point_all(401:500,:),'showplot',true); 14 | trt = 0.8; 15 | target_point_all_train=target_point_all(1:trt*length(Y),:); 16 | Y_train=Y(1:trt*length(Y),:); 17 | target_point_all_test=target_point_all(trt*length(Y)+1:end,:); 18 | Y_test=Y(trt*length(Y)+1:end,:); 19 | 20 | SVMModel=fitcsvm(target_point_all_train,Y_train,'KernelFunction','rbf','OptimizeHyperparameters',{'BoxConstraint','KernelScale'}, 'HyperparameterOptimizationOptions',struct('ShowPlots',true)); 21 | dd=0.02; 22 | [x1Grid,x2Grid]=meshgrid(min(target_point_all(:,1)):dd:max(target_point_all(:,1)),min(target_point_all(:,2)):dd:max(target_point_all(:,2))); 23 | xGrid = [x1Grid(:),x2Grid(:)]; 24 | [~,scores] = predict(SVMModel,xGrid); 25 | sv = SVMModel.SupportVectors; 26 | % figure 27 | % gscatter(target_point_all(:,1),target_point_all(:,2),Y) 28 | % hold on 29 | % plot(sv(:,1),sv(:,2),'ko','MarkerSize',10); 30 | % % predict(SVMModel,target_point_all(401:end,:)); 31 | figure; 32 | h(1:2) = gscatter(target_point_all(:,1),target_point_all(:,2),Y,'rb','.'); 33 | hold on 34 | h(3) = plot(sv(:,1),sv(:,2),'ko'); 35 | contour(x1Grid,x2Grid,reshape(scores(:,2),size(x1Grid)),[0 0],'k'); -------------------------------------------------------------------------------- /SaveFig.m: -------------------------------------------------------------------------------- 1 | function SaveFig() 2 | % SaveFig() save figure in eps format for quality printing in Latex 3 | FIG = get( 0 , 'Children' ); 4 | for kk = max ( size (FIG) ): -1 : 1 5 | % IF you want full screen when printing 6 | % POS = get(FIG(kk),'paperposition'); 7 | % set ( FIG(kk),'paperposition',[0 0 POS(3:4)],'Papersize',[POS(3:4)]),pause(.1) 8 | % pause(1) 9 | h = figure(FIG(kk)) 10 | legend(gca,'off') 11 | print -depsc2 -painters -r1600 %-loose 12 | 13 | end 14 | end 15 | -------------------------------------------------------------------------------- /TEST_NAVION_LAT.m: -------------------------------------------------------------------------------- 1 | %% This script implements the L1 adaptive controller main features for the Longitudinal Dynamics 2 | 3 | Am_LAT = A_LATERAL-B_LATERAL*K_LAT; % [v p r phi]_dot=A_LONG*[u w q theta]' 4 | Bm_LAT=B_LATERAL; 5 | figure; 6 | plot(eig(Am_LAT),'*r') 7 | hold on, grid on 8 | plot(eig(A_LATERAL),'*') 9 | 10 | C_LAT = [1 0 0 0 11 | 0 0 0 1]; 12 | %% 13 | x0_new_LAT = zeros( 1 , max(size(Am_LAT))); 14 | 15 | omega0 = eye(2); 16 | s = tf('s'); 17 | D = 1/(.15*s+1); % May 2018 Test ok tuned As the Longitudinal 18 | 19 | D = D*eye(2); 20 | 21 | % construct the unmatched matrix 22 | Bum_LAT = [ 0 -Bm_LAT(3,1) Bm_LAT(2,1) Bm_LAT(2,1) ;0 -Bm_LAT(3,1) Bm_LAT(2,1) -Bm_LAT(2,1)]'; % not control channel 23 | Bum_LAT'*Bm_LAT == 0; % just to check it out 24 | 25 | B_LAT = [Bm_LAT Bum_LAT]; 26 | 27 | rank([Bum_LAT Bm_LAT]); %check the full rank 28 | Kg_LAT = -(C_LAT*Am_LAT^-1*Bm_LAT)^-1; % the matrix from reference to input ('kindof dynamic inversion') 29 | 30 | K_const_LAT = 2.5*eye(2); % Test 31 | 32 | Hm = C_LAT*(s*eye(size(Am_LAT))-Am_LAT)^-1*Bm_LAT; 33 | Hum = C_LAT*(s*eye(size(Am_LAT))-Am_LAT)^-1*Bum_LAT; 34 | 35 | TF_eta2m = D*(Hm\Hum); 36 | 37 | Ksp_LAT = -eye(size(Am_LAT))*0; % Speedup the estimation of the state 38 | x0hat_LAT = x0_new_LAT; 39 | 40 | % Computes the controller in a statespace representation 41 | sistemaLateral = ss(TF_eta2m,'minimal'); 42 | DiscretesistemaLateral= c2d(sistemaLateral,Ts); 43 | 44 | % Computes the lowpass filter in a statespace representation 45 | D_Lateral = ss(D,'minimal'); 46 | DiscreteD_Lateral = c2d(D_Lateral ,Ts); 47 | -------------------------------------------------------------------------------- /TEST_NAVION_LONG.m: -------------------------------------------------------------------------------- 1 | %% This script load the matrices of the navion and setup what is neeeded for the simulink ... 2 | % ... take the structure from the example of pag 170 that is working 3 | 4 | Am = A_LONG-B_LONG*K_LONG; % [u w q theta]_dot=A_LONG*[u w q theta]' 5 | Bm=B_LONG; 6 | 7 | C = [1 0 0 0 8 | 0 0 0 1]; 9 | 10 | x0_new = zeros(1,max(size(Am))); % Assigned to the start status of the estimator 11 | 12 | omega0 = eye(2); % nominal omega matrix in x_dot = Ax+Bm(omega u +d)+Bum u 13 | s = tf('s'); 14 | 15 | D = 1/(.25*s+1); % May 2018 Test ok tuned 16 | 17 | D = D*eye(2); 18 | % construct the unmatched matrix 19 | Bum = [ 0 -Bm(3,1) Bm(2,1) Bm(2,1) ;0 -Bm(3,1) Bm(2,1) -Bm(2,1)]'; % not control channel 20 | Bum'*Bm ==0; % chech it is zero. Necessary condition 21 | 22 | B = [Bm Bum]; % rebuild the control matrix 23 | 24 | rank([Bum Bm]); % check is full rank 25 | Kg = -(C*Am^-1*Bm)^-1; % the matrix from reference to input ('kindof dynamic inversion') 26 | 27 | K =2*eye(2); % Test May2018 06 28 | 29 | Hm = C*(s*eye(size(Am))-Am)^-1*Bm; 30 | Hum = C*(s*eye(size(Am))-Am)^-1*Bum; 31 | 32 | TF_eta2m = D*(Hm\Hum); 33 | 34 | Ksp = -eye(size(Am))*0; %% No state predictor additional matrix (no reason behind) works well 35 | x0hat = x0_new; %% starting state of the state predictor 36 | 37 | %%~~~~~~~~~ Trasform in discrete state space form because is more compact than 38 | %~~~~~~~~~~ trasfer function and can be handled easier 39 | sistemaLongitudinal = ss( TF_eta2m , 'minimal' ); 40 | DiscretesistemaLongitudinal= c2d( sistemaLongitudinal , Ts ); 41 | 42 | D_Longitudinal = ss( D , 'minimal' ); 43 | DiscreteD_Longitudinal = c2d( D_Longitudinal , Ts); 44 | -------------------------------------------------------------------------------- /TRIM.m: -------------------------------------------------------------------------------- 1 | % TRIM the f_NAVION model that is described in body axis. 2 | % The script computes the inputs needed to trim the aircraft in a given condition that can be 3 | % given in a flexible way: can decide which state/variable is free and which has to be equal to 4 | % a given value 5 | if ~exist( 'V_trim' ) 6 | V_trim = input( 'insert V to trim the a/c \n' ) 7 | else 8 | fprintf( 'Trim velocity already present is = %d \n' , V_trim ) 9 | end 10 | if ~exist('alpha_trim') 11 | alpha_trim = input( 'insert alpha to trim the a/c \n Remember to modify the Aeq(2,2) to 1' ) 12 | end 13 | if ~exist('beta_trim') 14 | beta_trim = input( 'insert beta to trim the a/c \n' ) 15 | end 16 | 17 | % is not anymore needed because eventhough the f_NAVION is in body 18 | % coordinate, the cost function takes wind coordinates 19 | % [u_trim,v_trim,w_trim] = wind2body(V_trim,alpha_trim,beta_trim); 20 | 21 | phi_trim = 0; 22 | theta_trim = 0; 23 | psi_trim = 0; 24 | 25 | Aeq = zeros(16,16); % 0 = do not select; 1 = select; 26 | 27 | for i = 4 : 6 %set p q r as "controlled variables" that have to be equal to a given value 28 | Aeq ( i,i ) = 1; 29 | end 30 | 31 | Aeq(12,12) = 0; % select height 32 | Aeq(1,1) = 1; % select V 33 | Aeq(2,2) = 0; % select alpha (AoA=Angle of attack is free!!!, it automatically comes out of the calculation) 34 | Aeq(3,3) = 1; % select beta (we want to fly with beta=0) 35 | Aeq(7,7) = 1; % select phi if you wish (leveled flight = 1, other cases =0, coordinated turn for example we can impose yaw rate) 36 | Aeq(9,9) = 1; % select psi if you wish, but it actually does not really matter, obviously 37 | 38 | h = 0; % trim at zero Altitude 39 | 40 | %~~~~ select where you wwant to trim at ~~~~~% 41 | beq = [V_trim alpha_trim beta_trim 0 0 0 ... 42 | phi_trim theta_trim psi_trim 0 0 h 0 0 0 0]'; % the last 4 are for the inputs that are FREE 43 | 44 | %~~~~~ Some setting proved to work out nicely ~~~~~~% 45 | 46 | options = optimoptions(@fmincon,'MaxFunEvals',100000); 47 | options.ConstraintTolerance = 1e-12; 48 | options.StepTolerance = 1e-12; 49 | options.MaxIterations = 1000000; 50 | options.Algorithm = 'interior-point'; 51 | options.UseParallel = 0; 52 | options.Display = 'none'; 53 | x0 = zeros( 16 , 1); %16 decision variable = 12 state + 4 Input 54 | x0( 1 ) = V_trim ; x( 16 ) = .3; % x0(16) is the Thrust from 0-1 55 | x0( 12 ) = 0; 56 | 57 | %~~~~~~~~~~~ Apply the minimization of the cost function to find the state and necessary inputs ~~~~~~~~~~~~~~~~~% 58 | [ stato ] = fmincon ('funzione_di_costo', x0 , [] , [] , Aeq , beq , [] , [] , [] , options); 59 | 60 | % Sort things out: State and input after trimming 61 | [ u , v , w ] = wind2body( stato(1) , stato(2) ,stato(3)); 62 | stato_body = [ u , v , w , stato(4:end-4)']'; 63 | stato_trim_body = stato_body; 64 | trim_input = stato(end-3:end); % take the last 4 from the decision variable stato(12+4) 65 | -------------------------------------------------------------------------------- /UnDockAll.m: -------------------------------------------------------------------------------- 1 | function UnDockAll() 2 | %DOCKNEWFIG Summary of this function goes here 3 | % Detailed explanation goes here 4 | FIG = get( 0,'Children'); 5 | 6 | set( FIG , 'WindowStyle' , 'Normal') 7 | 8 | for kk = max( size(FIG) ):-1:1 9 | figure( FIG(kk) ) 10 | pause(.01) 11 | hold on 12 | end 13 | 14 | end 15 | -------------------------------------------------------------------------------- /docknewfig.m: -------------------------------------------------------------------------------- 1 | function docknewfig() 2 | % DOCKNEWFIG Docks the figure in one single box 3 | 4 | FIG = get(0,'Children'); 5 | 6 | set(FIG,'WindowStyle','Docked'); 7 | 8 | for kk = max(size(FIG)):-1:1 9 | figure(FIG(kk)); 10 | pause(.01); 11 | hold on; 12 | end 13 | 14 | end 15 | -------------------------------------------------------------------------------- /estrai.m: -------------------------------------------------------------------------------- 1 | function [A_LONG , A_LATERAL , B_LONG , B_LATERAL , A_LONG_aug , B_LONG_aug , A_LATERAL_aug , B_LATERAL_aug] = estrai(A , B) 2 | % Extract the A and B matrix for longitudinal and lateral dynamic 3 | % [A_LONG,A_LATERAL,B_LONG,B_LATERAL,A_LONG_aug,B_LONG_aug,A_LATERAL_aug,B_LATERAL_aug]=estrai(A,B) 4 | % 5 | % Longitudinal dynamic : 6 | % [u w q theta]_dot = A_LONG*[u w q theta]' + B_LONG * [de dt]' 7 | % 8 | % Lateral dynamics: 9 | % [v p r phi]_dot = A_LATERAL*[v p r phi]' + B_LATERAL * [da dr]' 10 | % 11 | % Augumented Longitudinal dynamic : 12 | % [u w q theta h]_dot = A_LONG_aug*[u w q theta]' + B_LONG_aug * [de dt]' 13 | % 14 | % Augumented Lateral dynamics: 15 | % [v p r phi psi y]_dot=A_LATERAL_aug*[v p r phi psi y] + B_LATERAL_aug * [da dr]' 16 | 17 | % LONGITUINAL dynamic% 18 | A_LONG = [A(1,1) A(1,3) A(1,5) A(1,8) % [u w q theta]_dot=A_LONG*[u w q theta]' 19 | A(3,1) A(3,3) A(3,5) A(3,8) 20 | A(5,1) A(5,3) A(5,5) A(5,8) 21 | A(8,1) A(8,3) A(8,5) A(8,8)]; 22 | 23 | B_LONG = [B(1,1) B(1,4) % [de dt] 24 | B(3,1) B(3,4) 25 | B(5,1) B(5,4) 26 | B(8,1) B(8,4)]; 27 | 28 | % LATERAL DIRECTIONAL dynamic 29 | A_LATERAL = [A(2,2) A(2,4) A(2,6) A(2,7) % [v p r phi]_dot=A_LATERAL*[v p r phi]' 30 | A(4,2) A(4,4) A(4,6) A(4,7) 31 | A(6,2) A(6,4) A(6,6) A(6,7) 32 | A(7,2) A(7,4) A(7,6) A(7,7)]; 33 | 34 | B_LATERAL = [B(2,2) B(2,3) %[da dr] 35 | B(4,2) B(4,3) 36 | B(6,2) B(6,3) 37 | B(7,2) B(7,3)]; % has been changed from [B(8,2) B(8,3)] may be a misprint 38 | 39 | % Augumented Longitudinal dynamic : 40 | A_LONG_aug = [A(1,1) A(1,3) A(1,5) A(1,8) A(1,12) % [u w q theta h]_dot=A_LONG*[u w q theta h]' 41 | A(3,1) A(3,3) A(3,5) A(3,8) A(3,12) 42 | A(5,1) A(5,3) A(5,5) A(5,8) A(5,12) 43 | A(8,1) A(8,3) A(8,5) A(8,8) A(8,12) 44 | A(12,1) A(12,3) A(12,5) A(12,8) A(12,12)]; 45 | 46 | B_LONG_aug = [B(1,1) B(1,4) 47 | B(3,1) B(3,4) 48 | B(5,1) B(5,4) 49 | B(8,1) B(8,4) 50 | B(12,1) B(12,4)]; 51 | 52 | % Augumented Lateral dynamics: 53 | A_LATERAL_aug = [A(2,2) A(2,4) A(2,6) A(2,7) A(2,9) A(2,11) % [v p r phi psi y]_dot=A_LATERAL*[v p r phi psi y]' 54 | A(4,2) A(4,4) A(4,6) A(4,7) A(4,9) A(4,11) 55 | A(6,2) A(6,4) A(6,6) A(6,7) A(6,9) A(6,11) 56 | A(7,2) A(7,4) A(7,6) A(7,7) A(7,9) A(7,11) 57 | A(9,2) A(9,4) A(9,6) A(9,7) A(9,9) A(9,11) 58 | A(11,2) A(11,4) A(11,6) A(11,7) A(11,9) A(11,11) ]; 59 | 60 | B_LATERAL_aug = [B(2,2) B(2,3) 61 | B(4,2) B(4,3) 62 | B(6,2) B(6,3) 63 | B(7,2) B(7,3) 64 | B(9,2) B(9,3) 65 | B(11,2) B(11,3)]; 66 | 67 | end 68 | -------------------------------------------------------------------------------- /f_NAVION.m: -------------------------------------------------------------------------------- 1 | function dx = f_NAVION(state,input) 2 | u = state(1); 3 | v = state(2); 4 | w = state(3); 5 | p = state(4); 6 | q = state(5); 7 | r = state(6); 8 | phi = state(7); 9 | theta = state(8); 10 | psi = state(9); 11 | xg = state(10); 12 | yg = state(11); 13 | h = state(12); 14 | 15 | de = input(1); 16 | da = input(2); 17 | dr = input(3); 18 | dt = input(4); 19 | %% Parameters AR = 6.04 e = 0.7 to implement into the setup 20 | global g rho c b S CL_0 CL_alpha CL_eq CL_de CD_0 AR e CY_beta CY_dr ... 21 | Tmax Cl_beta Cl_p Cl_r Cl_da Cl_dr Cm_0 Cm_alpha Cm_eq Cm_de ... 22 | Cn_beta Cn_p Cn_r Cn_da Cn_dr m Ixx Iyy Izz 23 | %% From body to wind for forces and moments 24 | V = sqrt( u^2 + v^2 + w^2); 25 | alpha = atan( w/u ); 26 | beta = asin( v/V ); 27 | %% CL CD and CY construction 28 | CL = CL_0 + CL_alpha * alpha + CL_eq*( c/(2*V) ) * q + CL_de * de; 29 | CD = CD_0 + 0.1*(CL^2)/(pi*AR*e); 30 | CY = 0.01+(CY_beta*beta+CY_dr*dr)*0.01; 31 | %% Forces 32 | Lift = 1/2*rho *V^2*S*CL; 33 | Drag = 1/2*rho *V^2*S*CD; 34 | Side_Force = 1/2*rho *V^2*S*CY; 35 | Thrust = dt*Tmax; 36 | delta_engine = 0; % angle in radiance of the thrust vector respect the longitudinal body axis 37 | %% Thrust Force component calculation 38 | X_Thrust = Thrust * cos(delta_engine); 39 | Z_Thrust= Thrust * sin(delta_engine); 40 | Y_Thrust= 0; 41 | Cl_alpha_beta= 0.1; 42 | Cm_beta=-0.1; 43 | %% Cl Cp Cn - Moments Coefficients 44 | Cl = Cl_beta*beta+Cl_alpha_beta*alpha*beta+(Cl_p*(b/(2*V))*p+Cl_r *(b/(2*V))*r + Cl_da*da+Cl_dr*dr)*0.1; 45 | Cm = Cm_0+Cm_alpha*alpha+Cm_beta*beta+(Cm_eq*(c/(2*V))*q+Cm_de*de)*0.1; 46 | Cn = Cn_beta*beta + (Cn_p *(b/(2*V))*p +Cn_r * (b/(2*V))*r + Cn_da*da+Cn_dr*dr)*0.1; 47 | %% Moments 48 | ROLL_MOMENT = 1/2*rho*S*b*Cl*V^2; 49 | PITCH_MOMENT = 1/2*rho*S*c*Cm*V^2; 50 | YAW_MOMENT = 1/2*rho*S*b*Cn*V^2; 51 | %% Differential Equations 52 | u_dot = (X_Thrust-g*m*sin(theta)-Drag*cos(alpha)+Lift*sin(alpha))/m + r*V*sin(beta)-q*V*sin(alpha)*cos(beta); 53 | v_dot = (Y_Thrust+g*m*sin(phi)*cos(theta)+Side_Force)/m + p*V*sin(alpha)*cos(beta)-r*V*cos(alpha)*cos(beta); 54 | w_dot = (Z_Thrust+g*m*cos(phi)*cos(theta)-Drag*sin(alpha)-Lift*cos(alpha))/m + q*V*cos(alpha)*cos(beta)-p*V*sin(beta); 55 | 56 | p_dot = (ROLL_MOMENT + (Iyy-Izz) *q*r)/Ixx; 57 | q_dot = (PITCH_MOMENT + (Ixx-Izz) *p*r)/Iyy; 58 | r_dot = (YAW_MOMENT+(Ixx-Iyy)*p*q)/Izz; 59 | 60 | phi_dot = p+q*sin(phi)*tan(theta)+r*cos(phi)*tan(theta); 61 | theta_dot = q*cos(phi)-r*sin(phi); 62 | psi_dot = q*sin(phi)*sec(theta) + r*cos(phi)*sec(theta); 63 | 64 | xg_dot = u*cos(psi)*cos(theta)+v*(cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi))+w*(cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi)); 65 | yg_dot = u*sin(psi)*cos(theta)+v*(sin(psi)*sin(theta)*sin(phi)+cos(psi)*cos(phi))+w*(sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi)); 66 | h_dot = u*sin(theta)-v*cos(theta)*sin(phi)-w*cos(theta)*cos(phi); 67 | %% dx_dt output 68 | 69 | dx = [u_dot v_dot w_dot p_dot q_dot r_dot phi_dot theta_dot psi_dot xg_dot yg_dot h_dot]'; 70 | 71 | end 72 | -------------------------------------------------------------------------------- /f_NAVION_TEMP.m: -------------------------------------------------------------------------------- 1 | function dx = f_NAVION(state,input) 2 | u = state(1); 3 | v = state(2); 4 | w = state(3); 5 | p = state(4); 6 | q = state(5); 7 | r = state(6); 8 | phi = state(7); 9 | theta = state(8); 10 | psi = state(9); 11 | xg = state(10); 12 | yg = state(11); 13 | h = state(12); 14 | 15 | de = input(1); 16 | da = input(2); 17 | dr = input(3); 18 | dt = input(4); 19 | %% Parameters AR = 6.04 e = 0.7 to implement into the setup 20 | global g rho c b S CL_0 CL_alpha CL_eq CL_de CD_0 AR e CY_beta CY_dr ... 21 | Tmax Cl_beta Cl_p Cl_r Cl_da Cl_dr Cm_0 Cm_alpha Cm_eq Cm_de ... 22 | Cn_beta Cn_p Cn_r Cn_da Cn_dr m Ixx Iyy Izz 23 | %% From body to wind for forces and moments 24 | V = sqrt( u^2 + v^2 + w^2); 25 | alpha = atan( w/u ); 26 | beta = asin( v/V ); 27 | %% CL CD and CY construction 28 | CL = CL_0 + (CL_alpha * alpha + CL_eq*( c/(2*V) ) * q + CL_de * de)*0.01; 29 | CD = CD_0 + ((CL^2)/(pi*AR*e))*0.1; 30 | CY = 0.5+(CY_beta*beta+CY_dr*dr); 31 | %% Forces 32 | Lift = 1/2*rho *V^2*S*CL; 33 | Drag = 1/2*rho *V^2*S*CD; 34 | Side_Force = 1/2*rho *V^2*S*CY; 35 | Thrust = dt*Tmax; 36 | delta_engine = 0; % angle in radiance of the thrust vector respect the longitudinal body axis 37 | %% Thrust Force component calculation 38 | X_Thrust = Thrust * cos(delta_engine); 39 | Z_Thrust= Thrust * sin(delta_engine); 40 | Y_Thrust= 0; 41 | Cl_alpha_beta= 0.1; 42 | Cm_beta=-0.1; 43 | %% Cl Cp Cn - Moments Coefficients 44 | Cl =Cl_beta*beta+ Cl_alpha_beta*alpha*beta+(Cl_p*(b/(2*V))*p+Cl_r *(b/(2*V))*r + Cl_da*da+Cl_dr*dr)*0.1; 45 | Cm = Cm_0+Cm_alpha*alpha+ Cm_beta*beta+( Cm_eq*(c/(2*V))*q+Cm_de*de)*0.1; 46 | Cn=Cn_beta*beta+(Cn_p*(b/(2*V))*p+Cn_r*(b/(2*V))*r+Cn_da*da+Cn_dr*dr)*0.01; 47 | %% Moments 48 | ROLL_MOMENT = 1/2*rho*S*b*Cl*V^2; 49 | PITCH_MOMENT = 1/2*rho*S*c*Cm*V^2; 50 | YAW_MOMENT = 1/2*rho*S*b*Cn*V^2; 51 | %% Differential Equations 52 | u_dot = (X_Thrust-g*m*sin(theta)-Drag*cos(alpha)+Lift*sin(alpha))/m + r*V*sin(beta)-q*V*sin(alpha)*cos(beta); 53 | v_dot = (Y_Thrust+g*m*sin(phi)*cos(theta)+Side_Force)/m + p*V*sin(alpha)*cos(beta)-r*V*cos(alpha)*cos(beta); 54 | w_dot = (Z_Thrust+g*m*cos(phi)*cos(theta)-Drag*sin(alpha)-Lift*cos(alpha))/m + q*V*cos(alpha)*cos(beta)-p*V*sin(beta); 55 | 56 | p_dot = (ROLL_MOMENT + (Iyy-Izz) *q*r)/Ixx; 57 | q_dot = (PITCH_MOMENT + (Ixx-Izz) *p*r)/Iyy; 58 | r_dot = (YAW_MOMENT+(Ixx-Iyy)*p*q)/Izz; 59 | 60 | phi_dot = p+q*sin(phi)*tan(theta)+r*cos(phi)*tan(theta); 61 | theta_dot = q*cos(phi)-r*sin(phi); 62 | psi_dot = q*sin(phi)*sec(theta) + r*cos(phi)*sec(theta); 63 | 64 | xg_dot = u*cos(psi)*cos(theta)+v*(cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi))+w*(cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi)); 65 | yg_dot = u*sin(psi)*cos(theta)+v*(sin(psi)*sin(theta)*sin(phi)+cos(psi)*cos(phi))+w*(sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi)); 66 | h_dot = u*sin(theta)-v*cos(theta)*sin(phi)-w*cos(theta)*cos(phi); 67 | %% dx_dt output 68 | 69 | dx = [u_dot v_dot w_dot p_dot q_dot r_dot phi_dot theta_dot psi_dot xg_dot yg_dot h_dot]'; 70 | 71 | end 72 | -------------------------------------------------------------------------------- /funzione_di_costo.m: -------------------------------------------------------------------------------- 1 | function J = funzione_di_costo(x) 2 | %% J = funzione_di_costo(x) 3 | % This function is the cost function for the trim, we want to minimize it. It means that the minimum is where some stato_dot 4 | % components are stationary; 5 | % x = [state+input] J = state_dot R state_dot, where R is not zero if want to minimize the variable,zero if it is free. 6 | % Example: V is given but has to be constant therefore V_dot has be as close as possible to zero 7 | 8 | stato = x( 1 : 12); input = x(13:end); 9 | [stato(1) , stato(2) , stato(3)] = wind2body( x(1) , x(2) , x(3) ); 10 | 11 | [stato_dot] = f_NAVION( stato(1:12) , input); % in body axis 12 | 13 | R = zeros(12); 14 | R(1:6,1:6) = eye(6); % (u v w p q r phi theta psi)dot 15 | R(4:6,4:6) = eye(3) * 60; % (p q r)dot the weight need to be higher to give a good cost function meaning 16 | R(12,12) = 100; % hdot 17 | 18 | J = stato_dot' * R * stato_dot; % u v w p q r phi theta psi dot 19 | end 20 | -------------------------------------------------------------------------------- /holdALL.m: -------------------------------------------------------------------------------- 1 | function [] = holdALL() 2 | % holdALL() set an hold all to all the figures 3 | FIG = get(0,'Children'); 4 | 5 | for kk = max(size(FIG)):-1:1 6 | figure(FIG(kk)) 7 | pause(.01) 8 | hold on 9 | end 10 | 11 | end 12 | -------------------------------------------------------------------------------- /linearizza.m: -------------------------------------------------------------------------------- 1 | function [A,B]=linearizza(fun,stato,input,step) 2 | % [A,B]=linearizza(fun,stato,input,step) 3 | % Linearize fun around a trim point stato input 4 | % Be sure for my control to use the right referece, hence body axis 5 | % It returns the dynamics matrix A and the control matrix B 6 | % fun is the function to linearize associated to the dynamic close_system 7 | % stato_dot = fun(stato,input) 8 | % step is dx in the central difference sense f' = (f(x+dx)-f(x-dx))/(2dx) 9 | 10 | x_trim = stato(1:12); 11 | input_trim = input(1:4); 12 | 13 | % step=1e-3; 14 | for j = 1 : 12 15 | dx = zeros(size(x_trim)); 16 | dx(j) = step; 17 | A(:,j) = (fun(x_trim+dx,input_trim)-fun(x_trim-dx,input_trim))/(2*step); 18 | end 19 | % step=1e-3; 20 | for j= 1 : 4 21 | 22 | du = zeros(size(input_trim)); 23 | du(j) = step; 24 | B(:,j) = (fun(x_trim,input_trim+du)-fun(x_trim,input_trim-du))/(2*step); 25 | 26 | end 27 | end 28 | -------------------------------------------------------------------------------- /min objective.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/min objective.fig -------------------------------------------------------------------------------- /objective funtion model.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/objective funtion model.fig -------------------------------------------------------------------------------- /phi.emf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/phi.emf -------------------------------------------------------------------------------- /phi.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/phi.fig -------------------------------------------------------------------------------- /plot_good.m: -------------------------------------------------------------------------------- 1 | figure(1); 2 | plot(time/2,BODY_Variables.Data(:,7)*180/pi) %7 3 | grid on,hold on; 4 | xlabel('Time [sec]'); 5 | ylabel(ETICHETTA(7)); 6 | hold on 7 | plot(time/2,zeros(length(time),1),'k--'); 8 | figure(2);plot(time/2,BODY_Variables.Data(:,8)*180/pi-20) %8 9 | grid on,hold on; 10 | xlabel('Time [sec]'); 11 | ylabel(ETICHETTA(8)); 12 | hold on 13 | plot(time/2,zeros(length(time),1),'k--'); 14 | figure(3) 15 | plot(time/2,BODY_Variables.Data(:,9)*180/pi+1) %9 16 | grid on,hold on; 17 | xlabel('Time [sec]'); 18 | ylabel(ETICHETTA(9)); 19 | hold on 20 | plot(time/2,(-0)*ones(length(time),1),'k--'); -------------------------------------------------------------------------------- /ploteveryMONTECARLO.m: -------------------------------------------------------------------------------- 1 | function ploteveryMONTECARLO(PARAMETRI_SIMULATI,PARAMETRI_SIMULATI_SUCCESS,PARAMETRI_SIMULATI_FAIL) 2 | close all; 3 | aaa = [7 8 9 17 18 19 20 21 23 24 26 27 28 29]; 4 | for i = 1:14 5 | figure(i); 6 | histogram(PARAMETRI_SIMULATI(aaa(i),:),50);grid on,hold on; 7 | histogram(PARAMETRI_SIMULATI_FAIL(aaa(i),:),50);grid on,hold on; 8 | histogram(PARAMETRI_SIMULATI_SUCCESS(aaa(i),:),50);legend('PARAMETRI','FAIL','SUCCESS') 9 | end 10 | 11 | 12 | -------------------------------------------------------------------------------- /ploteverything.m: -------------------------------------------------------------------------------- 1 | function ploteverything(BODY_Variables_RUNs,MAX_RUN,PARAMETRI_SIMULATI_SUCCESS,PARAMETRI_SIMULATI_SUCCESS_INDEX,PARAMETRI_SIMULATI_FAIL,PARAMETRI_SIMULATI_FAIL_INDEX) 2 | close all; 3 | ETICHETTA = {'V [m/sec]','\alpha [\circ]','\beta [\circ]','p [\circ/sec]','q[\circ/sec]','r[\circ/sec]','phi[\circ]','theta[\circ]','psi[\circ]','x [m]','y [m]','h [m]','de [\circ]','da [\circ]','dr [\circ]','dt [\circ]'}; 4 | success_index = find(PARAMETRI_SIMULATI_SUCCESS_INDEX); 5 | fail_index = find(PARAMETRI_SIMULATI_FAIL_INDEX); 6 | num_zero = length(success_index) ; 7 | for i=1:length(success_index) 8 | % time(:,i) = BODY_Variables_RUNs(index_zero(i)).Time; 9 | time(:,i) = linspace(0,30,30001); 10 | u(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,1); 11 | v(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,2); 12 | w(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,3); 13 | p(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,4); 14 | q(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,5); 15 | r(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,6); 16 | phi(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,7)*180/pi; 17 | theta(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,8)*180/pi; 18 | psi(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,9)*180/pi; 19 | x(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,10); 20 | y(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,11); 21 | h(:,i) = BODY_Variables_RUNs(success_index(i)).Data(:,12); 22 | V(:,i) = sqrt(u(:,i).^2+v(:,i).^2+w(:,i).^2); 23 | alpha(:,i) = atan(w(:,i)./V(:,i))*180/pi; 24 | beta(:,i) = asin(v(:,i)./V(:,i)).*180/pi; 25 | end 26 | figure(2); 27 | for i=1:num_zero 28 | plot(time(:,i),V(:,i));%1 29 | grid on,hold on; 30 | xlabel('Time [sec]'); 31 | ylabel(ETICHETTA(1)); 32 | end 33 | 34 | figure(3); 35 | for i=1:num_zero 36 | plot(time(:,i),alpha(:,i));%2 37 | grid on,hold on; 38 | xlabel('Time [sec]'); 39 | ylabel(ETICHETTA(2)); 40 | end 41 | figure(4); 42 | for i=1:num_zero 43 | plot(time(:,i),beta(:,i));%3 44 | grid on,hold on; 45 | xlabel('Time [sec]'); 46 | ylabel(ETICHETTA(3)); 47 | end 48 | 49 | figure(5); 50 | for i=1:num_zero 51 | plot(time(:,i),p(:,i)); %4 52 | grid on,hold on; 53 | xlabel('Time [sec]'); 54 | ylabel(ETICHETTA(4)); 55 | end 56 | 57 | figure(6); 58 | for i=1:num_zero 59 | plot(time(:,i),q(:,i)); %5 60 | grid on,hold on; 61 | xlabel('Time [sec]'); 62 | ylabel(ETICHETTA(5)); 63 | end 64 | 65 | figure(7); 66 | for i=1:num_zero 67 | plot(time(:,i),r(:,i)); %6 68 | grid on,hold on; 69 | xlabel('Time [sec]'); 70 | ylabel(ETICHETTA(6)); 71 | end 72 | 73 | figure(8); 74 | for i=1:num_zero 75 | plot(time(:,i),phi(:,i)*180/pi/1000) %7 76 | grid on,hold on; 77 | xlabel('Time [sec]'); 78 | ylabel(ETICHETTA(7)); 79 | end 80 | 81 | figure(9); 82 | for i=1:num_zero 83 | plot(time(:,i),theta(:,i)*180/pi/1000); %8 84 | grid on,hold on; 85 | xlabel('Time [sec]'); 86 | ylabel(ETICHETTA(8)); 87 | end 88 | 89 | figure(10); 90 | for i=1:num_zero 91 | plot(time(:,i),psi(:,i)*180/pi/1000); %9 92 | grid on,hold on; 93 | xlabel('Time [sec]'); 94 | ylabel(ETICHETTA(9)); 95 | end 96 | 97 | figure(11); 98 | for i=1:num_zero 99 | plot(time(:,i),x(:,i)); %10 100 | grid on,hold on; 101 | xlabel('Time [sec]'); 102 | ylabel(ETICHETTA(10)); 103 | end 104 | 105 | figure(12); 106 | for i=1:num_zero 107 | plot(time(:,i),y(:,i)); %11 108 | grid on,hold on; 109 | xlabel('Time [sec]'); 110 | ylabel(ETICHETTA(11)); 111 | end 112 | 113 | figure(13); 114 | for i=1:num_zero 115 | plot(time(:,i),h(:,i)); %12 116 | grid on,hold on; 117 | xlabel('Time [sec]'); 118 | ylabel(ETICHETTA(12)); 119 | end 120 | figure(14); 121 | for i=1:num_zero 122 | plot3(x(:,i),y(:,i),h(:,i)); %13 123 | grid on,hold on; 124 | xlabel(ETICHETTA(10)); 125 | ylabel(ETICHETTA(11)); 126 | zlabel(ETICHETTA(12)) 127 | end 128 | end 129 | -------------------------------------------------------------------------------- /psi.emf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/psi.emf -------------------------------------------------------------------------------- /psi.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/psi.fig -------------------------------------------------------------------------------- /run_control.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/run_control.m -------------------------------------------------------------------------------- /svm.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/svm.mat -------------------------------------------------------------------------------- /theta.emf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/theta.emf -------------------------------------------------------------------------------- /theta.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ML-Lab-of-SLU-EE/LIAC-Flight-Control-System/c5ed8426c050c33aed51e856b280531e37d5d027/theta.fig -------------------------------------------------------------------------------- /wind2body.m: -------------------------------------------------------------------------------- 1 | function [u,v,w] = wind2body(V,alpha,beta) 2 | % [u,v,w] = wind2body(V,alpha,beta) 3 | % This function converts from wind to body coordinates 4 | 5 | u = V * cos(alpha) * cos(beta); 6 | v = V * sin(beta); 7 | w = V * sin(alpha) * cos(beta); 8 | end 9 | --------------------------------------------------------------------------------