diff --git a/commonFunctions/kalman/run_ADA.m b/commonFunctions/kalman/run_ADA.m
index 86c6a974708f88d4394581e52d93b59a474c477c..bd881db7fb70ca24919a50ec5c588713c5e289c4 100644
--- a/commonFunctions/kalman/run_ADA.m
+++ b/commonFunctions/kalman/run_ADA.m
@@ -1,4 +1,4 @@
-function [sensorData, sensorTot, ada, flagApogee, flagOpenPara] = run_ADA(sensorData, sensorTot, settings,tf)
+function [sensorData, sensorTot] = run_ADA(sensorData, sensorTot, settings,tf)
% Author: Alessandro Del Duca
% Skyward Experimental Rocketry | ELC-SCS Dept | electronics@skywarder.eu
@@ -70,9 +70,9 @@ xp = zeros(length(t_ada),3);
P = zeros(3,3,length(t_ada));
xv = zeros(length(t_ada),2);
-xp(1,:) = sensorData.ada.xp(end,:);
-P(:,:,1) = sensorData.ada.P(:,:,end);
-xv(1,:) = sensorData.ada.xv(end,:);
+xp(1,:) = sensorData.old_ada.xp(end,:);
+P(:,:,1) = sensorData.old_ada.P(:,:,end);
+xv(1,:) = sensorData.old_ada.xv(end,:);
if length(t_ada)>1
% initialize dt
@@ -108,45 +108,42 @@ if length(t_ada)>1
xv(ii,1) = getaltitude(xp(ii,1),ada.temp_ref, ada.p_ref);
xv(ii,2) = getvelocity(xp(ii,1),xp(ii,2),ada.temp_ref, ada.p_ref);
- if ada.flag_apo == false
+ if sensorTot.old_ada.flagApogee == false
if xv(ii,2) < ada.v_thr && xv(ii,1) > 100
- ada.counter = ada.counter + 1;
+ sensorData.old_ada.counter = sensorData.old_ada.counter + 1;
else
- ada.counter = 0;
+ sensorData.old_ada.counter = 0;
end
- if ada.counter >= ada.count_thr
- ada.t_ada = t_ada(ii);
- ada.flag_apo = true;
+ if sensorData.old_ada.counter >= ada.count_thr
+ sensorTot.old_ada.t_ada = t_ada(ii);
+ sensorTot.old_ada.flagApogee = true;
end
end
- if strcmp(settings.scenario, 'descent') || ada.flag_apo
- if ada.flagOpenPara == false
- if xv(ii,1) < settings.ada.para.z_cut
- ada.paraCounter = ada.paraCounter+1;
+ if strcmp(settings.scenario, 'descent') || sensorTot.old_ada.flagApogee
+ if sensorTot.old_ada.flagOpenPara == false
+ if xv(ii,1) < ada.z_cut
+ sensorData.old_ada.paraCounter = sensorData.old_ada.paraCounter+1;
else
- ada.paraCounter = 0;
+ sensorData.old_ada.paraCounter = 0;
end
- if ada.paraCounter >= ada.altitude_confidence_thr
- ada.t_para = t_ada(ii);
- ada.flagOpenPara = true;
+ if sensorData.old_ada.paraCounter >= ada.altitude_confidence_thr
+ sensorTot.old_ada.t_para = t_ada(ii);
+ sensorTot.old_ada.flagOpenPara = true;
end
end
end
end
-flagApogee = ada.flag_apo;
-flagOpenPara = ada.flagOpenPara;
-
-sensorData.ada.xp = xp;
-sensorData.ada.xv = xv;
-sensorData.ada.P = P;
-sensorData.ada.time = t_ada;
-
-sensorTot.ada.xp(sensorTot.ada.n_old:sensorTot.ada.n_old + size(sensorData.ada.xp(:,1),1) -2,:) = sensorData.ada.xp(2:end,:);
-sensorTot.ada.xv(sensorTot.ada.n_old:sensorTot.ada.n_old + size(sensorData.ada.xv(:,1),1)-2,:) = sensorData.ada.xv(2:end,:);
-sensorTot.ada.time(sensorTot.ada.n_old:sensorTot.ada.n_old + size(sensorData.ada.xp(:,1),1)-2) = sensorData.ada.time(2:end);
-sensorTot.ada.n_old = sensorTot.ada.n_old + size(sensorData.ada.xp,1)-1;
+sensorData.old_ada.xp = xp;
+sensorData.old_ada.xv = xv;
+sensorData.old_ada.P = P;
+% sensorData.old_ada.time = t_ada;
+
+sensorTot.old_ada.xp(sensorTot.ada.n_old:sensorTot.ada.n_old + size(sensorData.old_ada.xp(:,1),1) -2,:) = sensorData.old_ada.xp(2:end,:);
+sensorTot.old_ada.xv(sensorTot.ada.n_old:sensorTot.ada.n_old + size(sensorData.old_ada.xv(:,1),1)-2,:) = sensorData.old_ada.xv(2:end,:);
+% sensorTot.ada.time(sensorTot.ada.n_old:sensorTot.ada.n_old + size(sensorData.ada.xp(:,1),1)-2) = sensorData.ada.time(2:end);
+% sensorTot.ada.n_old = sensorTot.ada.n_old + size(sensorData.ada.xp,1)-1;
end
diff --git a/commonFunctions/kalman/run_ADA_update.m b/commonFunctions/kalman/run_ADA_update.m
new file mode 100644
index 0000000000000000000000000000000000000000..6eef51a093a9695acc460b6f497bd553288cc1a6
--- /dev/null
+++ b/commonFunctions/kalman/run_ADA_update.m
@@ -0,0 +1,34 @@
+function [xp, P, xv, lastBaroTimestamp] = run_ADA_update(t_ada, dt, ada_settings, xp_prev, P_prev, baro_data, lastBaroTimestamp, getAltitude, getVelocity)
+
+ % Define state matrices
+ At = [1 dt 0.5*dt^2;
+ 0 1 dt;
+ 0 0 1];
+ Ct = [1 0 0];
+
+ % Prediction step:
+ xp = (At * xp_prev')';
+
+ % Prediction variance propagation:
+ P = ada_settings.Q + At * P_prev * At';
+
+ % Check if a new barometer measurement is available
+ t_baro = baro_data.time;
+ idx_baro = sum(t_ada >= t_baro);
+ if t_baro(idx_baro) > lastBaroTimestamp
+ % Perform the correction step ONLY IF a new barometer measurement
+ % is available
+ S = Ct * P * Ct' + ada_settings.R;
+ K = P * Ct' / S;
+ xp = (xp' + K*(baro_data.measures(idx_baro) - Ct*xp'))';
+ P = (eye(3) - K*Ct) * P;
+
+ % Update the last used barometer timestamp
+ lastBaroTimestamp = t_baro(idx_baro);
+ end
+
+ % Convert the pressure state in altitude and velocity
+ xv(1) = getAltitude(xp(1), ada_settings.temp_ref, ada_settings.p_ref);
+ xv(2) = getVelocity(xp(1), xp(2), ada_settings.temp_ref, ada_settings.p_ref);
+
+end
\ No newline at end of file
diff --git a/commonFunctions/kalman/run_Majority_ADA.m b/commonFunctions/kalman/run_Majority_ADA.m
new file mode 100644
index 0000000000000000000000000000000000000000..33740e466dfc5156590f0280c8d60ceb10633788
--- /dev/null
+++ b/commonFunctions/kalman/run_Majority_ADA.m
@@ -0,0 +1,145 @@
+function [sensorData, sensorTot, ada_settings, flagMajorityApogee, flagMajorityOpenPara] = run_Majority_ADA(Tf, ADA_N_instances, ada_settings, ada_frequency, sensorData, sensorTot, currentState, engineT0)
+
+ % Time vector for the current algorithm timestamps to simulate
+ t_ada = sensorTot.ada.time(end):1/ada_frequency:Tf(end);
+
+ % Set the matrices that will contain the states of the algorithms
+ % during the updates.
+ % The first value of each matrix will be the value at the last
+ % timestamp of the previous iteration of the algorithm
+ xp = zeros(length(t_ada), 3, ADA_N_instances);
+ P = zeros(3, 3, length(t_ada), ADA_N_instances);
+ xv = zeros(length(t_ada), 2, ADA_N_instances);
+ for jj = 1:ADA_N_instances
+ xp(1,:,jj) = sensorData.ada{jj}.xp(end,:);
+ P(:,:,1,jj) = sensorData.ada{jj}.P(:,:,end);
+ xv(1,:,jj) = sensorData.ada{jj}.xv(end,:);
+ end
+
+ % The majority flags are set as the previous iteration value
+ flagMajorityApogee = ada_settings.flag_apo;
+ flagMajorityOpenPara = ada_settings.flagOpenPara;
+
+ % Initialize flag matrices to hold the values of the flag for the
+ % current iterations
+ flagApogeeMat = false(length(t_ada), ADA_N_instances);
+ flagApogeeMat(1,:) = sensorTot.ada.flagApogee(end,:);
+ flagOpenParaMat = false(length(t_ada), ADA_N_instances);
+ flagOpenParaMat(1,:) = sensorTot.ada.flagOpenPara(end,:);
+
+ % If the length is 1 it means that t_ada only contains
+ % sensorTot.ada.time(end), which has already been simulated during the
+ % previous simulator iteration.
+ % If the length is <= 0 then there is a problem with the way t_ada was
+ % computed.
+ if length(t_ada) > 1
+
+ % Initialize dt
+ dt = diff(t_ada(1:2));
+
+ for ii = 2:length(t_ada)
+
+ % Update all the instances of the algorithm
+ for jj = 1:ADA_N_instances
+ [xp(ii,:,jj), P(:,:,ii,jj), xv(ii,:,jj), sensorData.ada{jj}.lastBaroTimestamp] = ...
+ run_ADA_update(t_ada(ii), dt, ada_settings, xp(ii-1, :, jj), P(:,:,ii-1,jj), ...
+ sensorData.barometer_sens{jj}, sensorData.ada{jj}.lastBaroTimestamp, @getAltitude, @getvelocity);
+ end
+
+ % If the rocket is flying, perform the checks for all instances for apogee
+ if currentState ~= 1 % state 1 is the on_ground state
+ for jj = 1:ADA_N_instances
+ if ada_settings.flag_apo == false && flagApogeeMat(ii-1, jj) == false
+ if xv(ii, 2, jj) < ada_settings.v_thr
+ sensorData.ada{jj}.counter = sensorData.ada{jj}.counter + 1;
+ else
+ sensorData.ada{jj}.counter = 0;
+ end
+
+ if sensorData.ada{jj}.counter >= ada_settings.count_thr
+ % If the apogee is detected while still in
+ % shadowmode issue a warning to notify of it
+ % happening
+ if t_ada(ii) < (ada_settings.shadowmode + engineT0)
+ % warning("ADA %d detected an apogee while still in shadowmode", jj);
+ else
+ flagApogeeMat(ii, jj) = true;
+ end
+ end
+ elseif flagApogeeMat(ii-1, jj) == true
+ flagApogeeMat(ii, jj) = true;
+ end
+ end
+ end
+
+ % Perform the checks for all instances for parachute opening
+ % only if the apogee has already been detected
+ for jj = 1:ADA_N_instances
+ if ada_settings.flag_apo
+ if ada_settings.flagOpenPara == false
+ if xv(ii,1,jj) < ada_settings.z_cut
+ sensorData.ada{jj}.paraCounter = sensorData.ada{jj}.paraCounter + 1;
+ else
+ sensorData.ada{jj}.paraCounter = 0;
+ end
+ end
+ end
+ if sensorData.ada{jj}.paraCounter >= ada_settings.altitude_confidence_thr
+ flagOpenParaMat(ii,jj) = true;
+ end
+ end
+
+ % If more than 50% of the instances detect apogee then the apogee flag
+ % is set to true
+ if ada_settings.flag_apo == false && sum(flagApogeeMat(ii,:)) >= ceil(ADA_N_instances/2)
+ flagMajorityApogee = true;
+ ada_settings.t_ada = t_ada(ii);
+ end
+
+ % If more than 50% of the instances detect parachute opening altitude
+ % then the OpenPara flag is set to true
+ if ada_settings.flag_apo && sum(flagOpenParaMat(ii,:)) >= ceil(ADA_N_instances/2)
+ flagMajorityOpenPara = true;
+ ada_settings.t_para = t_ada(ii);
+ end
+
+ end
+ end
+
+ ada_settings.flag_apo = flagMajorityApogee;
+ ada_settings.flagOpenPara = flagMajorityOpenPara;
+
+ % Update all the values in sensorData, so they can be used during the
+ % next iteration (the choice of jj instead of ii is for consistency with previous code)
+ for jj = 1:ADA_N_instances
+ sensorData.ada{jj}.xp = xp(:,:,jj);
+ sensorData.ada{jj}.P = P(:,:,:,jj);
+ sensorData.ada{jj}.xv = xv(:,:,jj);
+ end
+
+ % Update the sensorTot struct, used for logging of all the states and
+ % time
+ for jj = 1:ADA_N_instances
+ sensorTot.ada.data{jj}.xp(sensorTot.ada.n_old:sensorTot.ada.n_old + length(t_ada)-2,:) = sensorData.ada{jj}.xp(2:end, :);
+ sensorTot.ada.data{jj}.xv(sensorTot.ada.n_old:sensorTot.ada.n_old + length(t_ada)-2,:) = sensorData.ada{jj}.xv(2:end, :);
+ end
+ sensorTot.ada.flagApogee(sensorTot.ada.n_old:sensorTot.ada.n_old + length(t_ada)-2, :) = flagApogeeMat(2:end, :);
+ sensorTot.ada.flagOpenPara(sensorTot.ada.n_old:sensorTot.ada.n_old + length(t_ada)-2, :) = flagOpenParaMat(2:end, :);
+ sensorTot.ada.time(sensorTot.ada.n_old:sensorTot.ada.n_old + length(t_ada)-2) = t_ada(2:end);
+ sensorTot.ada.n_old = sensorTot.ada.n_old + length(t_ada)-1;
+
+end
+
+function h = getAltitude(p, temp_ref, p_ref)
+ a = 0.0065;
+ n = 9.807/(287.05*a);
+
+ h = temp_ref / a * (1 - (p / p_ref)^(1/n));
+end
+
+function v = getvelocity(p, dpdt, temp_ref, p_ref)
+ a = 0.0065;
+ n = 9.807/(287.05*a);
+
+ v = -(temp_ref * dpdt * (p / p_ref)^ (1/n)) / (a * n * p);
+end
\ No newline at end of file
diff --git a/commonFunctions/sensors/acquisition_Sys.m b/commonFunctions/sensors/acquisition_Sys.m
index 1f6890dac20749d9ac2d0e7e0fbe55f60a473311..114b2f1eaa1c754f32adbba4fdc0811871831918 100644
--- a/commonFunctions/sensors/acquisition_Sys.m
+++ b/commonFunctions/sensors/acquisition_Sys.m
@@ -27,7 +27,7 @@ OUTPUT:
%% Baro Acquisition loop
-if ~contains(mission.name, '2023')
+if ~contains(mission.name, {'2023', '2024', '2025'})
sensorSettings.barometer2 = sensorSettings.barometer1;
sensorSettings.barometer3 = sensorSettings.barometer1;
end
diff --git a/data/2024_Lyra_Roccaraso_September/config2024_Lyra_Roccaraso_September.m b/data/2024_Lyra_Roccaraso_September/config2024_Lyra_Roccaraso_September.m
index c2e74be136765162c0b321d16ed23c9d68ad48ee..c25bb3a35c0452edb6f26be3459f17dd57f8768b 100644
--- a/data/2024_Lyra_Roccaraso_September/config2024_Lyra_Roccaraso_September.m
+++ b/data/2024_Lyra_Roccaraso_September/config2024_Lyra_Roccaraso_September.m
@@ -145,27 +145,24 @@ settings.ada.P0 = [ 0.1 0 0; % In
[settings.ada.temp_ref, ~,...
settings.ada.p_ref, ~] = computeAtmosphericData(environment.z0); % Reference temperature in kelvin and pressure in Pa
-settings.ada.v0 = -10; % Vertical velocity initial condition
-settings.ada.a0 = -100; % Acceleration velocity initial condition
+settings.ada.v0 = 0; % Vertical velocity initial condition
+settings.ada.a0 = 0; % Acceleration velocity initial condition
settings.ada.x0 = [settings.ada.p_ref, settings.ada.v0, settings.ada.a0];
- % Ada initial condition
+ % Ada initial condition
-settings.ada.v_thr = 0; % Velocity threshold for the detected apogee
-settings.ada.count_thr = 5; % If the apogee is detected count_thr time, the algorithm will return the apogee event
+settings.ada.v_thr = 0; % Velocity threshold for the detected apogee
+settings.ada.count_thr = 5; % If the apogee is detected count_thr time, the algorithm will return the apogee event
settings.ada.counter = 0;
settings.ada.altitude_confidence_thr = 5; % If the ada recognizes altitude_confidence_thr samples under parachute cutting altitude, it sends the command
-
-settings.ada.t_ada = -1; % Apogee detection timestamp
-settings.ada.flag_apo = false; % True when the apogee is detected
-
settings.ada.shadowmode = 10;
if ~settings.parafoil
- settings.ada.para.z_cut = rocket.parachutes(1,1).finalAltitude;
+ settings.ada.z_cut = rocket.parachutes(1,1).finalAltitude;
else
- settings.ada.para.z_cut = rocket.parachutes(1,2).finalAltitude;
+ settings.ada.z_cut = rocket.parachutes(1,2).finalAltitude;
end
+
%% MEA TUNING PARAMETERS / MOTOR SHUT DOWN TUNING PARAMETERS
settings.mea.engine_model_A1 = [1.62583090191848 -0.680722129751093 0; 1 0 0; -0.00102053146869855 0.000494919888520664 1];
settings.mea.engine_model_B1 = [2;0;0];
diff --git a/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m b/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
index 6edd83fccbaee3559b8e235243f9862ee28b7640..06324f9839b7898190f3c84255811af74ad8c99c 100644
--- a/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
+++ b/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
@@ -40,7 +40,7 @@ settings.frequencies.accelerometerFrequency = 100; % [hz
settings.frequencies.gyroFrequency = 100; % [hz] sensor frequency
settings.frequencies.magnetometerFrequency = 100; % [hz] sensor frequency
settings.frequencies.gpsFrequency = 10; % [hz] sensor frequency
-settings.frequencies.barometerFrequency = 50; % [hz] sensor frequency
+settings.frequencies.barometerFrequency = 100; % [hz] sensor frequency
settings.frequencies.chamberPressureFrequency = 50; % [hz] sensor frequency
settings.frequencies.pitotFrequency = 20; % [hz] sensor frequency
settings.frequencies.NASFrequency = 50; % [hz] sensor frequency
@@ -148,23 +148,19 @@ settings.ada.P0 = [ 0.1 0 0; % In
[settings.ada.temp_ref, ~,...
settings.ada.p_ref, ~] = computeAtmosphericData(environment.z0); % Reference temperature in kelvin and pressure in Pa
-settings.ada.v0 = -10; % Vertical velocity initial condition
-settings.ada.a0 = -100; % Acceleration velocity initial condition
+settings.ada.v0 = 0; % Vertical velocity initial condition
+settings.ada.a0 = 0; % Acceleration velocity initial condition
settings.ada.x0 = [settings.ada.p_ref, settings.ada.v0, settings.ada.a0];
% Ada initial condition
-settings.ada.v_thr = 0; % Velocity threshold for the detected apogee
+settings.ada.v_thr = 0; % Velocity threshold for the detected apogee
settings.ada.count_thr = 5; % If the apogee is detected count_thr time, the algorithm will return the apogee event
-settings.ada.counter = 0;
settings.ada.altitude_confidence_thr = 5; % If the ada recognizes altitude_confidence_thr samples under parachute cutting altitude, it sends the command
-
-settings.ada.t_ada = -1; % Apogee detection timestamp
-settings.ada.flag_apo = false; % True when the apogee is detected
settings.ada.shadowmode = 18;
if ~settings.parafoil
- settings.ada.para.z_cut = rocket.parachutes(1,1).finalAltitude;
+ settings.ada.z_cut = rocket.parachutes(1,1).finalAltitude;
else
- settings.ada.para.z_cut = rocket.parachutes(1,2).finalAltitude;
+ settings.ada.z_cut = rocket.parachutes(1,2).finalAltitude;
end
%% MEA TUNING PARAMETERS / MOTOR SHUT DOWN TUNING PARAMETERS
diff --git a/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m b/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m
index e3b79ac9a7678ec3a53a6443462942f0c01d8392..1539516739876b968b1aa4c46821f7903c51e5f7 100644
--- a/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m
+++ b/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m
@@ -13,8 +13,8 @@ sensorSettings.barometer1 = SensorFault();
sensorSettings.barometer1.maxMeasurementRange = 1000; % 1100, 1300 in mbar
sensorSettings.barometer1.minMeasurementRange = 0; % 300, 10 in mbar
sensorSettings.barometer1.bit = 24; % adc on rocket is 24 bits
-sensorSettings.barometer1.fault_time = 9; % if negative it will be generated at random between a max and a min value
-sensorSettings.barometer1.max_fault_time = 96; % max seconds to wait before possible fault
+sensorSettings.barometer1.fault_time = -1; % if negative it will be generated at random between a max and a min value
+sensorSettings.barometer1.max_fault_time = 60; % max seconds to wait before possible fault
sensorSettings.barometer1.min_fault_time = 6; % min seconds to wait before possible fault
% fault generation
@@ -42,7 +42,7 @@ sensorSettings.barometer2.maxMeasurementRange = 1000; % 11
sensorSettings.barometer2.minMeasurementRange = 0; % 300, 10 in mbar
sensorSettings.barometer2.bit = 24; % adc on rocket is 24 bits
sensorSettings.barometer2.fault_time = -1; % if negative it will be generated at random between a max and a min value
-sensorSettings.barometer2.max_fault_time = 96; % max seconds to wait before possible fault
+sensorSettings.barometer2.max_fault_time = 24; % max seconds to wait before possible fault
sensorSettings.barometer2.min_fault_time = 6; % min seconds to wait before possible fault
% fault generation
@@ -70,7 +70,7 @@ sensorSettings.barometer3.maxMeasurementRange = 4060; % 11
sensorSettings.barometer3.minMeasurementRange = 260; % 300, 10 in mbar
sensorSettings.barometer3.bit = 24;
sensorSettings.barometer3.fault_time = -1; % if negative it will be generated at random between a max and a min value
-sensorSettings.barometer3.max_fault_time = 96; % max seconds to wait before possible fault
+sensorSettings.barometer3.max_fault_time = 60; % max seconds to wait before possible fault
sensorSettings.barometer3.min_fault_time = 6; % min seconds to wait before possible fault
% fault generation
diff --git a/data/2025_Orion_Roccaraso_September/config2025_Orion_Roccaraso_September.m b/data/2025_Orion_Roccaraso_September/config2025_Orion_Roccaraso_September.m
index b25b6f5e13e29d10ed1798e53081b7f6b7236646..2ca52bb37fecd18f0fe9fd30de91a2541da36f39 100644
--- a/data/2025_Orion_Roccaraso_September/config2025_Orion_Roccaraso_September.m
+++ b/data/2025_Orion_Roccaraso_September/config2025_Orion_Roccaraso_September.m
@@ -16,7 +16,8 @@ SPDX-License-Identifier: GPL-3.0-or-later
%}
-settings.launchDate = [2023 9 18];
+settings.launchDate = [2024 9 14];
+warning("Launch date set to 2024 as wrldmagm does not support 2025 launch date")
settings.HREmot = true;
%% TRAJECTORY GENERATION PARAMETERS
@@ -39,7 +40,7 @@ settings.frequencies.accelerometerFrequency = 100; % [hz
settings.frequencies.gyroFrequency = 100; % [hz] sensor frequency
settings.frequencies.magnetometerFrequency = 100; % [hz] sensor frequency
settings.frequencies.gpsFrequency = 10; % [hz] sensor frequency
-settings.frequencies.barometerFrequency = 50; % [hz] sensor frequency
+settings.frequencies.barometerFrequency = 100; % [hz] sensor frequency
settings.frequencies.chamberPressureFrequency = 50; % [hz] sensor frequency
settings.frequencies.pitotFrequency = 20; % [hz] sensor frequency
settings.frequencies.NASFrequency = 50; % [hz] sensor frequency
@@ -59,7 +60,7 @@ settings.arb.extPol(1) = -0.009083; % co
settings.arb.extPol(2) = 0.02473; % coefficient for extension - alpha^3
settings.arb.extPol(3) = -0.01677; % coefficient for extension - alpha^2
settings.arb.extPol(4) = 0.03129; % coefficient for extension - alpha
-settings.arb.maxExt = rocket.airbrakes.height(end);
+settings.arb.maxExt = rocket.airbrakes.height(end); % maximum extension of air brake aerodynamic surface
% servo angle to exposed surface of the airbrakes (GEMINI)
settings.arb.surfPol = 0.00932857142857; % coefficient for surface - alpha
@@ -72,7 +73,7 @@ settings.arb.guidePol(2) = 0.5337; % co
settings.arb.ma = 150e-3; % airbrake mass
settings.arb.mb = 20e-3; % tristar beam mass
settings.arb.mu = 0.05; % friction coefficient between air brake and guide
-settings.arb.R = 66e-3; % tristar beam length (rod)
+settings.arb.R = 66e-3; % tristar beam length (rod)
% Get maximum extension angle
x = @(alpha) settings.arb.extPol(1)*alpha.^4 + ...
@@ -97,8 +98,8 @@ settings.nas.P = 0.01*eye(12);
settings.nas.Mach_max = 0.4; % max mach number expected for the mission (for nas with pitot update purposes) - not currently used
settings.nas.GPS.a = 111132.95225;
settings.nas.GPS.b = 111412.87733;
-settings.nas.v_thr = 2.5; % Velocity threshold for the detected apogee
-settings.nas.count_thr = 5; % If the apogee is detected count_thr time, the algorithm will return the apogee event
+settings.nas.v_thr = 2.5; % Velocity threshold for the detected apogee
+settings.nas.count_thr = 5; % If the apogee is detected count_thr time, the algorithm will return the apogee event
settings.nas.counter = 0;
settings.nas.baro.a = 0.0065;
@@ -145,25 +146,21 @@ settings.ada.P0 = [ 0.1 0 0; % In
[settings.ada.temp_ref, ~,...
settings.ada.p_ref, ~] = computeAtmosphericData(environment.z0); % Reference temperature in kelvin and pressure in Pa
-settings.ada.v0 = -10; % Vertical velocity initial condition
-settings.ada.a0 = -100; % Acceleration velocity initial condition
+settings.ada.v0 = 0; % Vertical velocity initial condition
+settings.ada.a0 = 0; % Acceleration velocity initial condition
settings.ada.x0 = [settings.ada.p_ref, settings.ada.v0, settings.ada.a0];
- % Ada initial condition
+ % Ada initial condition
-settings.ada.v_thr = 0; % Velocity threshold for the detected apogee
-settings.ada.count_thr = 5; % If the apogee is detected count_thr time, the algorithm will return the apogee event
+settings.ada.v_thr = 0; % Velocity threshold for the detected apogee
+settings.ada.count_thr = 5; % If the apogee is detected count_thr time, the algorithm will return the apogee event
settings.ada.counter = 0;
settings.ada.altitude_confidence_thr = 5; % If the ada recognizes altitude_confidence_thr samples under parachute cutting altitude, it sends the command
-
-settings.ada.t_ada = -1; % Apogee detection timestamp
-settings.ada.flag_apo = false; % True when the apogee is detected
-
settings.ada.shadowmode = 10;
if ~settings.parafoil
- settings.ada.para.z_cut = rocket.parachutes(1,1).finalAltitude;
+ settings.ada.z_cut = rocket.parachutes(1,1).finalAltitude;
else
- settings.ada.para.z_cut = rocket.parachutes(1,2).finalAltitude;
+ settings.ada.z_cut = rocket.parachutes(1,2).finalAltitude;
end
%% MEA TUNING PARAMETERS / MOTOR SHUT DOWN TUNING PARAMETERS
@@ -182,7 +179,7 @@ settings.mea.z_shutdown = 800; % [m]
settings.mea.t_lower_shadowmode = 2; % minimunm burning time
settings.mea.t_higher_shadowmode = 10; % maximum burning time
settings.shutdownValveDelay = 0.2; % time from the shut down command to the actual valve closure
-settings.mea.cd_correction_factor = 2.69;
+settings.mea.cd_correction_factor = 1;
% accelerometer correction parameters
[~,~,settings.mea.P0] = computeAtmosphericData(103); %[Pa] reference pressure at the SFT location
diff --git a/data/2025_Orion_Roccaraso_September/initSensors2025_Orion_Roccaraso_September.m b/data/2025_Orion_Roccaraso_September/initSensors2025_Orion_Roccaraso_September.m
index e3b79ac9a7678ec3a53a6443462942f0c01d8392..be8164406350c1c93424fd8c04c8c65f74e69da1 100644
--- a/data/2025_Orion_Roccaraso_September/initSensors2025_Orion_Roccaraso_September.m
+++ b/data/2025_Orion_Roccaraso_September/initSensors2025_Orion_Roccaraso_September.m
@@ -13,8 +13,8 @@ sensorSettings.barometer1 = SensorFault();
sensorSettings.barometer1.maxMeasurementRange = 1000; % 1100, 1300 in mbar
sensorSettings.barometer1.minMeasurementRange = 0; % 300, 10 in mbar
sensorSettings.barometer1.bit = 24; % adc on rocket is 24 bits
-sensorSettings.barometer1.fault_time = 9; % if negative it will be generated at random between a max and a min value
-sensorSettings.barometer1.max_fault_time = 96; % max seconds to wait before possible fault
+sensorSettings.barometer1.fault_time = -1; % if negative it will be generated at random between a max and a min value
+sensorSettings.barometer1.max_fault_time = 60; % max seconds to wait before possible fault
sensorSettings.barometer1.min_fault_time = 6; % min seconds to wait before possible fault
% fault generation
@@ -42,7 +42,7 @@ sensorSettings.barometer2.maxMeasurementRange = 1000; % 11
sensorSettings.barometer2.minMeasurementRange = 0; % 300, 10 in mbar
sensorSettings.barometer2.bit = 24; % adc on rocket is 24 bits
sensorSettings.barometer2.fault_time = -1; % if negative it will be generated at random between a max and a min value
-sensorSettings.barometer2.max_fault_time = 96; % max seconds to wait before possible fault
+sensorSettings.barometer2.max_fault_time = 60; % max seconds to wait before possible fault
sensorSettings.barometer2.min_fault_time = 6; % min seconds to wait before possible fault
% fault generation
@@ -70,7 +70,7 @@ sensorSettings.barometer3.maxMeasurementRange = 4060; % 11
sensorSettings.barometer3.minMeasurementRange = 260; % 300, 10 in mbar
sensorSettings.barometer3.bit = 24;
sensorSettings.barometer3.fault_time = -1; % if negative it will be generated at random between a max and a min value
-sensorSettings.barometer3.max_fault_time = 96; % max seconds to wait before possible fault
+sensorSettings.barometer3.max_fault_time = 60; % max seconds to wait before possible fault
sensorSettings.barometer3.min_fault_time = 6; % min seconds to wait before possible fault
% fault generation
diff --git a/simulator/configs/configControl.m b/simulator/configs/configControl.m
index e2d962af0555b592b7c7aad8cf7c0a8fea433b42..5ae96e567b8ccb7128d7cea7264ac6f78ad2e45b 100644
--- a/simulator/configs/configControl.m
+++ b/simulator/configs/configControl.m
@@ -21,6 +21,19 @@ end
data = load(strcat(mission.dataPath, '/CAinterpCoeffs'));
contSettings.coeffs = data.coeffs;
+%% ADA Multiple instances parameters
+
+% Set this flag to true to also run the old version of the ada (the one
+% implemented in run_ADA). Useful to compare the results between old and
+% new implementation
+contSettings.run_old_ada = true;
+% Number of instances to be run
+contSettings.ADA_N_instances = 3;
+
+if mod(contSettings.ADA_N_instances, 2) == 0 || contSettings.ADA_N_instances <= 0
+ error("The number of instances of ADA must be odd and positive");
+end
+
%% CONTROL PARAMETERS
% choose strategy:
@@ -97,8 +110,6 @@ settings.motor.K = settings.mea.K_t;
% contSettings.Engine_model_Kgain = [0.237322102194205;0.242208876758461;-0.000686466033197479];
-%% engine control initialization - Mass Estimation Algorithm
-
%% MAGNETIC MAP
settings.hmax = 6000; % [m] Max altitude at which the world magnetic map must be computed
diff --git a/simulator/configs/configFaults.m b/simulator/configs/configFaults.m
index 607f2ae9d7f5e1175205e127ee9728914a26efd7..e5bd9b7fb7daa0eccf8afd3c280003d358e489e5 100644
--- a/simulator/configs/configFaults.m
+++ b/simulator/configs/configFaults.m
@@ -8,6 +8,7 @@ sensor fault configuration script
% how many faults do you want to simulate?
settings.fault_sim.N_faulty_sensors = -1; % if set to -1 it will go to manual fault setting, otherwise it will generate random faults at a set of random sensors
+settings.fault_sim.fault_type = ["no fault", "no fault", "no fault"];
if settings.fault_sim.N_faulty_sensors == -1
settings.fault_sim.selected_sensors = [];
diff --git a/simulator/configs/configFlags.m b/simulator/configs/configFlags.m
index f3a3dfb1f2f73634f6b577b6fa614895ea714d53..c3c8c615ad6cc16e0e38cbad672e8cd8393f26cd 100644
--- a/simulator/configs/configFlags.m
+++ b/simulator/configs/configFlags.m
@@ -27,8 +27,8 @@ scenarios explanation:
% scenario configuration
-conf.scenario = "controlled ascent";
-conf.board = "payload"; % Either "main" or "payload"
+conf.scenario = "full flight";
+conf.board = "main"; % Either "main" or "payload"
conf.HIL = false;
% WIP flags
diff --git a/simulator/configs/configPath.m b/simulator/configs/configPath.m
index 73b91ede284d0ead8fd0e618e571ca9164beab5e..b33ef37f1220db535c5b166cf06840351815b6a6 100644
--- a/simulator/configs/configPath.m
+++ b/simulator/configs/configPath.m
@@ -14,11 +14,6 @@ addpath(ConDataPath);
commonFunctionsPath = '../commonFunctions';
addpath(genpath(commonFunctionsPath));
-% % Remove unwanted paths
-% missionPath = strcat('../common/missions/', mission.name);
-% rmpath(genpath('../common/missions/'));
-% addpath(genpath(missionPath));
-
% only for hardware in the loop - path configuration
if conf.HIL
% add path for Hardware In the Loop
diff --git a/simulator/configs/configReferences.m b/simulator/configs/configReferences.m
index 9dd6d2261b77a2947eaa487399224b53a0f442e7..8f0f50920f6c8c96a9270a4f713e0f3a22f21009 100644
--- a/simulator/configs/configReferences.m
+++ b/simulator/configs/configReferences.m
@@ -109,7 +109,7 @@ switch mission.name
end
case '2024_Lyra_Roccaraso_September'
- load("TrajectoriesCFD.mat")
+ load("Trajectories.mat")
for i = 1:size(trajectories_saving,1)
for j = 1:size(trajectories_saving,2)
reference.vz_ref{i,j} = trajectories_saving{i,j}.VZ_ref;
diff --git a/simulator/mainMontecarlo.m b/simulator/mainMontecarlo.m
index e75023d1deb2f8eade3957f9bfb34b952716f18e..d66be5168345176f0211068092ce492f54358733 100644
--- a/simulator/mainMontecarlo.m
+++ b/simulator/mainMontecarlo.m
@@ -314,7 +314,25 @@ for alg_index = 4
p_50 = normcdf([settings.z_final-50, settings.z_final+50],apogee.altitude_mean,apogee.altitude_std);
apogee.accuracy_gaussian_50 =( p_50(2) - p_50(1) )*100;
-
+ %%
+ clc
+ for ii = 1:N_sim
+ if sum(save_thrust{ii}.sensors.ada.apo_times == -1) > 1
+ disp("Problems with ADA " + num2str(ii));
+ end
+ end
+
+ %%
+
+ ada_diff = zeros(N_sim, 1);
+ for ii = 1:N_sim
+ if save_thrust{ii}.sensors.old_ada.t_apogee ~= -1
+ ada_diff(ii) = save_thrust{ii}.sensors.ada.t_apogee - save_thrust{ii}.sensors.old_ada.t_apogee;
+ else
+ ada_diff(ii) = -1;
+ end
+ end
+
%% PLOTS
plotsMontecarlo;
@@ -343,6 +361,9 @@ for alg_index = 4
case {'2024_Lyra_Portugal_October', '2024_Lyra_Roccaraso_September'}
folder = [folder ; "MontecarloResults\"+mission.name+"\"+contSettings.algorithm+"\"+num2str(N_sim)+"sim_Mach"+num2str(100*rocket.airbrakes.maxMach)+"_"+simulationType_thrust+"_"+saveDate]; % offline
+
+ case {'2025_Orion_Portugal_October', '2025_Orion_Roccaraso_September'}
+ folder = [folder ; "MontecarloResults\"+mission.name+"\"+contSettings.algorithm+"\"+num2str(N_sim)+"sim_Mach"+num2str(100*rocket.airbrakes.maxMach)+"_"+simulationType_thrust+"_"+saveDate]; % offline
end
end
diff --git a/simulator/mainSimulator.m b/simulator/mainSimulator.m
index ad5cdfc593c3fbf6a3b22f03c4217f6a3d8cc38a..9a8651c2f5e09ad3740cb1d0980e4d2ea75fe8d5 100644
--- a/simulator/mainSimulator.m
+++ b/simulator/mainSimulator.m
@@ -89,7 +89,7 @@ end
if ~exist("../commonFunctions/graphics/general-utilities/","dir")
warning('To export file you need to download the repository, read the README file in the folder')
end
-std_plots(simOutput,settings,contSettings,mission,rocket,environment)
+std_plots(simOutput,settings,contSettings,mission,environment)
sensor_plots(simOutput, environment, rocket, settings);
% report_plots(simOutput,settings,contSettings)
diff --git a/simulator/montecarlo/plotsMontecarlo.m b/simulator/montecarlo/plotsMontecarlo.m
index c20efe466ec02785142ba18d461cedcd0de437d6..2e16b9dcf7d71712db16bf73f9e46dca08fe3695 100644
--- a/simulator/montecarlo/plotsMontecarlo.m
+++ b/simulator/montecarlo/plotsMontecarlo.m
@@ -3,7 +3,7 @@ N_histCol = min(N_sim,25); % best looking if we don't go higher than 200, but if
sim_per_interval = N_sim/N_histCol;
%% PLOT HISTOGRAM
-montFigures.apogee_histogram_pdf = figure('Color','white','Name','Apogee histogram','Units','Normalized','Position',[0,0,1,1]);
+montFigures.apogee_histogram_pdf = figure('Color','white','Name','Apogee histogram','Units','Normalized','WindowState','maximized');
hold on; grid on;
@@ -16,7 +16,7 @@ ylabel('Number of apogees in the same interval')
title('Reached apogee distribution')
legend
-montFigures.apogee_histogram_cumulative = figure('Color','white','Name','Apogee histogram Cumulative','Units','Normalized','Position',[0,0,1,1]);
+montFigures.apogee_histogram_cumulative = figure('Color','white','Name','Apogee histogram Cumulative','Units','Normalized','WindowState','maximized');
hold on; grid on;
xline(settings.z_final-10, 'r--', 'LineWidth', 1,'DisplayName','-10m from target')
xline(settings.z_final+10, 'r--', 'LineWidth', 1,'DisplayName','+10m from target')
@@ -39,7 +39,7 @@ if ~(strcmp(contSettings.algorithm,'engine')||strcmp(contSettings.algorithm,'NoC
arb_deploy_time_MEAN = mean(arb_deploy_time_vec);
arb_deploy_time_MODE = mode(arb_deploy_time_vec);
% figure
-montFigures.arb_deploy_histogram_pdf = figure('Color','white','Name','Air brakes deployment histogram','Units','Normalized','Position',[0,0,1,1]);
+montFigures.arb_deploy_histogram_pdf = figure('Color','white','Name','Air brakes deployment histogram','Units','Normalized','WindowState','maximized');
histogram(arb_deploy_time_vec,N_histCol)
hold on; grid on;
xline(arb_deploy_time_MEAN,'r--')
@@ -49,7 +49,7 @@ montFigures.arb_deploy_histogram_pdf = figure('Color','white','Name','Air brakes
title("Airbrakes deployment time's distribution")
legend('Airbrakes time deploy','Mean', 'Median')
-montFigures.arb_deploy_histogram_cumulative = figure('Color','white','Name','Air brakes deployment histogram cumulative','Units','Normalized','Position',[0,0,1,1]);
+montFigures.arb_deploy_histogram_cumulative = figure('Color','white','Name','Air brakes deployment histogram cumulative','Units','Normalized','WindowState','maximized');
histogram(arb_deploy_time_vec,N_histCol,'Normalization','cdf')
hold on; grid on;
xline(arb_deploy_time_MEAN,'r--')
@@ -69,17 +69,17 @@ apogee_time_MEAN = mean(apogee_time_vec);
apogee_time_MODE = mode(apogee_time_vec);
% figure
-montFigures.apogee_time_histogram_pdf = figure('Color','white','Name','Apogee time histogram','Units','Normalized','Position',[0,0,1,1]);
+montFigures.apogee_time_histogram_pdf = figure('Color','white','Name','Apogee time histogram','Units','Normalized','WindowState','maximized');
histogram(apogee_time_vec,N_histCol,'DisplayName','Time')
hold on; grid on;
xline(apogee_time_MEAN,'r--','DisplayName','Average')
xline(apogee_time_MODE,'g--','DisplayName','Mode')
-xlabel('Apogee value (m)')
+xlabel('Apogee value (s)')
ylabel('Number of apogees in the same interval')
title('Apogee time distribution')
legend
-montFigures.apogee_time_histogram_cumulative = figure('Color','white','Name','Apogee time histogram cumulative','Units','Normalized','Position',[0,0,1,1]);
+montFigures.apogee_time_histogram_cumulative = figure('Color','white','Name','Apogee time histogram cumulative','Units','Normalized','WindowState','maximized');
histogram(apogee_time_vec,N_histCol,'Normalization','cdf','DisplayName','Time')
hold on; grid on;
xline(apogee_time_MEAN,'r--','DisplayName','Average')
@@ -88,9 +88,32 @@ xlabel('Apogee value (m)')
ylabel('Number of apogees in the same interval')
title('Cumulative apogee time')
legend
+
+%% PLOT COMPARISON BETWEEN RUN_ADA AND MAJORITY VOTING ADA
+
+if contSettings.run_old_ada
+ old_ada_apogee_time_vec = zeros(N_sim, 1);
+ for ii = 1:N_sim
+ old_ada_apogee_time_vec(ii) = save_thrust{ii}.sensors.old_ada.t_apogee;
+ end
+ old_ada_apo_time_mean = mean(old_ada_apogee_time_vec);
+
+ montFigures.apogee_time_ada_comparison = figure('Color','white','Name','Apogee time histogram cumulative','Units','Normalized','WindowState','maximized');
+ histogram(apogee_time_vec, N_histCol, 'DisplayName', 'Majority ADA')
+ hold on; grid on;
+ histogram(old_ada_apogee_time_vec, N_histCol, 'DisplayName', 'run\_ADA');
+ xline(apogee_time_MEAN,'r--','DisplayName','Majority ADA Average')
+ xline(old_ada_apo_time_mean,'r--','DisplayName','run\_ADA Average')
+ xlabel('Apogee value (s)')
+ ylabel('Number of apogees in the same interval')
+ title('Apogee time distribution')
+ legend
+
+end
+
%% PLOT APOGEE MONTECARLO STATISTICS
if settings.scenario ~= "descent"
-montFigures.montecarlo_apogee_statistics = figure('Color','white','Name','Monte Carlo apogee statistics','Units','Normalized','Position',[0,0,1,1]);
+montFigures.montecarlo_apogee_statistics = figure('Color','white','Name','Monte Carlo apogee statistics','Units','Normalized','WindowState','maximized');
subplot(2,1,1)
plot(1:N_sim,apogee_mu,'DisplayName','Increasing mean')
ylabel("Mean value")
@@ -103,7 +126,7 @@ end
%% PLOT LANDING MONTECARLO STATISTICS
if (settings.scenario == "descent" || settings.scenario == "full flight") && settings.parafoil
-montFigures.montecarlo_landing_statistics = figure('Color','white','Name','Monte Carlo landing statistics','Units','Normalized','Position',[0,0,1,1]);
+montFigures.montecarlo_landing_statistics = figure('Color','white','Name','Monte Carlo landing statistics','Units','Normalized','WindowState','maximized');
subplot(2,1,1)
plot(1:N_sim,landing_mu,'DisplayName','Increasing mean')
ylabel("Mean value")
@@ -115,7 +138,7 @@ montFigures.montecarlo_landing_statistics = figure('Color','white','Name','Monte
end
%% PLOT CONTROL
-montFigures.control_action = figure('Color','white','Name','Control action','Units','Normalized','Position',[0,0,1,1]);
+montFigures.control_action = figure('Color','white','Name','Control action','Units','Normalized','WindowState','maximized');
for i = floor(linspace(1,N_sim,5))
plot(save_thrust{i}.t,save_thrust{i}.Y(:,14))
hold on; grid on;
@@ -126,7 +149,7 @@ ylabel('Servo angle \alpha (rad)')
legend(contSettings.algorithm);
%% PLOT APOGEE wrt THRUST
-montFigures.apogee_wrt_thrust = figure('Color','white','Name','Apogee wrt thrust','Units','Normalized','Position',[0,0,1,1]);
+montFigures.apogee_wrt_thrust = figure('Color','white','Name','Apogee wrt thrust','Units','Normalized','WindowState','maximized');
hold on; grid on;
scatter(thrust_percentage,apogee.altitude,'.')
yline(settings.z_final-10,'r--')
@@ -139,7 +162,7 @@ ylim([min(apogee.altitude)-20 max(apogee.altitude)+20])
legend(contSettings.algorithm);
%% PLOT APOGEE wrt MASS OFFSET
-montFigures.apogee_wrt_mass_offset = figure('Color','white','Name','Apogee wrt mass offset','Units','Normalized','Position',[0,0,1,1]);
+montFigures.apogee_wrt_mass_offset = figure('Color','white','Name','Apogee wrt mass offset','Units','Normalized','WindowState','maximized');
hold on; grid on;
scatter(stoch.mass_offset,apogee.altitude,'.')
yline(settings.z_final-10,'r--')
@@ -152,7 +175,7 @@ ylim([min(apogee.altitude)-20 max(apogee.altitude)+20])
legend(contSettings.algorithm);
%% PLOT APOGEE wrt RAIL OMEGA
-montFigures.apogee_wrt_rail_OMEGA= figure('Color','white','Name','Apogee wrt rail elevation','Units','Normalized','Position',[0,0,1,1]);
+montFigures.apogee_wrt_rail_OMEGA= figure('Color','white','Name','Apogee wrt rail elevation','Units','Normalized','WindowState','maximized');
hold on; grid on;
scatter(rad2deg(stoch.OMEGA_rail),apogee.altitude,'.')
yline(settings.z_final-10,'r--')
@@ -167,19 +190,19 @@ legend(contSettings.algorithm);
%% PLOT SHUTDOWN TIME 2D
%%% t_shutdown histogram
if settings.scenario~= "descent"
- montFigures.t_shutdown_histogram_pdf = figure('Color','white','Name','Shutdown time histogram','Units','Normalized','Position',[0,0,1,1]);
+ montFigures.t_shutdown_histogram_pdf = figure('Color','white','Name','Shutdown time histogram','Units','Normalized','WindowState','maximized');
histogram(t_shutdown.value,N_histCol)
xlabel('Shutdown time (s)')
ylabel('Number of shutdowns in the same time interval')
title('Engine shutdown time distribution')
- montFigures.t_shutdown_histogram_cumulative = figure('Color','white','Name','Shutdown time histogram cumulative','Units','Normalized','Position',[0,0,1,1]);
+ montFigures.t_shutdown_histogram_cumulative = figure('Color','white','Name','Shutdown time histogram cumulative','Units','Normalized','WindowState','maximized');
histogram(t_shutdown.value,N_histCol,'Normalization','cdf')
xlabel('Shutdown time (s)')
ylabel('Shutdown time cdf')
title('Engine shutdown time cumulative distribution')
%%% t_shutdown wrt wind
- montFigures.tShutdown_wind = figure('Color','white','Name','Shutdown time wrt wind','Units','Normalized','Position',[0,0,1,1]);
+ montFigures.tShutdown_wind = figure('Color','white','Name','Shutdown time wrt wind','Units','Normalized','WindowState','maximized');
subplot(1,3,1)
for i = 1:N_sim
plot(wind_el(i),save_thrust{i}.sensors.mea.t_shutdown,'.')
@@ -238,7 +261,7 @@ end
%% Predicted apogee
if (strcmp(contSettings.algorithm,'engine') || strcmp(contSettings.algorithm,'complete'))
- montFigures.apogee_prediction = figure('Color','white','Name','Apogee prediction','Units','Normalized','Position',[0,0,1,1]);
+ montFigures.apogee_prediction = figure('Color','white','Name','Apogee prediction','Units','Normalized','WindowState','maximized');
scatter(apogee.prediction_last_time,apogee.prediction,'k','DisplayName','Prediction');
hold on; grid on;
scatter(apogee.prediction_last_time,apogee.altitude,'r','DisplayName','Simulated');
@@ -249,7 +272,7 @@ if (strcmp(contSettings.algorithm,'engine') || strcmp(contSettings.algorithm,'co
end
%% PLOT APOGEE 3D
-montFigures.apogee_3D_wind_thrust = figure('Color','white','Name','Apogee 3D wind thrust','Units','Normalized','Position',[0,0,1,1]);
+montFigures.apogee_3D_wind_thrust = figure('Color','white','Name','Apogee 3D wind thrust','Units','Normalized','WindowState','maximized');
%%%%%%%%%% wind magnitude - thrust - apogee
subplot(2,2,1)
hold on; grid on;
@@ -327,7 +350,7 @@ for i =1:N_sim
max_force_kg(i) = max(abs(force))/9.81;
end
-montFigures.dynamic_pressure_and_forces = figure('Color','white','Name','Dynamic pressure and forces','Units','Normalized','Position',[0,0,1,1]);
+montFigures.dynamic_pressure_and_forces = figure('Color','white','Name','Dynamic pressure and forces','Units','Normalized','WindowState','maximized');
%%%%%%%%%%% max dynamic pressure (Pa)
subplot(2,1,1)
histogram(qdyn_max,N_histCol)
@@ -348,7 +371,7 @@ for i = 1:length(save_thrust)
est_mass(i) = save_thrust{i}.sensors.mea.mass(end);
true_mass(i) = save_thrust{i}.sensors.mea.true_mass_at_shutdown;
end
-montFigures.estimated_mass_histogram_pdf = figure('Color','white','Name','Estimated mass histogram','Units','Normalized','Position',[0,0,1,1]);
+montFigures.estimated_mass_histogram_pdf = figure('Color','white','Name','Estimated mass histogram','Units','Normalized','WindowState','maximized');
histogram(est_mass,N_histCol)
hold on;
histogram(true_mass,N_histCol,'DisplayName','Simulated')
@@ -357,7 +380,7 @@ xlabel('Mass [kg]')
ylabel('Number of simulations')
title('Estimated final mass')
-montFigures.estimated_mass_histogram_cumulative = figure('Color','white','Name','Estimated mass histogram cumulative','Units','Normalized','Position',[0,0,1,1]);
+montFigures.estimated_mass_histogram_cumulative = figure('Color','white','Name','Estimated mass histogram cumulative','Units','Normalized','WindowState','maximized');
histogram(est_mass,N_histCol,'DisplayName','Estimated','Normalization','cdf')
hold on;
histogram(true_mass,N_histCol,'DisplayName','Simulated','Normalization','cdf')
@@ -377,7 +400,7 @@ for ii = 1:N_sim
end
%%%%%%%%%%%%%%%% landing position w.r.t. target
-montFigures.landing_ellipses = figure('Color','white','Name','Landing ellipses','Units','Normalized','Position',[0,0,1,1]);
+montFigures.landing_ellipses = figure('Color','white','Name','Landing ellipses','Units','Normalized','WindowState','maximized');
scatter(landing.position(:,1),landing.position(:,2),'k.','DisplayName','Landings')
hold on;
scatter(settings.payload.target(1),settings.payload.target(2),'DisplayName','Target')
@@ -394,7 +417,7 @@ legend
%% Parafoil trajectories
-montFigures.prf_trajs = figure('Color','white','Name','Parafoil trajectories','Units','Normalized','Position',[0,0,1,1]);
+montFigures.prf_trajs = figure('Color','white','Name','Parafoil trajectories','Units','Normalized','WindowState','maximized');
for ii = 1:N_sim
plot3(save_thrust{ii}.Y(idx_dep:end, 2), save_thrust{ii}.Y(idx_dep:end, 1), -save_thrust{ii}.Y(idx_dep:end, 3));
hold on;
@@ -409,7 +432,7 @@ title('Parafoil trajectories')
% useful for realistic opening loads
% TODO:
% - modify into histogram
-montFigures.apogee_velocity = figure('Color','white','Name','Apogee velocity','Units','Normalized','Position',[0,0,1,1]);
+montFigures.apogee_velocity = figure('Color','white','Name','Apogee velocity','Units','Normalized','WindowState','maximized');
subplot(3,1,1)
plot(apogee.horizontalSpeedX)
title('vn')
diff --git a/simulator/src/std_plots.m b/simulator/src/std_plots.m
index e7ac91ccb59ee41d2f6cad4a63eee23138b6ff1b..627ce6b7bd50e82089d8369dd834bcb7c0584ae2 100644
--- a/simulator/src/std_plots.m
+++ b/simulator/src/std_plots.m
@@ -1,4 +1,4 @@
-function std_plots(simOutput, settings,contSettings,mission,rocket,environment)
+function std_plots(simOutput, settings,contSettings,mission,environment)
if ~exist("report_images\"+mission.name,"dir")
mkdir("report_images\"+mission.name)
@@ -69,25 +69,64 @@ end
drawnow
-%% ADA
-figures.ada = figure('Name', 'ADA vs trajectory');
-plot( simOutput.sensors.ada.time, simOutput.sensors.ada.xv(:,1),'DisplayName','$ADA_{z}$')
-hold on
-plot( simOutput.sensors.ada.time, simOutput.sensors.ada.xv(:,2),'DisplayName','$ADA_{vz}$')
-plot( simOutput.t, -simOutput.Y(:,3),'DisplayName','True z')
-plot( simOutput.t, -v_ned(:,3),'DisplayName','True Vz')
-legend
-title('ADA vs trajectory')
+%% ada
+figures.ada = figure('Name', 'ADA vs Trajectory');
+hold on; grid on;
+for ii = 1:contSettings.ADA_N_instances
+ plot(simOutput.sensors.ada.time, simOutput.sensors.ada.data{ii}.xv(:,1),'DisplayName',strcat('$ADA\_', num2str(ii), '_{z}$'));
+ plot(simOutput.sensors.ada.time, simOutput.sensors.ada.data{ii}.xv(:,2),'DisplayName',strcat('$ADA\_', num2str(ii), '_{vz}$'));
+end
+plot(simOutput.t, -simOutput.Y(:,3), 'DisplayName','True z');
+plot(simOutput.t, -v_ned(:,3), 'DisplayName','True Vz');
+legend('Interpreter','latex');
+title('ADA vs Trajectory');
xlabel("Time [s]"), ylabel("Altitude AGL \& Velocity [m, m/s]")
drawnow
figures.ADADer = figure('Name', 'ADA Derivatives');
-hold on
-plot(simOutput.sensors.ada.time, simOutput.sensors.ada.xp(:,2), 'DisplayName','ADA dp')
+hold on; grid on;
+for ii = 1:contSettings.ADA_N_instances
+ plot(simOutput.sensors.ada.time, simOutput.sensors.ada.data{ii}.xp(:,2), 'DisplayName', strcat('$ADA\_', num2str(ii), '\ dp$'))
+end
+legend('Interpreter','latex');
title('ADA pressure derivative')
xlabel("Time [s]"), ylabel("")
drawnow
+if contSettings.run_old_ada
+
+ figures.ADAComp = figure('Name', 'ADA Comparisons');
+ hold on; grid on;
+
+ plot(simOutput.sensors.ada.time, simOutput.sensors.old_ada.xv(:,1), 'LineWidth', 1.5, 'DisplayName', "$run\_ADA_{z}$");
+ plot(simOutput.sensors.ada.time, simOutput.sensors.old_ada.xv(:,2), 'LineWidth', 1.5, 'DisplayName', "$run\_ADA_{vz}$");
+ for ii = 1:contSettings.ADA_N_instances
+ plot(simOutput.sensors.ada.time, simOutput.sensors.ada.data{ii}.xv(:,1),'DisplayName',strcat('$ADA\_', num2str(ii), '_{z}$'));
+ plot(simOutput.sensors.ada.time, simOutput.sensors.ada.data{ii}.xv(:,2),'DisplayName',strcat('$ADA\_', num2str(ii), '_{vz}$'));
+ end
+ legend("Interpreter", "latex");
+ title("Comparison between run\_ADA and majority voting ADA");
+ drawnow
+
+ figures.ADAErr = figure('Name', 'ADA Absolute error wrt run_ADA');
+ subplot(2,1,1); hold on; grid on;
+ subplot(2,1,2); hold on; grid on;
+ for ii = 1:contSettings.ADA_N_instances
+ subplot(2,1,1);
+ plot(simOutput.sensors.ada.time, simOutput.sensors.old_ada.xv(:,1) - simOutput.sensors.ada.data{ii}.xv(:,1), 'DisplayName', strcat('$ADA\_', num2str(ii), '_{z}$'));
+ % disp("ADA " + num2str(ii) + " mean z error: " + num2str(mean(simOutput.sensors.old_ada.xv(:,1) - simOutput.sensors.ada.data{ii}.xv(:,1))) + " m");
+ % disp("ADA " + num2str(ii) + " std z error: " + num2str(std(simOutput.sensors.old_ada.xv(:,1) - simOutput.sensors.ada.data{ii}.xv(:,1))) + " m");
+ subplot(2,1,2);
+ plot(simOutput.sensors.ada.time, simOutput.sensors.old_ada.xv(:,2) - simOutput.sensors.ada.data{ii}.xv(:,2), 'DisplayName', strcat('$ADA\_', num2str(ii), '_{vz}$'));
+ % disp("ADA " + num2str(ii) + " mean vz error: " + num2str(mean(simOutput.sensors.old_ada.xv(:,2) - simOutput.sensors.ada.data{ii}.xv(:,2))) + " m/s");
+ % disp("ADA " + num2str(ii) + " std vz error: " + num2str(std(simOutput.sensors.old_ada.xv(:,2) - simOutput.sensors.ada.data{ii}.xv(:,2))) + " m/s");
+ end
+ subplot(2,1,1); legend("Interpreter", "latex");
+ subplot(2,1,2); legend("Interpreter", "latex");
+ sgtitle("Absolute error between run\_ADA and majority voting ADA instances");
+ drawnow
+end
+
%% reference
figures.NASABKRef = figure('Name', 'NAS vs ABK reference');
yyaxis left
diff --git a/simulator/src/std_run.m b/simulator/src/std_run.m
index 31c5728427df323545039aefc249cf6b9eb1f273..b1685d21d1b20532597f3fffd0a430adb750c9cc 100644
--- a/simulator/src/std_run.m
+++ b/simulator/src/std_run.m
@@ -613,6 +613,7 @@ struct_out.wind = wind;
% sensors (ADA, NAS, MEA, SFD, and all sensor data are stored here)
struct_out.sensors = sensorTot;
struct_out.sensors.ada.t_apogee = settings.ada.t_ada;
+struct_out.sensors.ada.t_para = settings.ada.t_para;
struct_out.sensors.nas.t_apogee = settings.nas.t_nas;
if settings.scenario ~= "descent"
struct_out.sensors.mea.mass_offset = settings.mass_offset;
@@ -689,7 +690,15 @@ if settings.montecarlo
struct_out.Y = interp1(struct_out.t,struct_out.Y,t_vec);
% sensors - ADA
- struct_out.sensors.ada = rmfield(struct_out.sensors.ada,{'time','n_old','xp','xv'});
+ apogee_idxs = sum(~struct_out.sensors.ada.flagApogee) + (struct_out.sensors.ada.flagApogee(end,:) ~= 0);
+ struct_out.sensors.ada.apo_times = struct_out.sensors.ada.time(apogee_idxs);
+ struct_out.sensors.ada.apo_times(struct_out.sensors.ada.flagApogee(end,:) == 0) = -1;
+ struct_out.sensors.ada = rmfield(struct_out.sensors.ada,{'time','n_old','flagApogee','flagOpenPara', 'data'});
+
+ if contSettings.run_old_ada
+ struct_out.sensors.old_ada.t_apogee = struct_out.sensors.old_ada.t_ada;
+ struct_out.sensors.old_ada = rmfield(struct_out.sensors.old_ada, {'flagApogee', 'flagOpenPara', 'xp', 'xv', 't_ada'});
+ end
% sensors - NAS
diff --git a/simulator/src/std_run_parts/std_setInitialParams.m b/simulator/src/std_run_parts/std_setInitialParams.m
index cc50102af4f164b47aa51fd8392aa1c4dd696c7a..f7bda1ae61c99ccd468825f9d5735a08604fff44 100644
--- a/simulator/src/std_run_parts/std_setInitialParams.m
+++ b/simulator/src/std_run_parts/std_setInitialParams.m
@@ -17,9 +17,6 @@ end
%% ADA Initialization
-settings.ada.counter = 0;
-settings.ada.paraCounter = 0;
-
settings.ada.t_ada = -1; % Apogee detection timestamp
settings.ada.flag_apo = false; % True when the apogee is detected
settings.ada.flagOpenPara = false; % True when the main parachute should be opened
@@ -132,24 +129,47 @@ sfd_mean_p = [];
%% ADA initial conditions (Apogee Detection Algorithm)
+% First we need to check that the number of barometer is sufficient for all
+% the instances of the ada we want to run
+if length(sensorData.barometer_sens) < contSettings.ADA_N_instances
+ error("The number of barometer is not sufficient for all the ADA instances to be run!");
+end
+
+% If we are simulating only the descent the initial condition need to match
+% the simulation one
if strcmp(settings.scenario, 'descent')
[~, ~, p_fin, ~] = computeAtmosphericData(settings.z_final+environment.z0); % Reference temperature in kelvin and pressure in Pa
- settings.ada.v0 = -10; % Vertical velocity initial condition
- settings.ada.a0 = -100; % Acceleration velocity initial condition
+ settings.ada.v0 = 10; % Pressure derivative initial condition
+ settings.ada.a0 = 100; % Pressure second derivative initial condition
settings.ada.x0 = [p_fin, settings.ada.v0, settings.ada.a0];
- % Ada initial condition
+ settings.ada.flag_apo = true;
end
if settings.flagADA
- sensorData.ada.xp = settings.ada.x0;
- sensorData.ada.xv = [0 0];
- sensorData.ada.P = settings.ada.P0;
- sensorData.ada.time = 0;
+ % Initialize all instances of the algorithm
+ for ii = 1:contSettings.ADA_N_instances
+ sensorData.ada{ii}.counter = 0;
+ sensorData.ada{ii}.paraCounter = 0;
+ sensorData.ada{ii}.xp = settings.ada.x0;
+ sensorData.ada{ii}.xv = [0 0];
+ sensorData.ada{ii}.P = settings.ada.P0;
+ sensorData.ada{ii}.lastBaroTimestamp = 0;
+ end
+
+ sensorTot.ada.flagApogee = false(1, contSettings.ADA_N_instances);
+ sensorTot.ada.flagOpenPara = false(1, contSettings.ADA_N_instances);
+
+ if contSettings.run_old_ada
+ sensorData.old_ada = sensorData.ada{1};
+ sensorTot.old_ada.t_ada = -1;
+ sensorTot.old_ada.t_para = -1;
+ sensorTot.old_ada.flagApogee = false;
+ sensorTot.old_ada.flagOpenPara = false;
+ end
end
-% ada_prev = settings.ada.x0; % erano dentro all'if, son qui perché
-% vorrei eliminarli concettualmente (oggi: 12/09/23)
-% Pada_prev = settings.ada.P0;
+
+
%% NAS initial conditions (Navigation and Attitude System)
@@ -225,25 +245,25 @@ sensorTot.comb_chamber.time = 0;
sensorTot.imu.time = 0;
sensorTot.gps.time = 0;
sensorTot.pitot.time = 0;
-sensorTot.nas.time = 0;
sensorTot.ada.time = 0;
+sensorTot.nas.time = 0;
sensorTot.mea.time = 0;
% initialization of the indexes
-sensorTot.barometer_sens{1}.n_old = 2;
-sensorTot.barometer_sens{2}.n_old = 2;
-sensorTot.barometer_sens{3}.n_old = 2;
-sensorTot.barometer.n_old = 2;
-sensorTot.imu.n_old = 2;
-sensorTot.gps.n_old = 2;
-sensorTot.pitot.n_old = 2;
-sensorTot.comb_chamber.n_old = 2;
-sensorTot.ada.n_old = 2;
-sensorTot.nas.n_old = 2;
-sensorTot.mea.n_old = 2;
-sensorTot.sfd.n_old = 2;
-sensorTot.gps.lastindex = 0;
-sensorTot.pitot.lastindex = 0;
+sensorTot.barometer_sens{1}.n_old = 2;
+sensorTot.barometer_sens{2}.n_old = 2;
+sensorTot.barometer_sens{3}.n_old = 2;
+sensorTot.barometer.n_old = 2;
+sensorTot.imu.n_old = 2;
+sensorTot.gps.n_old = 2;
+sensorTot.pitot.n_old = 2;
+sensorTot.comb_chamber.n_old = 2;
+sensorTot.ada.n_old = 2;
+sensorTot.nas.n_old = 2;
+sensorTot.mea.n_old = 2;
+sensorTot.sfd.n_old = 2;
+sensorTot.gps.lastindex = 0;
+sensorTot.pitot.lastindex = 0;
% initialization params
flagFlight = false;
diff --git a/simulator/src/std_run_parts/std_subsystems.m b/simulator/src/std_run_parts/std_subsystems.m
index c69c8513739f3e487f61c012bcf3912b0f35f10b..1cd970279f1c746a0125b6fafe78473e53c144b1 100644
--- a/simulator/src/std_run_parts/std_subsystems.m
+++ b/simulator/src/std_run_parts/std_subsystems.m
@@ -8,7 +8,11 @@ This function runs all subsystems in a simulated environment
%% ADA
if settings.flagADA && settings.dataNoise
- [sensorData, sensorTot, settings.ada, flagApogee, flagOpenPara] = run_ADA(sensorData, sensorTot, settings,t1);
+ if contSettings.run_old_ada
+ [sensorData, sensorTot] = run_ADA(sensorData, sensorTot, settings,t1);
+ end
+
+ [sensorData, sensorTot, settings.ada, flagApogee, flagOpenPara] = run_Majority_ADA(Tf, contSettings.ADA_N_instances, settings.ada, settings.frequencies.ADAFrequency, sensorData, sensorTot, currentState, engineT0);
end
%% Navigation system (NAS)