diff --git a/commonFunctions/kalman/run_MEA.m b/commonFunctions/kalman/run_MEA.m index cbf8e45d2b2acc7d9d9659031983ab04b3a434a3..a8a275748b1c5cf5167d347361b65d7ac388be33 100644 --- a/commonFunctions/kalman/run_MEA.m +++ b/commonFunctions/kalman/run_MEA.m @@ -50,7 +50,7 @@ for ii = 2:length(t_mea) P(:,:,ii) = A*P(:,:,ii-1)*A' + settings.mea.Q; % barometer correction - if sensorTot.comb_chamber.measures(index_chambPress) > 1 + if sensorTot.comb_chamber.measures(index_chambPress) > 1 S = C*P(:,:,ii)*C' + settings.mea.R; if ~det(S)<1e-3 K = P(:,:,ii)*C' / S; % if you want to try with constant gain [0.267161;-0.10199;-0.000205604 ]; @@ -60,7 +60,7 @@ for ii = 2:length(t_mea) end % accelerometer correction (not for 2023) - if contains(mission.name, '2024') + if contains(mission.name, '2024') || contains(mission.name, '2025') K_t = settings.mea.K_t; alpha = settings.mea.alpha; c = settings.mea.c; diff --git a/data/2024_Lyra_Portugal_October/config2024_Lyra_Portugal_October.m b/data/2024_Lyra_Portugal_October/config2024_Lyra_Portugal_October.m index 0f658f06f967baf5b53db722f9a557f6b48563db..478c0498a88e2e4fabb0a9f81ce895606c987f95 100644 --- a/data/2024_Lyra_Portugal_October/config2024_Lyra_Portugal_October.m +++ b/data/2024_Lyra_Portugal_October/config2024_Lyra_Portugal_October.m @@ -176,9 +176,12 @@ settings.mea.engine_model_C1 = [1.00196621875211 -0.485916431287183 0]; settings.mea.K_t = 105.2; % covariance matrices -settings.mea.Q = eye(3); % model noise covariance matrix -settings.mea.R = 0.36; -settings.mea.P0_mat = diag([0 0 0.36^2]); +% settings.mea.Q = eye(3); % model noise covariance matrix +% settings.mea.R = 0.36; +% settings.mea.P0_mat = diag([0 0 0.36^2]); +settings.mea.Q = diag([1 1 5]); % model noise covariance matrix +settings.mea.R = 3.6; +settings.mea.P0_mat = diag([0 0 3.6^2]); % shut down prediction altitude settings.mea.z_shutdown = 3200; % [m] target apogee prediction for shutdown @@ -186,11 +189,14 @@ settings.mea.t_lower_shadowmode = 0; % mi 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; + % accelerometer correction parameters -[~,~,settings.mea.P0] = computeAtmosphericData(103); %[Pa] reference pressure at the SFT location -settings.mea.acc_threshold = 40; %[m/s^2] minimum acceleration to perform correction with accelerometer -settings.mea.vel_threshold = 40; %[m/s] minimum velocity to perform correction with accelerometer -Rs = 1.0e+03*[0.4771 1.4391]; +[~,~,settings.mea.P0] = computeAtmosphericData(160); % [Pa] reference pressure at the SFT location +settings.mea.acc_threshold = 40; % [m/s^2] minimum acceleration to perform correction with accelerometer +settings.mea.vel_threshold = 40; % [m/s] minimum velocity to perform correction with accelerometer +% Rs = 1.0e+03*[0.4771 1.4391]; +Rs = 3.0e+03*[0.4771 1.4391]; + % variable variance coefficients for accelerometer settings.mea.alpha = (Rs(2) - Rs(1))/(100^2-30^2); settings.mea.c = -settings.mea.alpha*30^2+Rs(1); diff --git a/simulator/src/sensor_plots_MEA_test.m b/simulator/src/sensor_plots_MEA_test.m index 81c8e125a6748ac634ffc5f7e7aa7b1c092a9864..2f046b0a65ca174555c250a8dc17f2f0e77ea60e 100644 --- a/simulator/src/sensor_plots_MEA_test.m +++ b/simulator/src/sensor_plots_MEA_test.m @@ -1,27 +1,27 @@ function sensor_plots_MEA_test(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 -plot(simOutput.t, simOutput.recall.accelerations_body(:,1)./9.81); -legend("Accelerometer", "Real"); -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 -plot(simOutput.t, simOutput.recall.accelerations_body(:,2)./9.81); -legend("Accelerometer", "Real"); -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 -plot(simOutput.t, simOutput.recall.accelerations_body(:,3)./9.81); -legend("Accelerometer", "Real"); -xlabel("Time [s]"), ylabel("$a_Z$ [g]") -drawnow +% %% 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 +% plot(simOutput.t, simOutput.recall.accelerations_body(:,1)./9.81); +% legend("Accelerometer", "Real"); +% 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 +% plot(simOutput.t, simOutput.recall.accelerations_body(:,2)./9.81); +% legend("Accelerometer", "Real"); +% 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 +% plot(simOutput.t, simOutput.recall.accelerations_body(:,3)./9.81); +% legend("Accelerometer", "Real"); +% xlabel("Time [s]"), ylabel("$a_Z$ [g]") +% drawnow % %% Gyroscope measurements % figure('Name', "Gyroscope measurements") @@ -101,40 +101,40 @@ drawnow % 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)); -v_ned(1:drogue_idx,:) = quatrotate(quatconj(simOutput.Y(1:drogue_idx, 10:13)), simOutput.Y(1:drogue_idx, 4:6)); -if simOutput.state_lastTimes(6) == 0 - v_ned(drogue_idx+1:end,:) = simOutput.Y(drogue_idx+1:end,4:6); -else - prf_idx = sum(simOutput.t <= simOutput.state_lastTimes(4)); - v_ned(drogue_idx+1:prf_idx,:) = simOutput.Y(drogue_idx+1:prf_idx,4:6); - v_ned(prf_idx+1:end,:) = quatrotate(quatconj(simOutput.Y(prf_idx+1:end, 10:13)), simOutput.Y(prf_idx+1:end, 4:6)); -end - -figure("Name", "GPS Velocity measurements") -subplot(3,1,1) -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("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"); -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"); -xlabel("Time [s]"), ylabel("$V_D\ [m/s]$"); -drawnow +% %% Velocities +% % ODE velocity rotated in ned frame +% v_ned = zeros(length(simOutput.t), 3); +% drogue_idx = sum(simOutput.t <= simOutput.state_lastTimes(3)); +% v_ned(1:drogue_idx,:) = quatrotate(quatconj(simOutput.Y(1:drogue_idx, 10:13)), simOutput.Y(1:drogue_idx, 4:6)); +% if simOutput.state_lastTimes(6) == 0 +% v_ned(drogue_idx+1:end,:) = simOutput.Y(drogue_idx+1:end,4:6); +% else +% prf_idx = sum(simOutput.t <= simOutput.state_lastTimes(4)); +% v_ned(drogue_idx+1:prf_idx,:) = simOutput.Y(drogue_idx+1:prf_idx,4:6); +% v_ned(prf_idx+1:end,:) = quatrotate(quatconj(simOutput.Y(prf_idx+1:end, 10:13)), simOutput.Y(prf_idx+1:end, 4:6)); +% end +% +% figure("Name", "GPS Velocity measurements") +% subplot(3,1,1) +% 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("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"); +% 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"); +% xlabel("Time [s]"), ylabel("$V_D\ [m/s]$"); +% drawnow % %% Barometer measurements % [~, ~, P_real] = computeAtmosphericData(-simOutput.Y(:,3)+environment.z0); diff --git a/simulator/src/std_plots_MEA_test.m b/simulator/src/std_plots_MEA_test.m index 097dc9e2cc17432413296faa06c9f9288111a415..faecc01d65a7af5e194da76bac773aa046032fd4 100644 --- a/simulator/src/std_plots_MEA_test.m +++ b/simulator/src/std_plots_MEA_test.m @@ -45,20 +45,21 @@ if contains(mission.name,'2023') || contains(mission.name,'2024') || contains(mi sgtitle('MEA') subplot(2,1,1) - plot(simOutput.t, -simOutput.Y(:, 3),'DisplayName','Altitude'); - title('Predicted vs Real apogee'); + plot(simOutput.t, -simOutput.Y(:, 3),'DisplayName','Altitude') + title('Predicted vs Real apogee') hold on; grid on; - plot(simOutput.sensors.mea.time, simOutput.sensors.mea.prediction,'DisplayName','Prediction'); + plot(simOutput.sensors.mea.time, simOutput.sensors.mea.prediction,'DisplayName','Prediction') + xline(simOutput.sensors.mea.t_shutdown,'r--','DisplayName','MEA ShutDown') legend ylabel('Altitude AGL [m]') subplot(2,1,2) - plot(simOutput.sensors.mea.time, simOutput.sensors.mea.mass ,'DisplayName','Est mass'); - title('Estimated vs real mass'); + plot(simOutput.sensors.mea.time, simOutput.sensors.mea.mass,'DisplayName','Est mass') + title('Estimated vs real mass') hold on; - plot(simOutput.t, simOutput.recall.true_mass ,'DisplayName','True mass'); + plot(simOutput.t, simOutput.recall.true_mass,'DisplayName','True mass') + xline(simOutput.sensors.mea.t_shutdown,'r--','DisplayName','MEA ShutDown') legend - xline(simOutput.sensors.mea.t_shutdown,'r--') xlabel('Time [s]'); ylabel('Mass [kg]'); if settings.flagExportPLOTS == true @@ -69,17 +70,17 @@ 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') -xlabel("Time [s]"), ylabel("Altitude AGL \& Velocity [m, m/s]") -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') +% xlabel("Time [s]"), ylabel("Altitude AGL \& Velocity [m, m/s]") +% drawnow % figures.ADADer = figure('Name', 'ADA Derivatives'); % hold on @@ -221,131 +222,131 @@ drawnow % end % drawnow -%% Velocities BODY w.r.t. time against NAS -figures.velocities_BODY = figure('Name', 'Velocities BODY','ToolBar','auto'); -% -subplot(3,1,1) -plot(simOutput.t, simOutput.Y(:, 4),'DisplayName','$V_x$') -hold on; grid on; -plot(simOutput.sensors.nas.time, v_NAS_body(:, 1),'DisplayName','$V_x$ est') -if not(settings.scenario == "descent") - xline(simOutput.ARB.allowanceTime,'k--') -end -if settings.parafoil && (settings.scenario == "descent" || settings.scenario == "full flight") - xline(simOutput.t(simOutput.events.mainChuteIndex),'b--','DisplayName','Parafoil opening') -end -xline(simOutput.apogee.time,'r--','DisplayName','Apogee') -if any(~isnan(simOutput.sensors.nas.timestampPitotCorrection)) - xline(min(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Start pitot correction') - xline(max(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Stop pitot correction') -end -ylabel('$V_x$ [m/s]'); -legend -% -subplot(3,1,2) -plot(simOutput.t, simOutput.Y(:, 5),'DisplayName','$V_y$') -hold on; -plot(simOutput.sensors.nas.time, v_NAS_body(:, 2),'DisplayName','$V_y$ est') -if not(settings.scenario == "descent") - xline(simOutput.ARB.allowanceTime,'k--') -end -if settings.parafoil && (settings.scenario == "descent" || settings.scenario == "full flight") - xline(simOutput.t(simOutput.events.mainChuteIndex),'b--','DisplayName','Parafoil opening') -end -xline(simOutput.apogee.time,'r--','DisplayName','Apogee') -if any(~isnan(simOutput.sensors.nas.timestampPitotCorrection)) - xline(min(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Start pitot correction') - xline(max(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Stop pitot correction') -end -ylabel('$V_y$ [m/s]'); -legend -% -subplot(3,1,3) -plot(simOutput.t, simOutput.Y(:, 6),'DisplayName','$V_z$') -hold on; -plot(simOutput.sensors.nas.time, v_NAS_body(:, 3),'DisplayName','$V_z$ est') -if not(settings.scenario == "descent") - xline(simOutput.ARB.allowanceTime,'k--','DisplayName','Air brakes opening') -end -if settings.parafoil && (settings.scenario == "descent" || settings.scenario == "full flight") - xline(simOutput.t(simOutput.events.mainChuteIndex),'b--','DisplayName','Parafoil opening') -end -xline(simOutput.apogee.time,'r--','DisplayName','Apogee') -if any(~isnan(simOutput.sensors.nas.timestampPitotCorrection)) - xline(min(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Start pitot correction') - xline(max(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Stop pitot correction') -end -xlabel('Time [s]'); -ylabel('$V_z$ [m/s]'); -sgtitle('Velocities BODY'); -legend -if settings.flagExportPLOTS == true - exportStandardizedFigure(figures.velocities_BODY,"report_images\"+mission.name+"\src_velocities_BODY.pdf",0.9) -end -drawnow - -%% Velocities NED w.r.t. time against NAS -figures.velocities_NED = figure('Name', 'Velocities NED','ToolBar','auto'); -% -subplot(3,1,1) -plot(simOutput.t, v_ned(:, 1),'DisplayName','$V_n$') -hold on; grid on; -plot(simOutput.sensors.nas.time, simOutput.sensors.nas.states(:, 4),'DisplayName','$V_n$ est') -if not(settings.scenario == "descent") - xline(simOutput.ARB.allowanceTime,'k--') -end -ylabel('$V_x$ [m/s]'); -legend -% -subplot(3,1,2) -plot(simOutput.t, v_ned(:, 2),'DisplayName','$V_e$') -hold on; -plot(simOutput.sensors.nas.time,simOutput.sensors.nas.states(:, 5) ,'DisplayName','$V_e$ est') -if not(settings.scenario == "descent") - xline(simOutput.ARB.allowanceTime,'k--') -end -ylabel('$V_y$ [m/s]'); -legend -% -subplot(3,1,3) -plot(simOutput.t, v_ned(:, 3),'DisplayName','$V_d$') -hold on; -plot(simOutput.sensors.nas.time, simOutput.sensors.nas.states(:, 6),'DisplayName','$V_d$ est') -if not(settings.scenario == "descent") - xline(simOutput.ARB.allowanceTime,'k--','DisplayName','Air brakes opening') -end -xline(simOutput.apogee.time,'r--','DisplayName','Apogee') -xlabel('Time [s]'); -ylabel('$V_z$ [m/s]'); -sgtitle('Velocities NED'); -legend -if settings.flagExportPLOTS == true - exportStandardizedFigure(figures.velocities_NED,"report_images\"+mission.name+"\src_velocities_NED.pdf",0.9) -end -drawnow - -%% check consistency of NAS: -altitude = simOutput.sensors.nas.states(:,3)+environment.z0; -v_int_NAS = 0; -v_int_simulation = 0; -for i = 2:length(simOutput.sensors.nas.states(:,6)) - v_int_NAS(i) = v_int_NAS(i-1) + sum(simOutput.sensors.nas.states([i-1,i],6))/settings.frequencies.NASFrequency/2; -end +% %% Velocities BODY w.r.t. time against NAS +% figures.velocities_BODY = figure('Name', 'Velocities BODY','ToolBar','auto'); +% % +% subplot(3,1,1) +% plot(simOutput.t, simOutput.Y(:, 4),'DisplayName','$V_x$') +% hold on; grid on; +% plot(simOutput.sensors.nas.time, v_NAS_body(:, 1),'DisplayName','$V_x$ est') +% if not(settings.scenario == "descent") +% xline(simOutput.ARB.allowanceTime,'k--') +% end +% if settings.parafoil && (settings.scenario == "descent" || settings.scenario == "full flight") +% xline(simOutput.t(simOutput.events.mainChuteIndex),'b--','DisplayName','Parafoil opening') +% end +% xline(simOutput.apogee.time,'r--','DisplayName','Apogee') +% if any(~isnan(simOutput.sensors.nas.timestampPitotCorrection)) +% xline(min(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Start pitot correction') +% xline(max(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Stop pitot correction') +% end +% ylabel('$V_x$ [m/s]'); +% legend +% % +% subplot(3,1,2) +% plot(simOutput.t, simOutput.Y(:, 5),'DisplayName','$V_y$') +% hold on; +% plot(simOutput.sensors.nas.time, v_NAS_body(:, 2),'DisplayName','$V_y$ est') +% if not(settings.scenario == "descent") +% xline(simOutput.ARB.allowanceTime,'k--') +% end +% if settings.parafoil && (settings.scenario == "descent" || settings.scenario == "full flight") +% xline(simOutput.t(simOutput.events.mainChuteIndex),'b--','DisplayName','Parafoil opening') +% end +% xline(simOutput.apogee.time,'r--','DisplayName','Apogee') +% if any(~isnan(simOutput.sensors.nas.timestampPitotCorrection)) +% xline(min(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Start pitot correction') +% xline(max(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Stop pitot correction') +% end +% ylabel('$V_y$ [m/s]'); +% legend +% % +% subplot(3,1,3) +% plot(simOutput.t, simOutput.Y(:, 6),'DisplayName','$V_z$') +% hold on; +% plot(simOutput.sensors.nas.time, v_NAS_body(:, 3),'DisplayName','$V_z$ est') +% if not(settings.scenario == "descent") +% xline(simOutput.ARB.allowanceTime,'k--','DisplayName','Air brakes opening') +% end +% if settings.parafoil && (settings.scenario == "descent" || settings.scenario == "full flight") +% xline(simOutput.t(simOutput.events.mainChuteIndex),'b--','DisplayName','Parafoil opening') +% end +% xline(simOutput.apogee.time,'r--','DisplayName','Apogee') +% if any(~isnan(simOutput.sensors.nas.timestampPitotCorrection)) +% xline(min(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Start pitot correction') +% xline(max(simOutput.sensors.nas.timestampPitotCorrection(~isnan(simOutput.sensors.nas.timestampPitotCorrection))),'b--','Stop pitot correction') +% end +% xlabel('Time [s]'); +% ylabel('$V_z$ [m/s]'); +% sgtitle('Velocities BODY'); +% legend +% if settings.flagExportPLOTS == true +% exportStandardizedFigure(figures.velocities_BODY,"report_images\"+mission.name+"\src_velocities_BODY.pdf",0.9) +% end +% drawnow -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 +% %% Velocities NED w.r.t. time against NAS +% figures.velocities_NED = figure('Name', 'Velocities NED','ToolBar','auto'); +% % +% subplot(3,1,1) +% plot(simOutput.t, v_ned(:, 1),'DisplayName','$V_n$') +% hold on; grid on; +% plot(simOutput.sensors.nas.time, simOutput.sensors.nas.states(:, 4),'DisplayName','$V_n$ est') +% if not(settings.scenario == "descent") +% xline(simOutput.ARB.allowanceTime,'k--') +% end +% ylabel('$V_x$ [m/s]'); +% legend +% % +% subplot(3,1,2) +% plot(simOutput.t, v_ned(:, 2),'DisplayName','$V_e$') +% hold on; +% plot(simOutput.sensors.nas.time,simOutput.sensors.nas.states(:, 5) ,'DisplayName','$V_e$ est') +% if not(settings.scenario == "descent") +% xline(simOutput.ARB.allowanceTime,'k--') +% end +% ylabel('$V_y$ [m/s]'); +% legend +% % +% subplot(3,1,3) +% plot(simOutput.t, v_ned(:, 3),'DisplayName','$V_d$') +% hold on; +% plot(simOutput.sensors.nas.time, simOutput.sensors.nas.states(:, 6),'DisplayName','$V_d$ est') +% if not(settings.scenario == "descent") +% xline(simOutput.ARB.allowanceTime,'k--','DisplayName','Air brakes opening') +% end +% xline(simOutput.apogee.time,'r--','DisplayName','Apogee') +% xlabel('Time [s]'); +% ylabel('$V_z$ [m/s]'); +% sgtitle('Velocities NED'); +% legend +% if settings.flagExportPLOTS == true +% exportStandardizedFigure(figures.velocities_NED,"report_images\"+mission.name+"\src_velocities_NED.pdf",0.9) +% end +% drawnow -figures.AltNAS = figure('Name', 'Altitude NAS'); -plot(simOutput.sensors.nas.time,altitude,'DisplayName','Altitude NAS') -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') -legend -xlabel("Time [s]"), ylabel("-Altitude AGL [m]") -drawnow +% %% check consistency of NAS: +% altitude = simOutput.sensors.nas.states(:,3)+environment.z0; +% v_int_NAS = 0; +% v_int_simulation = 0; +% for i = 2:length(simOutput.sensors.nas.states(:,6)) +% v_int_NAS(i) = v_int_NAS(i-1) + sum(simOutput.sensors.nas.states([i-1,i],6))/settings.frequencies.NASFrequency/2; +% end +% +% 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 +% +% figures.AltNAS = figure('Name', 'Altitude NAS'); +% plot(simOutput.sensors.nas.time,altitude,'DisplayName','Altitude NAS') +% 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') +% legend +% xlabel("Time [s]"), ylabel("-Altitude AGL [m]") +% drawnow % %% euler angles % eul_NAS = quat2eul(simOutput.sensors.nas.states(:,[10,7:9]));