diff --git a/commonFunctions/kalman/run_MEA.m b/commonFunctions/kalman/run_MEA.m
index 609a23d68d41f1f4a41e44e188d97ebf2f12ff89..cbf8e45d2b2acc7d9d9659031983ab04b3a434a3 100644
--- a/commonFunctions/kalman/run_MEA.m
+++ b/commonFunctions/kalman/run_MEA.m
@@ -14,23 +14,26 @@ t_chambPress = sensorTot.comb_chamber.time;%(sensorTot.comb_chamber.time >= T1);
 t_nas = sensorTot.nas.time;%(sensorTot.nas.time>= T1); % we need also nas to estimate cd etc
 t_imu = sensorTot.imu.time;
 
-% initialise state update
+% initialize state update
 x(1,:) = sensorData.mea.x(end,:);
 P(:,:,1) = sensorData.mea.P(:,:,end);
 P_acc(:,:,1) = sensorData.mea.P_acc(:,:,end);
-predicted_apogee = 0;
+
 z_nas = zeros(length(t_mea), 1);
 vnorm_nas = zeros(length(t_mea), 1);
 vz_nas = zeros(length(t_mea), 1);
+
 z_nas(1) = sensorData.nas.states(end,3);
 vnorm_nas(1) = norm(sensorData.nas.states(end,4:6));
 vz_nas(1) = -sensorData.nas.states(end,6);
 
- if settings.flagFlightRef
-     coeffs = contSettings.coeff_Cd;
- else
-     coeffs = contSettings.coeffs;
- end
+predicted_apogee = 0;
+
+if settings.flagFlightRef
+    coeffs = contSettings.coeff_Cd;
+else
+    coeffs = contSettings.coeffs;
+end
 
 
 for ii = 2:length(t_mea)
@@ -55,7 +58,8 @@ for ii = 2:length(t_mea)
             x(ii,:) = x(ii,:)' + K* (sensorTot.comb_chamber.measures(index_chambPress) -  C * x(ii,:)'); % /1000 to have the measure in bar
         end
     end
-    %accelerometer correction (not for 2023)
+
+    % accelerometer correction (not for 2023)
     if contains(mission.name, '2024')
         K_t = settings.mea.K_t;
         alpha = settings.mea.alpha;
@@ -66,13 +70,11 @@ for ii = 2:length(t_mea)
         mass_max = settings.mea.mass_interval(2);
         mass_min = settings.mea.mass_interval(1);
 
-        if norm(sensorTot.imu.accelerometer_measures(index_imu, :)) > acc_threshold...
-                && vz_nas(ii) > vel_threshold
-
-            cd = 1*getDrag(vz_nas(ii), -z_nas(ii), 0, coeffs); %add correction shut_down??
+        if norm(sensorTot.imu.accelerometer_measures(index_imu, :)) > acc_threshold && vz_nas(ii) > vel_threshold
+            cd = 1*getDrag(vz_nas(ii), -z_nas(ii), 0, coeffs);      % add correction shut_down??
             [~,~,P_e, rho] = computeAtmosphericData(-z_nas(ii));
-            q = 0.5*rho*vz_nas(ii)^2; %dynamic pressure
-            F_a = q*rocket.crossSection*cd;       %aerodynamic force
+            q = 0.5*rho*vz_nas(ii)^2;                               % dynamic pressure
+            F_a = q*rocket.crossSection*cd;                         % aerodynamic force
 
             if  -z_nas(ii,1)> 800
                 F_s = (P_0-P_e)*rocket.motor.ae;
@@ -94,7 +96,6 @@ for ii = 2:length(t_mea)
                 P(:,:,ii) = (eye(3)-K*H)*P(:,:,ii);
                 x(ii,:) = x(ii,:)' + K.*(sensorTot.imu.accelerometer_measures(index_imu, 1) - y_est);
             end
-
         end
         
         % use only reasonable masses to predict the apogee
@@ -105,16 +106,15 @@ for ii = 2:length(t_mea)
         else
             mass = x(ii,3);
         end
-
     else
         mass = x(ii,3);
     end
 
-    %propagate apogee
+    % propagate apogee
     CD = settings.CD_correction_shutDown*getDrag(vz_nas(ii), -z_nas(ii), 0, coeffs); % coeffs potrebbe essere settings.coeffs
     [~,~,~,rho] = computeAtmosphericData(-z_nas(ii));
 
-    propagation_steps = 0;%contSettings.N_prediction_threshold - settings.mea.counter_shutdown;
+    propagation_steps = 0; % contSettings.N_prediction_threshold - settings.mea.counter_shutdown;
     if propagation_steps >=1
         [z_pred, vz_pred] = PropagateState(-z_nas(ii),-vz_nas(ii), ...
             K_t .* sensorTot.comb_chamber.measures(index_chambPress), ...
@@ -133,13 +133,13 @@ for ii = 2:length(t_mea)
     vnorm_nas(ii,1) = norm(sensorTot.nas.states(index_NAS,4:6));
     vz_nas(ii,1) = sensorTot.nas.states(index_NAS,6);
 end
+
 % pressure estimation
 estimated_pressure = C*x';
 
 % mass estimation
 estimated_mass = x(:,3);
 
-
 % update local state
 sensorData.mea.time = t_mea;
 sensorData.mea.x = x;
@@ -156,5 +156,5 @@ sensorTot.mea.prediction(sensorTot.mea.n_old:sensorTot.mea.n_old + size(sensorDa
 sensorTot.mea.time(sensorTot.mea.n_old:sensorTot.mea.n_old + size(sensorData.mea.x(:,1),1)-2) = sensorData.mea.time(2:end);
 sensorTot.mea.n_old = sensorTot.mea.n_old + size(sensorData.mea.x,1) -1;
 
+end
 
-end
\ No newline at end of file
diff --git a/simulator/src/sensor_plots_MEA_test.m b/simulator/src/sensor_plots_MEA_test.m
index c2dea3d0a9661239ca9774f7293e7a88ad84c168..81c8e125a6748ac634ffc5f7e7aa7b1c092a9864 100644
--- a/simulator/src/sensor_plots_MEA_test.m
+++ b/simulator/src/sensor_plots_MEA_test.m
@@ -23,83 +23,83 @@ legend("Accelerometer", "Real");
 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
-plot(simOutput.t, simOutput.Y(:,7));
-legend("Gyroscope", "Real");
-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
-plot(simOutput.t, simOutput.Y(:,8));
-legend("Gyroscope", "Real");
-ylabel("$\omega_Y$ $[rad/s]$")
-subplot(3,1,3)
-plot(simOutput.sensors.imu.time, simOutput.sensors.imu.gyro_measures(:,3));
-hold on, grid on
-plot(simOutput.t, simOutput.Y(:,9));
-legend("Gyroscope", "Real");
-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);
-magnFieldBody = quatrotate(Q, magnFieldInertial)/1e3;
-
-figure('Name', "Magnetometer measurements")
-subplot(3,1,1)
-plot(simOutput.sensors.imu.time, simOutput.sensors.imu.magnetometer_measures(:,1)/1e1);
-hold on; grid on;
-plot(simOutput.t, magnFieldBody(:,1));
-legend("Magnetometer", "Real");
-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");
-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");
-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);
-
-figure("Name", "GPS Position measurements")
-subplot(3,1,1)
-plot(simOutput.sensors.gps.time, GPS_NED(:,1));
-hold on; grid on;
-plot(simOutput.t, simOutput.Y(:,1));
-legend("GPS", "Real");
-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");
-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");
-xlabel("Time [s]"), ylabel("Up agl [m]");
-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
+% plot(simOutput.t, simOutput.Y(:,7));
+% legend("Gyroscope", "Real");
+% 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
+% plot(simOutput.t, simOutput.Y(:,8));
+% legend("Gyroscope", "Real");
+% ylabel("$\omega_Y$ $[rad/s]$")
+% subplot(3,1,3)
+% plot(simOutput.sensors.imu.time, simOutput.sensors.imu.gyro_measures(:,3));
+% hold on, grid on
+% plot(simOutput.t, simOutput.Y(:,9));
+% legend("Gyroscope", "Real");
+% 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);
+% magnFieldBody = quatrotate(Q, magnFieldInertial)/1e3;
+% 
+% figure('Name', "Magnetometer measurements")
+% subplot(3,1,1)
+% plot(simOutput.sensors.imu.time, simOutput.sensors.imu.magnetometer_measures(:,1)/1e1);
+% hold on; grid on;
+% plot(simOutput.t, magnFieldBody(:,1));
+% legend("Magnetometer", "Real");
+% 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");
+% 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");
+% 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);
+% 
+% figure("Name", "GPS Position measurements")
+% subplot(3,1,1)
+% plot(simOutput.sensors.gps.time, GPS_NED(:,1));
+% hold on; grid on;
+% plot(simOutput.t, simOutput.Y(:,1));
+% legend("GPS", "Real");
+% 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");
+% 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");
+% xlabel("Time [s]"), ylabel("Up agl [m]");
+% drawnow
 
 %% Velocities
 % ODE velocity rotated in ned frame
@@ -136,29 +136,29 @@ legend("GPS", "Real");
 xlabel("Time [s]"), ylabel("$V_D\ [m/s]$");
 drawnow
 
-%% Barometer measurements
-[~, ~, P_real] = computeAtmosphericData(-simOutput.Y(:,3)+environment.z0);
-
-figure("Name", "Barometer measurements")
-hold on;
-plot(simOutput.sensors.barometer_sens{1, 1}.time,simOutput.sensors.barometer_sens{1, 1}.pressure_measures,'b','DisplayName','Baro 1')
-plot(simOutput.sensors.barometer_sens{1, 2}.time,simOutput.sensors.barometer_sens{1, 2}.pressure_measures,'k','DisplayName','Baro 2')
-plot(simOutput.sensors.barometer_sens{1, 3}.time,simOutput.sensors.barometer_sens{1, 3}.pressure_measures,'r','DisplayName','Baro 3')
-plot(simOutput.t, P_real, 'g', 'DisplayName', 'Real pressure');
-legend
-title('Barometer measurements')
-xlabel("Time [s]"), ylabel("Pressure [Pa]")
-drawnow
+% %% Barometer measurements
+% [~, ~, P_real] = computeAtmosphericData(-simOutput.Y(:,3)+environment.z0);
+% 
+% figure("Name", "Barometer measurements")
+% hold on;
+% plot(simOutput.sensors.barometer_sens{1, 1}.time,simOutput.sensors.barometer_sens{1, 1}.pressure_measures,'b','DisplayName','Baro 1')
+% plot(simOutput.sensors.barometer_sens{1, 2}.time,simOutput.sensors.barometer_sens{1, 2}.pressure_measures,'k','DisplayName','Baro 2')
+% plot(simOutput.sensors.barometer_sens{1, 3}.time,simOutput.sensors.barometer_sens{1, 3}.pressure_measures,'r','DisplayName','Baro 3')
+% plot(simOutput.t, P_real, 'g', 'DisplayName', 'Real pressure');
+% legend
+% title('Barometer measurements')
+% xlabel("Time [s]"), ylabel("Pressure [Pa]")
+% drawnow
 
-%% Pitot measurements
-figure("Name", "Pitot measurements")
-plot(simOutput.sensors.pitot.time,simOutput.sensors.pitot.static_pressure, 'DisplayName', "Pitot static pressure")
-hold on;
-plot(simOutput.t, P_real, 'DisplayName', "Real pressure");
-legend()
-title('Pitot static pressure')
-xlabel("Time [s]"), ylabel("Pressure [Pa]")
-drawnow
+% %% Pitot measurements
+% figure("Name", "Pitot measurements")
+% plot(simOutput.sensors.pitot.time,simOutput.sensors.pitot.static_pressure, 'DisplayName', "Pitot static pressure")
+% hold on;
+% plot(simOutput.t, P_real, 'DisplayName', "Real pressure");
+% legend()
+% title('Pitot static pressure')
+% xlabel("Time [s]"), ylabel("Pressure [Pa]")
+% drawnow
 
 %% MEA pressure vs true pressure
 if length(simOutput.sensors.mea.time) > 1
diff --git a/simulator/src/std_plots_MEA_test.m b/simulator/src/std_plots_MEA_test.m
index a04dfbafb7e55d3b81bd5e4e4f65d37d28521f20..097dc9e2cc17432413296faa06c9f9288111a415 100644
--- a/simulator/src/std_plots_MEA_test.m
+++ b/simulator/src/std_plots_MEA_test.m
@@ -159,23 +159,23 @@ drawnow
 
 %% Control variable: servo angle + reference values
 % air brakes
-if not(settings.scenario == "descent")
-    figures.servo_angle = figure('Name', 'Servo angle after burning phase','ToolBar','auto');
-    plot(simOutput.t, simOutput.Y(:,14));
-    hold on; grid on;
-    stairs(simOutput.ARB.cmdTime,simOutput.ARB.cmdPosition,'r');
-    xline(simOutput.ARB.allowanceTime,'k--')
-    xline(simOutput.apogee.time,'r--')
-    xlabel('Time [s]');
-    ylabel('$\alpha$ [rad]');
-    title('Servo angle');
-    legend('simulated','reference values','Airbrakes deployment','Apogee')
-
-    if settings.flagExportPLOTS == true
-        exportStandardizedFigure(figures.servo_angle,"report_images\"+mission.name+"\src_servo_angle.pdf",0.9)
-    end
-    drawnow
-end
+% if not(settings.scenario == "descent")
+%     figures.servo_angle = figure('Name', 'Servo angle after burning phase','ToolBar','auto');
+%     plot(simOutput.t, simOutput.Y(:,14));
+%     hold on; grid on;
+%     stairs(simOutput.ARB.cmdTime,simOutput.ARB.cmdPosition,'r');
+%     xline(simOutput.ARB.allowanceTime,'k--')
+%     xline(simOutput.apogee.time,'r--')
+%     xlabel('Time [s]');
+%     ylabel('$\alpha$ [rad]');
+%     title('Servo angle');
+%     legend('simulated','reference values','Airbrakes deployment','Apogee')
+% 
+%     if settings.flagExportPLOTS == true
+%         exportStandardizedFigure(figures.servo_angle,"report_images\"+mission.name+"\src_servo_angle.pdf",0.9)
+%     end
+%     drawnow
+% end
 % % parafoil
 % if settings.parafoil && (settings.scenario == "descent" || settings.scenario == "full flight")
 %     figures.parafoil_servo_action = figure('Name', 'Parafoil deltaA','ToolBar','auto');
@@ -190,36 +190,36 @@ end
 % end
 
 %% Trajectory
-figures.trajectory = figure('Name', 'Trajectory','ToolBar','auto');
-plot3(simOutput.Y(1:end-10, 2), simOutput.Y(1:end-10, 1), -simOutput.Y(1:end-10, 3),'DisplayName','True trajectory');
-hold on; grid on;
-plot3(simOutput.sensors.nas.states(1:end-10, 2), simOutput.sensors.nas.states(1:end-10, 1), -simOutput.sensors.nas.states(1:end-10, 3)-environment.z0,'DisplayName','NAS trajectory');
-
-if not(settings.scenario == "descent")
-    plot3(simOutput.ARB.openingPosition(2),simOutput.ARB.openingPosition(1),simOutput.ARB.openingPosition(3),'ko','DisplayName','Airbrake deployment')
-end
-plot3(simOutput.apogee.position(2),simOutput.apogee.position(1),-simOutput.apogee.position(3),'ro','DisplayName','Apogee')
-
-if settings.parafoil  && (settings.scenario == "descent" || settings.scenario == "full flight")
-    plot3(simOutput.Y(simOutput.events.mainChuteIndex, 2), simOutput.Y(simOutput.events.mainChuteIndex, 1), -simOutput.Y(simOutput.events.mainChuteIndex, 3),'d','DisplayName','Main chute opening');
-    plot3(settings.payload.target(2),settings.payload.target(1),settings.payload.target(3),'go','DisplayName','Payload Target')
-    if contSettings.payload.guidance_alg == "t-approach"
-        makeCone(simOutput.payload.EMC([2,1]),15,-simOutput.Y(simOutput.events.mainChuteIndex,3),'EMC')
-        makeCone(simOutput.payload.M1([2,1]),15,-simOutput.Y(simOutput.events.mainChuteIndex,3),'M1')
-        makeCone(simOutput.payload.M2([2,1]),15,-simOutput.Y(simOutput.events.mainChuteIndex,3),'M2')
-    end
-end
-xlabel('E [m]');
-ylabel('N [m]');
-zlabel('U [m]');
-title('Trajectory (ENU)');
-axis equal
-view([157,55])
-legend
-if settings.flagExportPLOTS == true
-    exportStandardizedFigure(figures.trajectory,"report_images\"+mission.name+"\src_trajectory.pdf",0.49)
-end
-drawnow
+% figures.trajectory = figure('Name', 'Trajectory','ToolBar','auto');
+% plot3(simOutput.Y(1:end-10, 2), simOutput.Y(1:end-10, 1), -simOutput.Y(1:end-10, 3),'DisplayName','True trajectory');
+% hold on; grid on;
+% plot3(simOutput.sensors.nas.states(1:end-10, 2), simOutput.sensors.nas.states(1:end-10, 1), -simOutput.sensors.nas.states(1:end-10, 3)-environment.z0,'DisplayName','NAS trajectory');
+% 
+% if not(settings.scenario == "descent")
+%     plot3(simOutput.ARB.openingPosition(2),simOutput.ARB.openingPosition(1),simOutput.ARB.openingPosition(3),'ko','DisplayName','Airbrake deployment')
+% end
+% plot3(simOutput.apogee.position(2),simOutput.apogee.position(1),-simOutput.apogee.position(3),'ro','DisplayName','Apogee')
+% 
+% if settings.parafoil  && (settings.scenario == "descent" || settings.scenario == "full flight")
+%     plot3(simOutput.Y(simOutput.events.mainChuteIndex, 2), simOutput.Y(simOutput.events.mainChuteIndex, 1), -simOutput.Y(simOutput.events.mainChuteIndex, 3),'d','DisplayName','Main chute opening');
+%     plot3(settings.payload.target(2),settings.payload.target(1),settings.payload.target(3),'go','DisplayName','Payload Target')
+%     if contSettings.payload.guidance_alg == "t-approach"
+%         makeCone(simOutput.payload.EMC([2,1]),15,-simOutput.Y(simOutput.events.mainChuteIndex,3),'EMC')
+%         makeCone(simOutput.payload.M1([2,1]),15,-simOutput.Y(simOutput.events.mainChuteIndex,3),'M1')
+%         makeCone(simOutput.payload.M2([2,1]),15,-simOutput.Y(simOutput.events.mainChuteIndex,3),'M2')
+%     end
+% end
+% xlabel('E [m]');
+% ylabel('N [m]');
+% zlabel('U [m]');
+% title('Trajectory (ENU)');
+% axis equal
+% view([157,55])
+% legend
+% if settings.flagExportPLOTS == true
+%     exportStandardizedFigure(figures.trajectory,"report_images\"+mission.name+"\src_trajectory.pdf",0.49)
+% end
+% drawnow
 
 %% Velocities BODY w.r.t. time against NAS
 figures.velocities_BODY = figure('Name', 'Velocities BODY','ToolBar','auto');
diff --git a/simulator/src/std_run_parts/std_setInitialParams.m b/simulator/src/std_run_parts/std_setInitialParams.m
index cc50102af4f164b47aa51fd8392aa1c4dd696c7a..d927cf26bc0ada6c05d3fecb7d36827e232d95ad 100644
--- a/simulator/src/std_run_parts/std_setInitialParams.m
+++ b/simulator/src/std_run_parts/std_setInitialParams.m
@@ -7,18 +7,17 @@ integration initialization script- setting initial condition before control phas
 
 %% kalman initialization
 if not(settings.scenario == "descent")
-    sensorData.kalman.vz = 0;                                                   % Vertical velocity
+    sensorData.kalman.vz = 0;                                               % Vertical velocity
     sensorData.kalman.z  = -environment.z0;
 else 
-    sensorData.kalman.vz = -settings.Vz_final;                                                   % Vertical velocity
+    sensorData.kalman.vz = -settings.Vz_final;                              % Vertical velocity
     sensorData.kalman.z  = -settings.z_final;
-end    
-    % Altitude
+end
+% Altitude
 
 %% ADA Initialization
-
-settings.ada.counter     =   0;
-settings.ada.paraCounter =   0;
+settings.ada.counter     =   0;                     % Counter for ADA
+settings.ada.paraCounter =   0;                     % Counter for ADA
 
 settings.ada.t_ada       =   -1;                    % Apogee detection timestamp
 settings.ada.flag_apo    =   false;                 % True when the apogee is detected
@@ -27,33 +26,20 @@ settings.ada.flagOpenPara =  false;                 % True when the main parachu
 %% Initialization of sensor measurement time
 control_freq = settings.frequencies.controlFrequency;
 
-sensorData.accelerometer.t0 = initSensorT0...
-    (control_freq ,settings.frequencies.accelerometerFrequency);
-
-sensorData.gyro.t0 = initSensorT0...
-    (control_freq,settings.frequencies.gyroFrequency);
-
-sensorData.magnetometer.t0 = initSensorT0...
-    (control_freq,settings.frequencies.magnetometerFrequency);
-
-sensorData.gps.t0 = initSensorT0...
-    (control_freq,settings.frequencies.gpsFrequency);
-
+sensorData.accelerometer.t0 = initSensorT0(control_freq ,settings.frequencies.accelerometerFrequency);
+sensorData.gyro.t0 = initSensorT0(control_freq,settings.frequencies.gyroFrequency);
+sensorData.magnetometer.t0 = initSensorT0(control_freq,settings.frequencies.magnetometerFrequency);
+sensorData.gps.t0 = initSensorT0(control_freq,settings.frequencies.gpsFrequency);
 
 % triplicate sensors for sensor fault detection testing
-sensorData.barometer_sens{1}.t0 = initSensorT0...
-    (control_freq,settings.frequencies.barometerFrequency);
-sensorData.barometer_sens{2}.t0 = initSensorT0...
-    (control_freq,settings.frequencies.barometerFrequency);
-sensorData.barometer_sens{3}.t0 = initSensorT0...
-    (control_freq,settings.frequencies.barometerFrequency);
+sensorData.barometer_sens{1}.t0 = initSensorT0(control_freq,settings.frequencies.barometerFrequency);
+sensorData.barometer_sens{2}.t0 = initSensorT0(control_freq,settings.frequencies.barometerFrequency);
+sensorData.barometer_sens{3}.t0 = initSensorT0(control_freq,settings.frequencies.barometerFrequency);
 
-sensorData.pitot.t0 = initSensorT0...
-    (control_freq,settings.frequencies.pitotFrequency);
+sensorData.pitot.t0 = initSensorT0(control_freq,settings.frequencies.pitotFrequency);
 
 if contains(mission.name,'2023')  || contains(mission.name,'2024') || contains(mission.name,'2025')
-sensorData.chamberPressure.t0 = initSensorT0...
-    (control_freq,settings.frequencies.chamberPressureFrequency);
+    sensorData.chamberPressure.t0 = initSensorT0(control_freq,settings.frequencies.chamberPressureFrequency);
 end
 
 sensorData.barometer_sens{1}.time = [];
@@ -82,8 +68,9 @@ ap_ref_old = 0;
 ap_ref = [ ap_ref_old ap_ref_new ];
 
 % servo motor time delay - in ode it needs to be set to change reference value
-t_change_ref_ABK =      t0 + settings.servo.delay;
+t_change_ref_ABK = t0 + settings.servo.delay;
 t_last_arb_control = 0;
+
 %% parafoil control action initialization
 deltaA_ref_new = 0;
 deltaA_ref_old = 0;
@@ -92,7 +79,6 @@ deltaA_ref = [deltaA_ref_old,deltaA_ref_new];
 t_change_ref_PRF =      t0 + rocket.parachutes(2,2).controlParams.deltaA_delay;
 t_last_prf_control = 0;
 
-
 %% initialization of other variables - for speed purposes
 mach        =       0;                                                      % Mach number
 ext         =       0;                                                      % air brake extension
@@ -121,6 +107,7 @@ idx_apogee = NaN;
 idx_landing = NaN;
 
 engineT0 = 0;       % Initial time for engine time computations
+
 %% sensor fault initial conditions
 sensorData.chunk{1} = zeros(1,50);
 sensorData.chunk{2} = zeros(1,50);
@@ -131,7 +118,6 @@ barometer_time = [];
 sfd_mean_p = [];
 
 %% ADA initial conditions (Apogee Detection Algorithm)
-
 if strcmp(settings.scenario, 'descent')
     [~, ~, p_fin, ~]  =   computeAtmosphericData(settings.z_final+environment.z0);               % Reference temperature in kelvin and pressure in Pa
 
@@ -152,7 +138,6 @@ end
 % Pada_prev =   settings.ada.P0;
 
 %% NAS initial conditions (Navigation and Attitude System)
-
 if settings.flagNAS || settings.electronics
     % initialize states of the NAS 
     sensorData.nas.states = [X0; V0; Q0(2:4); Q0(1);0;0;0]';
@@ -180,6 +165,7 @@ sensorData.mea.predicted_apogee = 0;
 settings.t_shutdown = Inf;
 settings.mea.counter_shutdown = 0;
 settings.flagMEAInit = false;
+
 %% parafoil
 deltaA = contSettings.payload.deltaA_0;
 para = 1;
@@ -209,7 +195,8 @@ sensorTot.nas.states                            =   sensorData.nas.states;
 sensorTot.nas.timestampPitotCorrection          =   nan;
 sensorTot.nas.timestampMagnetometerCorrection   =   0;
 sensorTot.mea.pressure                          =   0; %it's a differential pressure sensor
-sensorTot.mea.mass                              =   rocket.motor.mass(1);
+% sensorTot.mea.mass                              =   rocket.motor.mass(1);
+sensorTot.mea.mass                              =   rocket.mass(1);
 sensorTot.mea.prediction                        =   0;
 sensorTot.mea.time                              =   0;
 
@@ -251,4 +238,5 @@ flagBurning = false;
 flagAeroBrakes = false;
 flagApogee = false;
 flagPara1 = false;
-flagPara2 = false;
\ No newline at end of file
+flagPara2 = false;
+