diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc
index 8d0824d31..5ba20aab0 100644
--- a/src/algorithms/PVT/libs/vtl_engine.cc
+++ b/src/algorithms/PVT/libs/vtl_engine.cc
@@ -112,6 +112,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
             kf_R(i, i) = 80.0;
             kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;
             kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0;
+
+            if (i == 6)
+                {
+                    kf_R(i, i) = 10e5;
+                    kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e5;
+                    kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 10e6;
+                }
         }
 
     //**************************************
@@ -146,7 +153,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
             double acc_x = 0;
             double acc_y = 0;
             double acc_z = 0;
-            // model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter);
+            model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter);
             kf_x(6) = acc_x;
             kf_x(7) = acc_y;
             kf_x(8) = acc_z;
diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
index bda054d7a..1c67a3f5f 100644
--- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
@@ -660,19 +660,19 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
                                 {
                                     if (abs(d_x_old_old(2) - tmp_x(2)) > 50)
                                         {
-                                            std::cout << "channel: " << this->d_channel
-                                                      << " tracking_cmd TOO FAR: "
-                                                      << abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
-                                                      << " \n";
+                                            // std::cout << "channel: " << this->d_channel
+                                            //           << " tracking_cmd TOO FAR: "
+                                            //           << abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
+                                            //           << " \n";
                                         }
                                     else
                                         {
-                                            std::cout << "channel: " << this->d_channel
-                                                      << " tracking_cmd NEAR: "
-                                                      << abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
-                                                      << " \n";
+                                            // std::cout << "channel: " << this->d_channel
+                                            //           << " tracking_cmd NEAR: "
+                                            //           << abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
+                                            //           << " \n";
                                         }
-                                    // d_x_old_old(2) = tmp_x(2);  // replace DOPPLER
+                                    d_x_old_old(2) = tmp_x(2);  // replace DOPPLER
                                     // d_x_old_old(3) = tmp_x(3);  //replace DOPPLER RATE
                                 }
                             else