diff --git a/commonFunctions/control/control_Interp.m b/commonFunctions/control/control_Interp.m
index 940cd0ce56e8096774081fa7bd22c3c239a3b23d..edc39063dec6d57fc3b1802f19fe607deaa80111 100644
--- a/commonFunctions/control/control_Interp.m
+++ b/commonFunctions/control/control_Interp.m
@@ -1,4 +1,4 @@
-function [alpha0,contSettings] = control_Interp(z,Vz,settings,contSettings,alpha0_old)
+function [alpha0,contSettings] = control_Interp(z,Vz,settings,contSettings,alpha0_old,Theta)
% HELP
%
@@ -68,6 +68,15 @@ if z>settings.z_final
alpha0_base = settings.servo.maxAngle;
end
+% alpha0_base = alpha0_base*1/Theta;
+%
+% if alpha0_base<0
+% alpha0_base = 0;
+% end
+% if alpha0_base>settings.servo.maxAngle
+% alpha0_base = settings.servo.maxAngle;
+% end
+
% filter control action
if contSettings.flagFirstControlABK == false % the first reference is given the fastest possible (unfiltered), then filter
alpha0 = alpha0_old + (alpha0_base - alpha0_old)*contSettings.filter_coeff;
diff --git a/commonFunctions/control/control_MRAC.m b/commonFunctions/control/control_MRAC.m
index 0e1fb7f8b82e83f0acb496fad90eafa0967f507c..f8c8bcd7fa71262e11a2b20d126dbe0ebb0b1a1d 100644
--- a/commonFunctions/control/control_MRAC.m
+++ b/commonFunctions/control/control_MRAC.m
@@ -1,4 +1,4 @@
-function [alpha_rad,store] = control_MRAC(z,vz,sensorData,contSettings,store)
+function [alpha_rad,store,Theta] = control_MRAC(z,vz,sensorData,contSettings,store)
%{
-sensorData.kalman.z-settings.z0 = z
-sensorData.kalman.vz = v
@@ -31,11 +31,11 @@ B_p = [0 1]';
Gamma_x = 0.0001;
Gamma_r = 0.0001;
Gamma_th = 0.001;
-Q = 0.5*eye(2);
+Q = 5*eye(2);
P = lyap(A_ref',Q);
% function phi
-phi = 0.5 * vz^2 ;
+phi = -0.5 * vz^2 ;
% phi = vz ;
@@ -54,14 +54,27 @@ for i = 2:N
end
x_ref= x_ref(:,i);
+% N = 200;
+% f = @(x) A_ref*x+B_ref*u_ref;
+% t = linspace(0,dt,N);
+% states = length(store.x0);
+% h = dt/N;
+% x = zeros(states,N);
+% x(:,1) = store.x0;
+% for i = 1:length(t)-1
+% xp1 = x(:,i) + h/2*f(x(:,i));
+% xp2 = x(:,i) + h/2*f(xp1);
+% xp3 = x(:,i) + h*f(xp2);
+% x(:,i+1) = x(:,i) + h*(1/6*f(x(:,i)) + 1/3*f(xp1) + 1/3*f(xp2) + 1/6*f(xp3) );
+% end
+%
+% x_ref = x(:,end)
+
% error
e = ([z;vz]-x_ref);
-% e = z-x_ref(1);
-% Projection operator
-% Gamma = Gamma_th;
-% eps_proj = 0.002;
-% theta_M = 1e-3;
+
+
% Forward euler
@@ -69,8 +82,11 @@ for i = 1:N
K_x = store.K_x(:,i) - Gamma_x*x_ref*e'*P*B_p*dt;
K_r = store.K_r(:,i) - Gamma_r*r*e'*P*B_p*dt;
- Theta = store.Theta(:,i) - Gamma_th*phi*e'*P*B_p*dt;
+ Theta = store.Theta(:,i) + Gamma_th*phi*e'*P*B_p*dt;
+if Theta > 800
+ Theta = 800;
+end
store.K_x(:,i+1) = K_x ;
store.K_r(:,i+1) = K_r ;
store.Theta(:,i+1) = Theta;
@@ -92,9 +108,10 @@ for i = 1:N
end
%%
% control action
- % alpha_rad = K_x'*[z vz]' + K_r*r - Theta*phi;
-K = 0.5*[10 0.5];
-alpha_rad = 1/Theta * (K*e);
+ % alpha_rad = Theta*(K_x'*[z vz]' + K_r*r) ;
+ K = 0.8*[10 0];
+% K = K_x';
+alpha_rad = 1/Theta* (K*e);
% u = K_x'*[z vz]' - Theta*phi;
% translated in servo angle
@@ -119,9 +136,9 @@ alpha_rad = 1/Theta * (K*e);
% delta_S = S - S_nom;
% alpha_rad = delta_S/0.009564;
% estension = getEstension(delta_S);
-
-if alpha_rad > 1.1 && vz>10
- alpha_rad = 1.1;
+% settings.servo.maxAngle
+if alpha_rad > 1.17 && vz>10 % mettere settings ------------------------------------------------- CAMBIAREEEEEEEEEEEEEEEEEEEE
+ alpha_rad = 1.17;
elseif alpha_rad < 0 || vz<10
alpha_rad = 0;
end
diff --git a/data/Gemini_Portugal_October_2023/Trajectories.mat b/data/Gemini_Portugal_October_2023/Trajectories.mat
index 09c60d6188cc6b8553d9a10d50869f3fd4ab8654..dc406552cf699439cddcd7e50a7e62bc2c6a49d5 100644
Binary files a/data/Gemini_Portugal_October_2023/Trajectories.mat and b/data/Gemini_Portugal_October_2023/Trajectories.mat differ
diff --git a/data/Gemini_Portugal_October_2023/configGemini_Portugal_October_2023.m b/data/Gemini_Portugal_October_2023/configGemini_Portugal_October_2023.m
index 91a97fb1803f27cd6e07e0ce0c12eb6adb26b3b8..028a9d1043ee934145de50d6669f38a427b82550 100644
--- a/data/Gemini_Portugal_October_2023/configGemini_Portugal_October_2023.m
+++ b/data/Gemini_Portugal_October_2023/configGemini_Portugal_October_2023.m
@@ -159,4 +159,4 @@ settings.mea.R = 0.36;
settings.mea.z_shutdown = 3200; % [m] target apogee prediction for shutdown
settings.mea.t_lower_shadowmode = 0; % minimunm burning time
settings.mea.t_higher_shadowmode = 10; % maximum burning time
-settings.shutdownValveDelay = 0.6; % time from the shut down command to the actual valve closure
\ No newline at end of file
+settings.shutdownValveDelay = 0.3; % time from the shut down command to the actual valve closure
\ No newline at end of file
diff --git a/simulator/MontecarloResults/Gemini_Portugal_October_2023/complete/200sim_Mach80_gaussian_07_Dec_2023__21_20_23/completeResults07_Dec_2023__21_20_23.txt b/simulator/MontecarloResults/Gemini_Portugal_October_2023/complete/200sim_Mach80_gaussian_07_Dec_2023__21_20_23/completeResults07_Dec_2023__21_20_23.txt
new file mode 100644
index 0000000000000000000000000000000000000000..877de9f2f8c1badaed08fbcdf36fe0c93638cb36
Binary files /dev/null and b/simulator/MontecarloResults/Gemini_Portugal_October_2023/complete/200sim_Mach80_gaussian_07_Dec_2023__21_20_23/completeResults07_Dec_2023__21_20_23.txt differ
diff --git a/simulator/configs/configMontecarlo.m b/simulator/configs/configMontecarlo.m
index 633281bcf4142ad69708cc5a7aa9fa3b9b07a574..265a1d2e6f72f67a10c777ff0e3ba0fb9f8ce37d 100644
--- a/simulator/configs/configMontecarlo.m
+++ b/simulator/configs/configMontecarlo.m
@@ -14,7 +14,7 @@ if settings.montecarlo
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% settable parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% how many simulations
-N_sim = 500; % set to at least 500
+N_sim = 200; % set to at least 500
simulationType_thrust = "gaussian"; % "gaussian", "exterme"
displayIter = true; % set to false if you don't want to see the iteration number (maybe if you want to run Montecarlos on hpe)
diff --git a/simulator/configs/configWind.m b/simulator/configs/configWind.m
index c7fed6056cc19db9d62b4bbafe67a751fa583efb..552dd0e937de037e1e2d45c2ef9e090b8b40d328 100644
--- a/simulator/configs/configWind.m
+++ b/simulator/configs/configWind.m
@@ -13,7 +13,7 @@ NOTE: wind azimuth angle indications (wind directed towards):
% select which model you want to use:
-settings.windModel = "multiplicative"; % choose between: "constant" "multiplicative" "atmospheric"
+settings.windModel = "multipicative"; % choose between: "constant" "multiplicative" "atmospheric"
@@ -64,8 +64,8 @@ if conf.script == "simulator"
settings.wind.input = false;
settings.wind.model = false;
- settings.wind.MagMin = 0; % [m/s] Minimum Magnitude
- settings.wind.MagMax = 0; % [m/s] Maximum Magnitude
+ settings.wind.MagMin = 8; % [m/s] Minimum Magnitude
+ settings.wind.MagMax = 8; % [m/s] Maximum Magnitude
settings.wind.ElMin = 0*pi/180; % [rad] Minimum Elevation, user input in degrees (ex. 0)
settings.wind.ElMax = 0*pi/180; % [rad] Maximum Elevation, user input in degrees (ex. 0) (Max == 90 Deg)
settings.wind.AzMin = (180)*pi/180; % [rad] Minimum Azimuth, user input in degrees (ex. 90)
diff --git a/simulator/src/std_plots.m b/simulator/src/std_plots.m
index 705e58dfcd95b3b3ed25cf4cabcbece348bcb691..f52a697f15bac1de4f5ee63d2d49fbc0c4894df5 100644
--- a/simulator/src/std_plots.m
+++ b/simulator/src/std_plots.m
@@ -161,6 +161,20 @@ xlabel("Time")
ylabel("Gain")
title("Estimated Theta")
+figure()
+plot(simOutput.ARB.x0(1,:))
+hold on
+xlabel("Time")
+ylabel("height")
+yyaxis right
+plot(simOutput.ARB.x0(2,:))
+ylabel("height")
+title("Reference state")
+
+
+
+
+
return
@@ -381,6 +395,9 @@ v_ned = quatrotate(quatconj(structIn.Y(:, 10:13)), structIn.Y(:, 4:6));
plot( -structIn.Y(:, 3), -v_ned(:,3),'b','DisplayName','Traj')
plot( -structIn.sensors.nas.states(:,3), -structIn.sensors.nas.states(:,6),'m--','DisplayName','NAS')
% plot( structIn.ADA(:,4), structIn.ADA(:,5),'b','DisplayName','ADA z')
+plot(simOutput.ARB.x0(1,:),'DisplayName','z_ref')
+plot(simOutput.ARB.x0(2,:),'DisplayName','vz_ref')
+
yyaxis right
plot( -structIn.Y(:, 3), structIn.Y(:, 14),'g','DisplayName','arb')
diff --git a/simulator/src/std_run.m b/simulator/src/std_run.m
index ce0951ecc9ff2137dc34820439a3f097d016029c..81f7f18006855bc7d5ffd1a37d10a7b0b8d4653f 100644
--- a/simulator/src/std_run.m
+++ b/simulator/src/std_run.m
@@ -554,9 +554,9 @@ if settings.montecarlo
% sensors - SFD
- struct_out.sensors.sfd.pressure = interp1(struct_out.sensors.sfd.time',struct_out.sensors.sfd.pressure',t_vec);
- struct_out.sensors.sfd = rmfield(struct_out.sensors.sfd,'faults');
- struct_out.sensors.sfd.time = t_vec;
+ % struct_out.sensors.sfd.pressure = interp1(struct_out.sensors.sfd.time',struct_out.sensors.sfd.pressure',t_vec);
+ % struct_out.sensors.sfd = rmfield(struct_out.sensors.sfd,'faults');
+ % struct_out.sensors.sfd.time = t_vec;
% sensors - remove unwanted fields
struct_out.sensors = rmfield(struct_out.sensors,{'barometer_sens','barometer','comb_chamber','imu','gps','pitot'});
diff --git a/simulator/src/std_run_parts/run_ARB_SIM.m b/simulator/src/std_run_parts/run_ARB_SIM.m
index e8f1df02c0c616e7dc7eb034283048409b1bbc19..b7c07ebfbe4e7aa6592a3de08fe07b89245037e2 100644
--- a/simulator/src/std_run_parts/run_ARB_SIM.m
+++ b/simulator/src/std_run_parts/run_ARB_SIM.m
@@ -30,10 +30,10 @@ switch true % set this value in configControl.m
case strcmp(contSettings.algorithm,'interp') || strcmp(contSettings.algorithm,'complete')
% help in the function
-
- % [ap_ref_new,contSettings] = control_Interp(-sensorData.kalman.z-settings.z0,-sensorData.kalman.vz,settings,contSettings,ap_ref_old); % cambiare nome alla funzione tra le altre cose
- [ap_ref_new,store] = control_MRAC(-sensorData.kalman.z-settings.z0,-sensorData.kalman.vz,sensorData,contSettings,store);
-
+[ap_ref_new,store,Theta] = control_MRAC(-sensorData.kalman.z-settings.z0,-sensorData.kalman.vz,sensorData,contSettings,store);
+
+ [~,contSettings] = control_Interp(-sensorData.kalman.z-settings.z0,-sensorData.kalman.vz,settings,contSettings,ap_ref_old,Theta); % cambiare nome alla funzione tra le altre cose
+
case strcmp (contSettings.algorithm,'shooting')
% shooting algorithm:
diff --git a/trajectoryGeneration/mainTrajectoryGeneration.m b/trajectoryGeneration/mainTrajectoryGeneration.m
index 45ab8d5cd7a43bd598751d64a6fd90995ad02662..c9ace26517240e160ea9ccc28027f73eb46e3bed 100644
--- a/trajectoryGeneration/mainTrajectoryGeneration.m
+++ b/trajectoryGeneration/mainTrajectoryGeneration.m
@@ -55,10 +55,10 @@ configSimulator;
%% AIRBRAKES RADIAL EXTENSION
% Airbrakes extension vector
-delta_alpha_values = linspace(settings.servo.minAngle,settings.servo.maxAngle,2);
+delta_alpha_values = linspace(settings.servo.minAngle,settings.servo.maxAngle,10);
[deltaX_values] = extension_From_Angle(delta_alpha_values, settings);
%%%%% MRAC
-delta_alpha = (delta_alpha_values(2) + delta_alpha_values(1))/2;
+delta_alpha = delta_alpha_values(5) ;
[deltaX_values] = extension_From_Angle(delta_alpha, settings);
%%%%%
% I exclude the limits for robustness