From 5e86597339f08df72291a9005afd595445ad7e44 Mon Sep 17 00:00:00 2001
From: "Pier Francesco A. Bachini" <pierfrancesco.bachini@skywarder.eu>
Date: Wed, 19 Feb 2025 23:46:04 +0100
Subject: [PATCH] [ADA] Add posibility to have run_ADA also running
---
commonFunctions/kalman/run_ADA.m | 57 +++++++++----------
simulator/configs/configControl.m | 5 ++
simulator/src/std_plots.m | 37 +++++++++++-
simulator/src/std_run.m | 10 +++-
.../src/std_run_parts/std_setInitialParams.m | 7 +++
5 files changed, 84 insertions(+), 32 deletions(-)
diff --git a/commonFunctions/kalman/run_ADA.m b/commonFunctions/kalman/run_ADA.m
index 86c6a974..bd881db7 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/simulator/configs/configControl.m b/simulator/configs/configControl.m
index 57069d57..5ae96e56 100644
--- a/simulator/configs/configControl.m
+++ b/simulator/configs/configControl.m
@@ -23,6 +23,11 @@ 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
diff --git a/simulator/src/std_plots.m b/simulator/src/std_plots.m
index 5b39273a..b061b3a9 100644
--- a/simulator/src/std_plots.m
+++ b/simulator/src/std_plots.m
@@ -84,7 +84,7 @@ xlabel("Time [s]"), ylabel("Altitude AGL \& Velocity [m, m/s]")
drawnow
figures.ADADer = figure('Name', 'ADA Derivatives');
-hold on
+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
@@ -93,6 +93,41 @@ title('ADA pressure derivative')
xlabel("Time [s]"), ylabel("")
drawnow
+if contSettings.run_old_ada
+
+ figure('Position', [100,100,600,400]);
+ 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");
+
+ figure('Position', [100,100,600,400]);
+ 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");
+end
+
+return
+
+
%% 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 31c57284..d7e01a61 100644
--- a/simulator/src/std_run.m
+++ b/simulator/src/std_run.m
@@ -689,7 +689,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 e70598dd..0bc1ff2d 100644
--- a/simulator/src/std_run_parts/std_setInitialParams.m
+++ b/simulator/src/std_run_parts/std_setInitialParams.m
@@ -158,6 +158,13 @@ if settings.flagADA
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.flagApogee = false;
+ sensorTot.old_ada.flagOpenPara = false;
+ end
end
--
GitLab