From b9394bd79d3ab252d5956500ff84c093afc7e0d6 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 9 Oct 2022 22:36:40 +0000 Subject: [PATCH] ADD: Necessary variables for KF --- src/algorithms/PVT/libs/vtl_engine.h | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index daba6d141..64fcdb104 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -50,11 +50,30 @@ public: private: Vtl_Conf config; //TODO: Internal VTL persistent variables here - // Kalman filter variables + + //State variables + double x_u; + double y_u; + double z_u; + double xDot_u; + double yDot_u; + double zDot_u; + double cdeltat_u; + double cdeltatDot_u; + + // Transformation variables + arma::colvec d; + arma::colvec rho_pri; + arma::colvec rhoDot_pri; + arma::colvec a_x; + arma::colvec a_y; + arma::colvec a_z; + + // Kalman filter matrices arma::mat kf_P_x_ini; // initial state error covariance matrix arma::mat kf_P_x; // state error covariance matrix arma::mat kf_P_x_pre; // Predicted state error covariance matrix - arma::mat kf_P_y; // innovation covariance matrix + arma::mat kf_S; // innovation covariance matrix arma::mat kf_F; // state transition matrix arma::mat kf_H; // system matrix