Update main to ensure timestamps are read correctly

main.m

@@ -6,12 +6,15 @@ clc;
 
 IMU = table2array(readtable('IMU.csv'));
 dt_IMU = 1000;
+lastImuIdx = 0;
 
 GPS = table2array(readtable('GPS.csv'));
 dt_GPS = 200000;
+lastGPSIdx = 0;
 
 baro = table2array(readtable('barometer.csv'));
 dt_baro = mean(diff(baro(:, 1)));
+lastBaroIdx = 0;
 
 t_end = min([IMU(end,1), baro(end,1), GPS(end,1)]);
 
@@ -26,10 +29,30 @@ apogee_time = -1;
 % Simulation
 for t = 0:2000:t_end
 
-    % Getting measurment form sensors
-    pressure_measurment = baro(floor(t/dt_baro)+1,2);
-    IMU_measurment = IMU(floor(t/dt_IMU)+1, 2:7);
-    GPS_measurment = GPS(floor(t/dt_GPS)+1, 2:4);
+    % Getting measurment from sensors
+    baro_idx = sum(baro(:,1) <= t);
+    if baro_idx > lastBaroIdx
+        pressure_measurment = baro(baro_idx, 2);
+        lastBaroIdx = baro_idx;
+    else
+        pressure_measurment = baro(lastBaroIdx,2);
+    end
+
+    imu_idx = sum(IMU(:,1) <= t);
+    if imu_idx > lastImuIdx
+        IMU_measurment = IMU(imu_idx, 2);
+        lastImuIdx = imu_idx;
+    else
+        IMU_measurment = IMU(lastImuIdx,2:7);
+    end
+
+    gps_idx = sum(GPS(:,1) <= t);
+    if gps_idx > lastGPSIdx
+        GPS_measurment = GPS(gps_idx, 2:4);
+        lastGPSIdx = gps_idx;
+    else
+        GPS_measurment = GPS(lastGPSIdx,2:4);
+    end
     
     % Apogee detection
     [flag_apogee, data] = apogee_detector(pressure_measurment, IMU_measurment, GPS_measurment, data);