diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
index cf5eb2d39..19969fb1e 100644
--- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
+++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
@@ -1999,13 +1999,16 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
             //             |  0  1  0  |
             // P_NED_ENU = |  1  0  0  |
             //             |  0  0 -1  |
-
+            //
+            // TODO:
             // Plus temporary inversion of the North values due to IMU placement
             // Plus temporary inversion of the Up values due to IMU placement
             // I think, the IMU is mounted rotated 180 deg around the East axis
 
+            imu_data.data_available = true;
+
             imu_data.vel(0) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[1 * sizeof(float)]));
-            imu_data.vel(1) =- static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[0 * sizeof(float)]));
+            imu_data.vel(1) = -static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[0 * sizeof(float)]));
             imu_data.vel(2) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[2 * sizeof(float)]));
             imu_data.acc(0) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[4 * sizeof(float)]));
             imu_data.acc(1) = -static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[3 * sizeof(float)]));
diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h
index 7187a523a..3dd74db5a 100644
--- a/src/algorithms/PVT/libs/vtl_data.h
+++ b/src/algorithms/PVT/libs/vtl_data.h
@@ -71,6 +71,8 @@ public:
         arma::colvec acc;      // Receiver ENU acceleration from IMU [m/(s^2)]
         arma::colvec ang_vel;  // Receiver ENU angular velocity from IMU [rad/s]
         arma::colvec ang_acc;  // Receiver ENU angular velocity from IMU [rad/(s^2)]
+
+        bool data_available = false;
     } imu_data;
 
     void debug_print();
diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc
index 562862f86..3f71dde3a 100644
--- a/src/algorithms/PVT/libs/vtl_engine.cc
+++ b/src/algorithms/PVT/libs/vtl_engine.cc
@@ -133,12 +133,18 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
             kf_x(0) = new_data.rx_p(0);
             kf_x(1) = new_data.rx_p(1);
             kf_x(2) = new_data.rx_p(2);
-            kf_x(3) = new_data.rx_v(0);
-            kf_x(4) = new_data.rx_v(1);
-            kf_x(5) = new_data.rx_v(2);
-            kf_x(6) = 0;
-            kf_x(7) = 0;
-            kf_x(8) = 0;
+            kf_x(3) = new_data.imu_data.data_available ? new_data.imu_data.vel(0) : new_data.rx_v(0);
+            kf_x(4) = new_data.imu_data.data_available ? new_data.imu_data.vel(1) : new_data.rx_v(1);
+            kf_x(5) = new_data.imu_data.data_available ? new_data.imu_data.vel(2) : new_data.rx_v(2);
+            kf_x(6) = new_data.imu_data.data_available ? new_data.imu_data.acc(0) : 0.0;
+            kf_x(7) = new_data.imu_data.data_available ? new_data.imu_data.acc(1) : 0.0;
+            kf_x(8) = new_data.imu_data.data_available ? new_data.imu_data.acc(2) : 0.0;
+            // kf_x(3) = new_data.rx_v(0);
+            // kf_x(4) = new_data.rx_v(1);
+            // kf_x(5) = new_data.rx_v(2);
+            // kf_x(6) = 0;
+            // kf_x(7) = 0;
+            // kf_x(8) = 0;
             kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S;
             kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S;
 
@@ -150,9 +156,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
             // receiver solution from previous KF step
             kf_x = kf_x;
             // acceleration model
-            double acc_x = 0;
-            double acc_y = 0;
-            double acc_z = 0;
+            double acc_x = new_data.imu_data.data_available ? new_data.imu_data.acc(0) : 0.0;
+            double acc_y = new_data.imu_data.data_available ? new_data.imu_data.acc(1) : 0.0;
+            double acc_z = new_data.imu_data.data_available ? new_data.imu_data.acc(2) : 0.0;
             kf_x(6) = acc_x;
             kf_x(7) = acc_y;
             kf_x(8) = acc_z;