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..9dc87813718d226f8e5b378992b5a09b4f99dd52
--- /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_apo = 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/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/simulator/configs/configControl.m b/simulator/configs/configControl.m
index e2d962af0555b592b7c7aad8cf7c0a8fea433b42..57069d573bfdd6cc35420d1c210baf2a24664918 100644
--- a/simulator/configs/configControl.m
+++ b/simulator/configs/configControl.m
@@ -21,6 +21,14 @@ end
data = load(strcat(mission.dataPath, '/CAinterpCoeffs'));
contSettings.coeffs = data.coeffs;
+%% ADA Multiple instances parameters
+
+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 +105,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/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/src/std_plots.m b/simulator/src/std_plots.m
index e7ac91ccb59ee41d2f6cad4a63eee23138b6ff1b..5b39273afbfae885568dad6a4a1cc3401ee92fad 100644
--- a/simulator/src/std_plots.m
+++ b/simulator/src/std_plots.m
@@ -69,21 +69,26 @@ 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('Position',[100,100,600,400]);
+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')
+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
diff --git a/simulator/src/std_run_parts/std_setInitialParams.m b/simulator/src/std_run_parts/std_setInitialParams.m
index cc50102af4f164b47aa51fd8392aa1c4dd696c7a..e70598ddac422e5fe47b9dab9f20a304d24308ff 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,38 @@ 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.x0 = [p_fin, settings.ada.v0, settings.ada.a0];
- % Ada initial condition
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);
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 +236,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..025f9cebcc51e3302ac74f1ae8640e19377c597c 100644
--- a/simulator/src/std_run_parts/std_subsystems.m
+++ b/simulator/src/std_run_parts/std_subsystems.m
@@ -8,7 +8,9 @@ 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);
+ % [sensorData, sensorTot, settings.ada, flagApogee, flagOpenPara] = run_ADA(sensorData, sensorTot, settings,t1);
+
+ [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)