From 6b320c540e978f474519d5257c9f929e3f67f666 Mon Sep 17 00:00:00 2001
From: Stefano Belletti <stefano.belletti@skywarder.eu>
Date: Mon, 10 Feb 2025 16:44:29 +0100
Subject: [PATCH] Fix plots

---
 simulator/src/sensor_plots.m | 77 +++++++++++++++++++-----------------
 simulator/src/std_plots.m    | 26 ++++++------
 2 files changed, 53 insertions(+), 50 deletions(-)

diff --git a/simulator/src/sensor_plots.m b/simulator/src/sensor_plots.m
index a8d5774..5168e00 100644
--- a/simulator/src/sensor_plots.m
+++ b/simulator/src/sensor_plots.m
@@ -1,53 +1,52 @@
 function sensor_plots(simOutput, environment, rocket, settings)
 
 %% Accelerometer measurements
-
 figure('Name', "Accelerometer measurements")
 subplot(3,1,1)
 plot(simOutput.sensors.imu.time, simOutput.sensors.imu.accelerometer_measures(:,1)./9.81);
-hold on; grid on;
+hold on, grid on
 plot(simOutput.t, simOutput.recall.accelerations_body(:,1)./9.81);
 legend("Accelerometer", "Real");
-title("$a_X$ BODY $[g]$", 'Interpreter','latex');
+title("Acceleration BODY", 'Interpreter','latex');
+ylabel("$a_X$ [g]")
 subplot(3,1,2)
 plot(simOutput.sensors.imu.time, simOutput.sensors.imu.accelerometer_measures(:,2)./9.81);
-hold on; grid on;
+hold on, grid on
 plot(simOutput.t, simOutput.recall.accelerations_body(:,2)./9.81);
 legend("Accelerometer", "Real");
-title("$a_Y$ BODY $[g]$", 'Interpreter','latex');
+ylabel("$a_Y$ [g]")
 subplot(3,1,3)
 plot(simOutput.sensors.imu.time, simOutput.sensors.imu.accelerometer_measures(:,3)./9.81);
-hold on; grid on;
+hold on, grid on
 plot(simOutput.t, simOutput.recall.accelerations_body(:,3)./9.81);
 legend("Accelerometer", "Real");
-title("$a_Z$ BODY $[g]$", 'Interpreter','latex');
-xlabel("Time [s]")
+xlabel("Time [s]"), ylabel("$a_Z$ [g]")
+drawnow
 
 %% Gyroscope measurements
-
 figure('Name', "Gyroscope measurements")
 subplot(3,1,1)
 plot(simOutput.sensors.imu.time, simOutput.sensors.imu.gyro_measures(:,1));
-hold on; grid on;
+hold on, grid on
 plot(simOutput.t, simOutput.Y(:,7));
 legend("Gyroscope", "Real");
-title("$\omega_X$ BODY $[rad/s]$", 'Interpreter','latex');
+title("$\omega$ BODY");
+ylabel("$\omega_X$ $[rad/s]$")
 subplot(3,1,2)
 plot(simOutput.sensors.imu.time, simOutput.sensors.imu.gyro_measures(:,2));
-hold on; grid on;
+hold on, grid on
 plot(simOutput.t, simOutput.Y(:,8));
 legend("Gyroscope", "Real");
-title("$\omega_Y$ BODY $[rad/s]$", 'Interpreter','latex');
+ylabel("$\omega_Y$ $[rad/s]$")
 subplot(3,1,3)
 plot(simOutput.sensors.imu.time, simOutput.sensors.imu.gyro_measures(:,3));
-hold on; grid on;
+hold on, grid on
 plot(simOutput.t, simOutput.Y(:,9));
 legend("Gyroscope", "Real");
-title("$\omega_Z$ BODY $[rad/s]$", 'Interpreter','latex');
-xlabel("Time [s]")
+xlabel("Time [s]"), ylabel("$\omega_Z$ $[rad/s]$")
+drawnow
 
 %% Magnetometer measurements
-
 std_magneticField;
 magnFieldInertial = magneticFieldApprox(-simOutput.Y(:,3)+environment.z0);
 Q = simOutput.Y(:,10:13);
@@ -59,23 +58,23 @@ plot(simOutput.sensors.imu.time, simOutput.sensors.imu.magnetometer_measures(:,1
 hold on; grid on;
 plot(simOutput.t, magnFieldBody(:,1));
 legend("Magnetometer", "Real");
-title("Magnetic field BODY $X\ [\mu T]$", 'Interpreter','latex');
+title("Magnetic field BODY");
+ylabel("$X\ [\mu T]$");
 subplot(3,1,2)
 plot(simOutput.sensors.imu.time, simOutput.sensors.imu.magnetometer_measures(:,2)/1e1);
 hold on; grid on;
 plot(simOutput.t, magnFieldBody(:,2));
 legend("Magnetometer", "Real");
-title("Magnetic field BODY $Y\ [\mu T]$", 'Interpreter','latex');
+ylabel("$Y\ [\mu T]$");
 subplot(3,1,3)
 plot(simOutput.sensors.imu.time, simOutput.sensors.imu.magnetometer_measures(:,3)/1e1);
 hold on; grid on;
 plot(simOutput.t, magnFieldBody(:,3));
 legend("Magnetometer", "Real");
-title("Magnetic field BODY $Z\ [\mu T]$", 'Interpreter','latex');
-xlabel("Time [s]")
+xlabel("Time [s]"), ylabel("$Z\ [\mu T]$");
+drawnow
 
 %% GPS measurements
-
 % Positions
 [GPS_NED(:,1), GPS_NED(:,2), GPS_NED(:,3)] = geodetic2ned(simOutput.sensors.gps.position_measures(:,1), simOutput.sensors.gps.position_measures(:,2), simOutput.sensors.gps.position_measures(:,3), ...
     environment.lat0, environment.lon0, environment.z0, wgs84Ellipsoid);
@@ -86,23 +85,23 @@ plot(simOutput.sensors.gps.time, GPS_NED(:,1));
 hold on; grid on;
 plot(simOutput.t, simOutput.Y(:,1));
 legend("GPS", "Real");
-title("North [m]", 'Interpreter','latex');
+title("Positions");
+ylabel("North [m]");
 subplot(3,1,2)
 plot(simOutput.sensors.gps.time, GPS_NED(:,2));
 hold on; grid on;
 plot(simOutput.t, simOutput.Y(:,2));
 legend("GPS", "Real");
-title("East [m]", 'Interpreter','latex');
+ylabel("East [m]");
 subplot(3,1,3)
 plot(simOutput.sensors.gps.time, -GPS_NED(:,3));
 hold on; grid on;
 plot(simOutput.t, -simOutput.Y(:,3));
 legend("GPS", "Real");
-title("Up agl [m]", 'Interpreter','latex');
-xlabel("Time [s]")
-
-% Velocities
+xlabel("Time [s]"), ylabel("Up agl [m]");
+drawnow
 
+%% Velocities
 % ODE velocity rotated in ned frame
 v_ned = zeros(length(simOutput.t), 3);
 drogue_idx = sum(simOutput.t <= simOutput.state_lastTimes(3));
@@ -121,23 +120,23 @@ plot(simOutput.sensors.gps.time, simOutput.sensors.gps.velocity_measures(:,1));
 hold on; grid on;
 plot(simOutput.t, v_ned(:,1));
 legend("GPS", "Real");
-title("$V_N\ [m/s]$", 'Interpreter','latex');
+title("Velocities");
+ylabel("$V_N\ [m/s]$");
 subplot(3,1,2)
 plot(simOutput.sensors.gps.time, simOutput.sensors.gps.velocity_measures(:,2));
 hold on; grid on;
 plot(simOutput.t, v_ned(:,2));
 legend("GPS", "Real");
-title("$V_E\ [m/s]$", 'Interpreter','latex');
+ylabel("$V_E\ [m/s]$");
 subplot(3,1,3)
 plot(simOutput.sensors.gps.time, simOutput.sensors.gps.velocity_measures(:,3));
 hold on; grid on;
 plot(simOutput.t, v_ned(:,3));
 legend("GPS", "Real");
-title("$V_D\ [m/s]$", 'Interpreter','latex');
-xlabel("Time [s]")
+xlabel("Time [s]"), ylabel("$V_D\ [m/s]$");
+drawnow
 
 %% Barometer measurements
-
 [~, ~, P_real] = computeAtmosphericData(-simOutput.Y(:,3)+environment.z0);
 
 figure("Name", "Barometer measurements")
@@ -161,22 +160,26 @@ title('Pitot static pressure')
 xlabel("Time [s]"), ylabel("Pressure [Pa]")
 drawnow
 
-%% mea pressure vs true pressure
+%% MEA pressure vs true pressure
 if length(simOutput.sensors.mea.time) > 1
-
     dt = 1/settings.frequencies.controlFrequency;
     idx_t0 = sum(simOutput.t <= (simOutput.state_lastTimes(1)-dt));
     P_cc_real = interp1(rocket.motor.time, rocket.motor.thrust, simOutput.t(idx_t0:end)-(simOutput.state_lastTimes(1)-dt))/settings.motor.K;
     P_cc_real = [zeros(idx_t0-1,1); P_cc_real];
 
+    cut_idx = sum(simOutput.sensors.comb_chamber.time <= simOutput.state_lastTimes(2)+5);
     figure("Name","Combustion chamber pressure")
+    hold on
+    title("Combustion chamber pressure")
     plot(simOutput.sensors.mea.time,simOutput.sensors.mea.pressure,'DisplayName','Estimated pressure')
-    plot(simOutput.sensors.comb_chamber.time,simOutput.sensors.comb_chamber.measures,'DisplayName','Sensor')
+    plot(simOutput.sensors.comb_chamber.time(1:cut_idx),simOutput.sensors.comb_chamber.measures(1:cut_idx),'DisplayName','Sensor')
     plot(simOutput.t, P_cc_real, 'DisplayName', 'Real cc pressure');
-    legend()
+    legend
+    xlabel("Time [s]"), ylabel("Pressure [Pa]")
 end
 drawnow
 
+
 return
 
 %%
diff --git a/simulator/src/std_plots.m b/simulator/src/std_plots.m
index 8f7bd5c..44e1687 100644
--- a/simulator/src/std_plots.m
+++ b/simulator/src/std_plots.m
@@ -69,29 +69,30 @@ end
 
 drawnow
 
-%% ada
-figures.ada = figure;
+%% 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;
+plot( simOutput.t, -simOutput.Y(:,3),'DisplayName','True z')
+plot( simOutput.t, -v_ned(:,3),'DisplayName','True Vz')
+legend
 title('ADA vs trajectory')
 xlabel("Time [s]"), ylabel("Altitude AGL \& Velocity [m, m/s]")
 drawnow
 
-figure
+figures.ADADer = figure('Name', 'ADA Derivatives');
 hold on
-plot( simOutput.sensors.ada.time,  simOutput.sensors.ada.xp(:,2),'DisplayName','ADA dp')
+plot(simOutput.sensors.ada.time, simOutput.sensors.ada.xp(:,2), 'DisplayName','ADA dp')
 title('ADA pressure derivative')
-xlabel("Time [s]")
+xlabel("Time [s]"), ylabel("")
 drawnow
 
 %% reference
-figure
+figures.NASABKRef = figure('Name', 'NAS vs ABK reference');
 yyaxis left
 hold on
+title('NAS vs ABK reference');
 if ~settings.electronics
     contSettings = simOutput.contSettings; % because the trajectory are chosen during the simulation, not a priori
     if not(settings.scenario == "descent")
@@ -221,7 +222,6 @@ end
 drawnow
 
 %% Velocities BODY w.r.t. time against NAS
-
 figures.velocities_BODY = figure('Name', 'Velocities BODY','ToolBar','auto');
 %
 subplot(3,1,1)
@@ -285,7 +285,6 @@ end
 drawnow
 
 %% Velocities NED w.r.t. time against NAS
-
 figures.velocities_NED = figure('Name', 'Velocities NED','ToolBar','auto');
 %
 subplot(3,1,1)
@@ -337,9 +336,10 @@ for i = 2:length(simOutput.Y(:,6))
     v_int_simulation(i) = v_int_simulation(i-1) + sum(v_ned([i-1,i],3)*0.01)/2;
 end
 
-figure
+figures.AltNAS = figure('Name', 'Altitude NAS');
 plot(simOutput.sensors.nas.time,altitude,'DisplayName','Altitude NAS')
-hold on;
+hold on
+title("Altitude NAS and simulation")
 plot(simOutput.sensors.nas.time,v_int_NAS,'DisplayName','Velocity NAS integrated')
 plot(simOutput.t,simOutput.Y(:,3),'DisplayName','Altitude Simulation')
 plot(simOutput.t,v_int_simulation,'DisplayName','Velocity simulation integrated')
-- 
GitLab