diff --git a/main.m b/main.m
index 3da04f02d60e2c7414e9a67ef20b3d733df42823..3bba06f35de04c3038623d4049f153b7543af483 100644
--- a/main.m
+++ b/main.m
@@ -40,7 +40,7 @@ for t = 0:2000:t_end
 
     imu_idx = sum(IMU(:,1) <= t);
     if imu_idx > lastImuIdx
-        IMU_measurment = IMU(imu_idx, 2);
+        IMU_measurment = IMU(imu_idx, 2:7);
         lastImuIdx = imu_idx;
     else
         IMU_measurment = IMU(lastImuIdx,2:7);