From 92665492bcf42a7e88c6ea8757ea193266ceee2f Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 8 Dec 2022 14:01:27 +0100 Subject: [PATCH] fix: wrong sign in state error correction --- src/algorithms/PVT/libs/vtl_engine.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 94fd7ea06..b24b3c416 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -80,7 +80,7 @@ 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.rx_dts(0); + kf_x(6) = new_data.kf_state[6];//new_data.rx_dts(0); kf_x(7) = new_data.rx_dts(1); kf_P_x=new_data.kf_P; @@ -175,7 +175,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_S = kf_H * kf_P_x* kf_H.t() + kf_R; // innovation covariance matrix (S) kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain kf_xerr = kf_K * (kf_yerr); // Error state estimation - //kf_x = kf_x + kf_xerr; // updated state estimation (a priori + error) + kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix new_data.kf_state=kf_x; //updated state estimation //new_data.kf_state.print("computed kf_state");