diff --git a/src/algorithms/PVT/libs/pvt_kf.cc b/src/algorithms/PVT/libs/pvt_kf.cc
index 9608890c6..187f3998b 100644
--- a/src/algorithms/PVT/libs/pvt_kf.cc
+++ b/src/algorithms/PVT/libs/pvt_kf.cc
@@ -22,14 +22,14 @@
 void Pvt_Kf::init_Kf(const arma::vec& p,
     const arma::vec& v,
     const arma::vec& res_p,
-    double update_interval_s,
+    double solver_interval_s,
     double measures_ecef_pos_sd_m,
     double measures_ecef_vel_sd_ms,
     double system_ecef_pos_sd_m,
     double system_ecef_vel_sd_ms)
 {
     // Kalman Filter class variables
-    const double Ti = update_interval_s;
+    const double Ti = solver_interval_s;
 
     d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0},
         {0.0, 1.0, 0.0, 0.0, Ti, 0.0},
@@ -110,6 +110,11 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
                     d_R(1,0) = res_p[3]; d_R(1,1) = res_p[1]; d_R(1,2) = res_p[4];
                     d_R(2,0) = res_p[5]; d_R(2,1) = res_p[4]; d_R(2,2) = res_p[2];
 
+                    // Measurement residuals
+                    d_R(0,0) = res_p[0]; d_R(0,1) = res_p[3]; d_R(0,2) = res_p[5];
+                    d_R(1,0) = res_p[3]; d_R(1,1) = res_p[1]; d_R(1,2) = res_p[4];
+                    d_R(2,0) = res_p[5]; d_R(2,1) = res_p[4]; d_R(2,2) = res_p[2];
+
                     // Measurement update
                     arma::vec z = arma::join_cols(p, v);
                     arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R);  // Kalman gain
@@ -117,6 +122,7 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
                     d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old);
                     arma::mat A = (arma::eye(6, 6) - K * d_H);
                     d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t();
+
                     // prepare data for next KF epoch
                     d_x_old_old = d_x_new_new;
                     d_P_old_old = d_P_new_new;
diff --git a/src/algorithms/PVT/libs/pvt_kf.h b/src/algorithms/PVT/libs/pvt_kf.h
index 22640f3ff..4be754505 100644
--- a/src/algorithms/PVT/libs/pvt_kf.h
+++ b/src/algorithms/PVT/libs/pvt_kf.h
@@ -38,7 +38,7 @@ public:
     void init_Kf(const arma::vec& p,
         const arma::vec& v,
         const arma::vec& res_p,
-        double update_interval_s,
+        double solver_interval_s,
         double measures_ecef_pos_sd_m,
         double measures_ecef_vel_sd_ms,
         double system_ecef_pos_sd_m,
diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc
index e1a003a46..39242fdda 100644
--- a/src/algorithms/PVT/libs/rtklib_solver.cc
+++ b/src/algorithms/PVT/libs/rtklib_solver.cc
@@ -898,6 +898,7 @@ void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, ui
 }
 
 
+bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s)
 bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s)
 {
     std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
@@ -1554,7 +1555,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                     d_pvt_kf.init_Kf(p,
                                         v,
                                         res_p,
-                                        kf_update_interval_s,
+                                        kf_update_interval_s,                                      
                                         d_conf.measures_ecef_pos_sd_m,
                                         d_conf.measures_ecef_vel_sd_ms,
                                         d_conf.system_ecef_pos_sd_m,