From 6b74976a4a6871ba083d51e7cf512a1046bc8ef9 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 8 Dec 2022 19:36:32 +0000 Subject: [PATCH] fix: state is not clk, is clk times C --- src/algorithms/PVT/libs/vtl_engine.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 6ebc63fc9..1c8fd7244 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -66,8 +66,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) 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) = new_data.rx_dts(0); - kf_x(7) = 1e-8; + kf_x(6) = new_data.rx_dts(0)*SPEED_OF_LIGHT_M_S; + kf_x(7) = new_data.rx_dts(1)*SPEED_OF_LIGHT_M_S; } else { // receiver solution from previous KF step @@ -77,8 +77,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(3) = new_data.kf_state[3]; kf_x(4) = new_data.kf_state[4]; kf_x(5) = new_data.kf_state[5]; - kf_x(6) = new_data.kf_state[6];//new_data.rx_dts(0); - kf_x(7) = 1e-8; + kf_x(6) = new_data.kf_state[6]; + kf_x(7) = new_data.kf_state[7]; kf_P_x=new_data.kf_P; } // State error Covariance Matrix Q (PVT) @@ -108,8 +108,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) xDot_u=kf_x(3); yDot_u=kf_x(4); zDot_u=kf_x(5); - cdeltat_u=new_data.rx_dts(0)*SPEED_OF_LIGHT_M_S; - cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S; + cdeltat_u=kf_x(6); + cdeltatDot_u=kf_x(7); d = arma::zeros(new_data.sat_number, 1); rho_pri = arma::zeros(new_data.sat_number, 1);