From ae13cf7ef2c518a0967e51fd63021d0f9fba35ba Mon Sep 17 00:00:00 2001
From: Alessandro Donadi <48148478+dondi01@users.noreply.github.com>
Date: Sun, 17 Sep 2023 11:53:42 +0200
Subject: [PATCH] fixed bug in weighted average
---
simulator/configs/configFaults.m | 40 +++-
simulator/configs/configFlags.m | 2 +-
simulator/mainSimulator.m | 2 +-
simulator/src/sensor_plots.m | 28 ++-
simulator/src/std_plots.m | 224 +++++++++---------
simulator/src/std_run.m | 7 +
.../src/std_run_parts/std_setInitialParams.m | 4 +-
simulator/src/std_run_parts/std_subsystems.m | 3 +-
8 files changed, 178 insertions(+), 132 deletions(-)
diff --git a/simulator/configs/configFaults.m b/simulator/configs/configFaults.m
index 1e19ec01..2ca3775a 100644
--- a/simulator/configs/configFaults.m
+++ b/simulator/configs/configFaults.m
@@ -31,17 +31,35 @@ max_degradation = 1300; %Pa
min_degradation = 200; %Pa
-offset_value = round((max_offset-min_offset)*rand() + min_offset);
-
-switch degradation_type
-
- case "offset"
-
- case "degradation"
-
- case "freezing"
-
-end
+setting.offset_value_1 = round((max_offset-min_offset)*rand() + min_offset);
+setting.offset_value_2 = round((max_offset-min_offset)*rand() + min_offset);
+setting.offset_value_3 = round((max_offset-min_offset)*rand() + min_offset);
+
+
+setting.degradation_value_1 = round((max_offset-min_offset)*rand() + min_offset);
+setting.degradation_value_2 = round((max_degradation-min_degradation)*rand() + min_degradation);
+setting.degradation_value_3 = round((max_degradation-min_degradation)*rand() + min_degradation);
+
+% for i = 1:3
+% rand_fault = randi(3);
+% switch rand_fault
+% case 1
+% setting.fault_type(i) = "offset";
+% case 2
+% setting.fault_type(i) = "degradation";
+% case 3
+% setting.fault_type(i) = "freezing";
+% end
+% end
+% switch fault_type
+%
+% case "offset"
+%
+% case "degradation"
+%
+% case "freezing"
+%
+% end
%% SENSOR FAULT DETECTION PARAMETERS
diff --git a/simulator/configs/configFlags.m b/simulator/configs/configFlags.m
index 0771a624..31c410a7 100644
--- a/simulator/configs/configFlags.m
+++ b/simulator/configs/configFlags.m
@@ -27,7 +27,7 @@ scenarios explanation:
% scenario configuration
-conf.scenario = "full flight";
+conf.scenario = "controlled ascent";
conf.board = "payload"; % Either "main" or "payload"
conf.HIL = false;
diff --git a/simulator/mainSimulator.m b/simulator/mainSimulator.m
index 2f1bc4e6..c01326ea 100644
--- a/simulator/mainSimulator.m
+++ b/simulator/mainSimulator.m
@@ -75,7 +75,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)
+%std_plots(simOutput,settings,contSettings)
sensor_plots(simOutput)
%% state visualiser
diff --git a/simulator/src/sensor_plots.m b/simulator/src/sensor_plots.m
index 4a133811..09ae4803 100644
--- a/simulator/src/sensor_plots.m
+++ b/simulator/src/sensor_plots.m
@@ -2,10 +2,11 @@ function sensor_plots(structIn)
figure('Position',[100,100,600,400])
hold on;
-plot(structIn.barometer_times,structIn.sfd_mean_p,'DisplayName','SFD output')
-plot(structIn.barometer_times,structIn.barometer_measures{1},'b','DisplayName','Baro 1')
+plot(structIn.barometer_times,structIn.sfd_mean_p,'DisplayName','SFD output after ADA')
+plot(structIn.barometer_times,structIn.sfd_mean_p_before_filter,'DisplayName','SFD output before filtering')
+%plot(structIn.barometer_times,structIn.barometer_measures{1},'b','DisplayName','Baro 1')
stairs(structIn.barometer_times,structIn.faults(:,1)*100000,'b--','DisplayName','Fault 1')
-plot(structIn.barometer_times,structIn.barometer_measures{2},'k','DisplayName','Baro 2')
+%plot(structIn.barometer_times,structIn.barometer_measures{2},'k','DisplayName','Baro 2')
stairs(structIn.barometer_times,structIn.faults(:,2)*100000,'k--','DisplayName','Fault 2')
plot(structIn.barometer_times,structIn.barometer_measures{3},'r','DisplayName','Baro 3')
stairs(structIn.barometer_times,structIn.faults(:,3)*100000,'r--','DisplayName','Fault 3')
@@ -26,4 +27,23 @@ hold on;
% subplot(3,1,2)
semilogy(f_2,x_2)
% subplot(3,1,3)
-semilogy(f_3,x_3)
\ No newline at end of file
+semilogy(f_3,x_3)
+%% Weight log plot
+figure
+hold on;
+plot(structIn.barometer_times, structIn.weight_log_W1(1,:), 'DisplayName','Weight Sensor 1 height')
+plot(structIn.barometer_times, structIn.weight_log_W1(2,:), 'DisplayName','Weight Sensor 2 height')
+plot(structIn.barometer_times, structIn.weight_log_W1(3,:), 'DisplayName','Weight Sensor 3 height')
+plot(structIn.barometer_times, structIn.weight_log_W2(1,:), 'DisplayName','Weight Sensor 1 baro')
+plot(structIn.barometer_times, structIn.weight_log_W2(2,:), 'DisplayName','Weight Sensor 2 baro')
+plot(structIn.barometer_times, structIn.weight_log_W2(3,:), 'DisplayName','Weight Sensor 3 baro')
+legend
+title('Weight of sensors for weighted mean')
+%% delay due to sfd filtering
+delta_baro = structIn.sfd_mean_p - structIn.sfd_mean_p_before_filter;
+figure
+hold on;
+plot(structIn.barometer_times,delta_baro,'DisplayName','SFD filter induced delay in Pa')
+
+
+
diff --git a/simulator/src/std_plots.m b/simulator/src/std_plots.m
index c08e7284..8e805713 100644
--- a/simulator/src/std_plots.m
+++ b/simulator/src/std_plots.m
@@ -229,122 +229,122 @@ plot( structIn.t_ada_tot, structIn.ADA(:,2),'DisplayName','ADA dp')
title('ADA pressure derivative')
%% quaternions
-figures.EulerAngles = figure('Name','Euler angles','Position',[100,100,600,400]);
-%
-subplot(2,2,1)
-plot(structIn.t,structIn.Y(:,10),'k','DisplayName','q_w');
-hold on;
-plot(structIn.t_nas,structIn.NAS(:,10),'r','DisplayName','q_w est');
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-legend
-ylabel('q_w')
-%
-subplot(2,2,2)
-plot(structIn.t,structIn.Y(:,11),'k','DisplayName','q_x');
-hold on;
-plot(structIn.t_nas,structIn.NAS(:,7),'r','DisplayName','q_x est');
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-legend
-ylabel('q_x')
-%
-subplot(2,2,3)
-plot(structIn.t,structIn.Y(:,12),'k','DisplayName','q_y');
-hold on;
-plot(structIn.t_nas,structIn.NAS(:,8),'r','DisplayName','q_y est');
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-legend
-ylabel('q_y')
-%
-subplot(2,2,4)
-plot(structIn.t,structIn.Y(:,13),'k','DisplayName','q_z');
-hold on;
-plot(structIn.t_nas,structIn.NAS(:,9),'r','DisplayName','q_z est');
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-legend
-ylabel('q_z')
-
-legend
-sgtitle('Euler angles')
-xlabel('Time (s)')
+% figures.EulerAngles = figure('Name','Euler angles','Position',[100,100,600,400]);
+% %
+% subplot(2,2,1)
+% plot(structIn.t,structIn.Y(:,10),'k','DisplayName','q_w');
+% hold on;
+% plot(structIn.t_nas,structIn.NAS(:,10),'r','DisplayName','q_w est');
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% legend
+% ylabel('q_w')
+% %
+% subplot(2,2,2)
+% plot(structIn.t,structIn.Y(:,11),'k','DisplayName','q_x');
+% hold on;
+% plot(structIn.t_nas,structIn.NAS(:,7),'r','DisplayName','q_x est');
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% legend
+% ylabel('q_x')
+% %
+% subplot(2,2,3)
+% plot(structIn.t,structIn.Y(:,12),'k','DisplayName','q_y');
+% hold on;
+% plot(structIn.t_nas,structIn.NAS(:,8),'r','DisplayName','q_y est');
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% legend
+% ylabel('q_y')
+% %
+% subplot(2,2,4)
+% plot(structIn.t,structIn.Y(:,13),'k','DisplayName','q_z');
+% hold on;
+% plot(structIn.t_nas,structIn.NAS(:,9),'r','DisplayName','q_z est');
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% legend
+% ylabel('q_z')
+%
+% legend
+% sgtitle('Euler angles')
+% xlabel('Time (s)')
%% euler angles
-eul = quat2eul(structIn.Y(:,10:13));
-eul = flip(eul,2);
-eul = unwrap(eul);
-eul = rad2deg(eul);
-eul_NAS = quat2eul(structIn.NAS(:,[10,7:9]));
-eul_NAS = flip(eul_NAS,2);
-eul_NAS = unwrap(eul_NAS);
-eul_NAS = rad2deg(eul_NAS);
-figures.EulerAngles = figure('Name','Euler angles','Position',[100,100,600,400]);
-%
-subplot(3,1,1)
-plot(structIn.t,eul(:,1),'DisplayName','\phi');
-hold on;
-plot(structIn.t_nas,eul_NAS(:,1),'DisplayName','\phi est');
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-legend
-ylabel('Roll (°)')
-%
-subplot(3,1,2)
-plot(structIn.t,eul(:,2),'DisplayName','\theta');
-hold on;
-plot(structIn.t_nas,eul_NAS(:,2),'DisplayName','\theta est');
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-legend
-ylabel('Pitch (°)')
-%
-subplot(3,1,3)
-plot(structIn.t,eul(:,3),'DisplayName','\psi');
-hold on;
-plot(structIn.t_nas,eul_NAS(:,3),'DisplayName','\psi est');
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-ylabel('Yaw (°)')
-legend
-sgtitle('Euler angles')
-xlabel('Time (s)')
-
+% eul = quat2eul(structIn.Y(:,10:13));
+% eul = flip(eul,2);
+% eul = unwrap(eul);
+% eul = rad2deg(eul);
+% eul_NAS = quat2eul(structIn.NAS(:,[10,7:9]));
+% eul_NAS = flip(eul_NAS,2);
+% eul_NAS = unwrap(eul_NAS);
+% eul_NAS = rad2deg(eul_NAS);
+% figures.EulerAngles = figure('Name','Euler angles','Position',[100,100,600,400]);
+% %
+% subplot(3,1,1)
+% plot(structIn.t,eul(:,1),'DisplayName','\phi');
+% hold on;
+% plot(structIn.t_nas,eul_NAS(:,1),'DisplayName','\phi est');
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% legend
+% ylabel('Roll (°)')
+% %
+% subplot(3,1,2)
+% plot(structIn.t,eul(:,2),'DisplayName','\theta');
+% hold on;
+% plot(structIn.t_nas,eul_NAS(:,2),'DisplayName','\theta est');
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% legend
+% ylabel('Pitch (°)')
+% %
+% subplot(3,1,3)
+% plot(structIn.t,eul(:,3),'DisplayName','\psi');
+% hold on;
+% plot(structIn.t_nas,eul_NAS(:,3),'DisplayName','\psi est');
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% ylabel('Yaw (°)')
+% legend
+% sgtitle('Euler angles')
+% xlabel('Time (s)')
+%
%% angular rotations
-figures.velocities = figure('Name', 'Angular rotations BODY','ToolBar','auto','Position',[100,100,600,400]);
-%
-subplot(3,1,1)
-plot(structIn.t, structIn.Y(:, 7),'DisplayName','p')
-hold on; grid on;
-if not(settings.scenario == "descent")
- xline(structIn.ARB_allowanceTime,'k--')
-end
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-xline(structIn.apogee_time,'r--','DisplayName','Apogee')
-ylabel('p [rad/s]');
-legend
-%
-subplot(3,1,2)
-plot(structIn.t, structIn.Y(:, 8),'DisplayName','q')
-hold on;
-if not(settings.scenario == "descent")
- xline(structIn.ARB_allowanceTime,'k--')
-end
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-xline(structIn.apogee_time,'r--','DisplayName','Apogee')
-ylabel('q [rad/s]');
-legend
-%
-subplot(3,1,3)
-plot(structIn.t, structIn.Y(:, 9),'DisplayName','r')
-hold on;
-if not(settings.scenario == "descent")
- xline(structIn.ARB_allowanceTime,'k--','DisplayName','Air brakes opening')
-end
-xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
-xline(structIn.apogee_time,'r--','DisplayName','Apogee')
-xlabel('Time [s]');
-ylabel('r [rad/s]');
-sgtitle('Angular rotations BODY');
-legend
-if settings.flagExportPLOTS == true
- exportStandardizedFigure(figures.velocities,"report_images\"+settings.mission+"\src_Angular_rotations_BODY.pdf",0.9)
-end
+% figures.velocities = figure('Name', 'Angular rotations BODY','ToolBar','auto','Position',[100,100,600,400]);
+% %
+% subplot(3,1,1)
+% plot(structIn.t, structIn.Y(:, 7),'DisplayName','p')
+% hold on; grid on;
+% if not(settings.scenario == "descent")
+% xline(structIn.ARB_allowanceTime,'k--')
+% end
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% xline(structIn.apogee_time,'r--','DisplayName','Apogee')
+% ylabel('p [rad/s]');
+% legend
+% %
+% subplot(3,1,2)
+% plot(structIn.t, structIn.Y(:, 8),'DisplayName','q')
+% hold on;
+% if not(settings.scenario == "descent")
+% xline(structIn.ARB_allowanceTime,'k--')
+% end
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% xline(structIn.apogee_time,'r--','DisplayName','Apogee')
+% ylabel('q [rad/s]');
+% legend
+% %
+% subplot(3,1,3)
+% plot(structIn.t, structIn.Y(:, 9),'DisplayName','r')
+% hold on;
+% if not(settings.scenario == "descent")
+% xline(structIn.ARB_allowanceTime,'k--','DisplayName','Air brakes opening')
+% end
+% xline(structIn.t(structIn.events.mainChuteIndex),'b--','DisplayName','Parafoil opening')
+% xline(structIn.apogee_time,'r--','DisplayName','Apogee')
+% xlabel('Time [s]');
+% ylabel('r [rad/s]');
+% sgtitle('Angular rotations BODY');
+% legend
+% if settings.flagExportPLOTS == true
+% exportStandardizedFigure(figures.velocities,"report_images\"+settings.mission+"\src_Angular_rotations_BODY.pdf",0.9)
+% end
end
diff --git a/simulator/src/std_run.m b/simulator/src/std_run.m
index b799e0b7..dcff2288 100644
--- a/simulator/src/std_run.m
+++ b/simulator/src/std_run.m
@@ -377,6 +377,9 @@ while settings.flagStopIntegration && n_old < nmax % St
barometer_time = [barometer_time t1];
sfd_mean_p = [sfd_mean_p sp.pn(end)];
faults = [faults; settings.faulty_sensors];
+ weight_log_W1 = [weight_log_W1, settings.sfd.W1];
+ weight_log_W2 = [weight_log_W2, settings.sfd.W2];
+ sfd_mean_p_before_filter = [sfd_mean_p_before_filter prev_filter_baro_data(end)];
n_old = n_old + n -1;
@@ -494,6 +497,10 @@ struct_out.barometer_measures = barometer_measure;
struct_out.barometer_times = barometer_time;
struct_out.sfd_mean_p = sfd_mean_p;
struct_out.faults = faults;
+struct_out.weight_log_W1 = weight_log_W1;
+struct_out.weight_log_W2 = weight_log_W2;
+struct_out.sfd_mean_p_before_filter = sfd_mean_p_before_filter;
+
if exist('t_airbrakes','var')
struct_out.ARB_allowanceTime = t_airbrakes;
struct_out.ARB_allowanceIdx = idx_airbrakes;
diff --git a/simulator/src/std_run_parts/std_setInitialParams.m b/simulator/src/std_run_parts/std_setInitialParams.m
index 55169848..a1ee221c 100644
--- a/simulator/src/std_run_parts/std_setInitialParams.m
+++ b/simulator/src/std_run_parts/std_setInitialParams.m
@@ -135,7 +135,9 @@ faults = [];
barometer_measure = cell(1,3);
barometer_time = [];
sfd_mean_p = [];
-
+weight_log_W1 = [];
+weight_log_W2 = [];
+sfd_mean_p_before_filter = [];
%% ADA initial conditions
if settings.flagADA
diff --git a/simulator/src/std_run_parts/std_subsystems.m b/simulator/src/std_run_parts/std_subsystems.m
index b2463af0..7635ff8b 100644
--- a/simulator/src/std_run_parts/std_subsystems.m
+++ b/simulator/src/std_run_parts/std_subsystems.m
@@ -11,8 +11,7 @@ This function runs all subsystems in a simulated environment
[sensorData,sp, settings] = run_SensorFaultDetection_SVM(settings, iTimes, sensorData,sp,t0);
-
-
+prev_filter_baro_data = sp.pn_unfiltered;
%% ADA
if iTimes>3
if settings.flagADA
--
GitLab