MEA working on different NAS output now

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

RoccarasoFlight/postprocessing/commonFunctions/Kalman/run_MEA.m

-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