diff --git a/RoccarasoFlight/postprocessing/GNC/postProcess.m b/RoccarasoFlight/postprocessing/GNC/postProcess.m index 1e3bd78240916f5ad176184a90948dd4d9eac18c..a0fa8e97a93587847f0406d2ef7e31b765d0fef7 100644 --- a/RoccarasoFlight/postprocessing/GNC/postProcess.m +++ b/RoccarasoFlight/postprocessing/GNC/postProcess.m @@ -109,7 +109,7 @@ title('Mass finite difference 1HZ vs venting valve') %% NAS obsw vs NAS risimulato % build sensors sensorTot.nas.time = main.NASData(:,1); -sensorTot.nas.states = [main.NASData(1,2:end-4), 0, 0.6428, 0,0.7660]; +sensorTot.nas.states = [main.NASData(1,2:end)]; sensorTot.nas.n_old = 2; sensorTot.nas.P = eye(12); sensorTot.nas.P(3,3) = 10; @@ -177,6 +177,8 @@ nas.flagPitotCorrection = false; nas.flagPitotCorrection = true; [sensorTot_WithPitot] = run_NAS(XYZ0*0.01,sensorTot,nas); +sensorTot_OBSW.nas.time = main.NASData(:,1); +sensorTot_OBSW.nas.states = main.NASData(:,2:end); Nr = 3; Nc = 2; @@ -383,7 +385,7 @@ ada.v_thr = 2.5; ada.count_thr = 5; [sensorTot,ada] = run_ADA(sensorTot,ada); -%% + Nr = 2; Nc = 2; @@ -455,7 +457,9 @@ mea.engine_model_A1 = [mea.model.F_11,mea.model.F_12,mea.model.F_13;mea.model.F_ mea.engine_model_B1 = [mea.model.B_1;mea.model.B_2;mea.model.B_3]; mea.engine_model_C1 = [mea.model.C_1,mea.model.C_2,mea.model.C_3]; -[sensorTot] = run_MEA(sensorTot,sensorTot_NoPitot,mea); +[sensorTot_OBSW] = run_MEA(sensorTot,sensorTot_OBSW,mea); +[sensorTot_NoPitot] = run_MEA(sensorTot,sensorTot_NoPitot,mea); +[sensorTot_WithPitot] = run_MEA(sensorTot,sensorTot_WithPitot,mea); figure sgtitle('MEA Comparison') @@ -463,15 +467,19 @@ sgtitle('MEA Comparison') subplot(3,1,1) plot(main.MEAControllerStatus(main.MEAControllerStatus(:,1)<t_end,1),main.MEAControllerStatus(main.MEAControllerStatus(:,1)<t_end,2),'DisplayName','OBSW') hold on; -plot(sensorTot.mea.time,sensorTot.mea.estimatedMass,'DisplayName','Simulator') +plot(sensorTot_OBSW.mea.time,sensorTot_OBSW.mea.estimatedMass,'DisplayName','Simulator OBSW') +plot(sensorTot_NoPitot.mea.time,sensorTot_NoPitot.mea.estimatedMass,'DisplayName','Simulator NoPitot' ) +plot(sensorTot_WithPitot.mea.time,sensorTot_WithPitot.mea.estimatedMass,'DisplayName','Simulator WithPitot' ) legend title('Estimated Mass') xlabel('Time [s]') ylabel('Mass [kg]') % pressure subplot(3,1,2) -plot(sensorTot.mea.time,sensorTot.mea.estimatedPressure,'DisplayName','Simulator' ) +plot(sensorTot_OBSW.mea.time,sensorTot_OBSW.mea.estimatedPressure,'DisplayName','Simulator OBSW' ) hold on; +plot(sensorTot_NoPitot.mea.time,sensorTot_NoPitot.mea.estimatedPressure,'DisplayName','Simulator NoPitot' ) +plot(sensorTot_WithPitot.mea.time,sensorTot_WithPitot.mea.estimatedPressure,'DisplayName','Simulator WithPitot' ) plot(motor.CC_PRESSURE(motor.CC_PRESSURE(:,1)<t_end,1) ,motor.CC_PRESSURE(motor.CC_PRESSURE(:,1)<t_end,2),'DisplayName','Pressure sensor') title('Pressure') legend @@ -481,7 +489,9 @@ ylabel('Pressure [Pa]') subplot(3,1,3) plot(main.MEAControllerStatus(main.MEAControllerStatus(:,1)<t_end,1),main.MEAControllerStatus(main.MEAControllerStatus(:,1)<t_end,3),'DisplayName','OBSW') hold on; -plot(sensorTot.mea.time,sensorTot.mea.predictedApogee,'DisplayName','Simulator'); +plot(sensorTot_OBSW.mea.time,sensorTot_OBSW.mea.predictedApogee,'DisplayName','Simulator OBSW'); +plot(sensorTot_NoPitot.mea.time,sensorTot_NoPitot.mea.predictedApogee,'DisplayName','Simulator NoPitot' ) +plot(sensorTot_WithPitot.mea.time,sensorTot_WithPitot.mea.predictedApogee,'DisplayName','Simulator WithPitot' ) plot(main.NASData(main.NASData(:,1)<t_end,1),-main.NASData(main.NASData(:,1)<t_end,4),'DisplayName','NAS Altitude') yline(main.MEATarget,'--','SHUTDOWN TARGET','HandleVisibility','off') legend diff --git a/RoccarasoFlight/postprocessing/commonFunctions/Kalman/run_MEA.m b/RoccarasoFlight/postprocessing/commonFunctions/Kalman/run_MEA.m index 94ed57e84ea1cd5bda1afe7adb774b5b81d135e8..589c120064549934752e20a3a89682f8892425a6 100644 --- a/RoccarasoFlight/postprocessing/commonFunctions/Kalman/run_MEA.m +++ b/RoccarasoFlight/postprocessing/commonFunctions/Kalman/run_MEA.m @@ -1,4 +1,4 @@ -function [sensorTot] = run_MEA(sensorTot,sensorTotNAS,mea) +function [sensorTotNAS] = run_MEA(sensorTot,sensorTotNAS,mea) % mass estimation @@ -71,10 +71,10 @@ predictedApogee = -z_nas + 1./(2.*( 0.5.*rho .* CD .* Surf ./ estimatedMass))... .* log(1 + (vz_nas.^2 .* (0.5 .* rho .* CD .* Surf) ./ estimatedMass) ./ 9.81 ); % update total state -sensorTot.mea.estimatedPressure = estimatedPressure; -sensorTot.mea.estimatedMass = estimatedMass; -sensorTot.mea.predictedApogee = predictedApogee; -sensorTot.mea.time = t_mea; +sensorTotNAS.mea.estimatedPressure = estimatedPressure; +sensorTotNAS.mea.estimatedMass = estimatedMass; +sensorTotNAS.mea.predictedApogee = predictedApogee; +sensorTotNAS.mea.time = t_mea; end \ No newline at end of file