From f734f8ca0ca9b9f5c916e346bae59e081f4b8ad3 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 21 Nov 2022 19:00:53 +0100 Subject: [PATCH] ADD: first KF close loop --- src/algorithms/PVT/libs/rtklib_solver.cc | 8 +++- src/algorithms/PVT/libs/vtl_data.cc | 5 ++- src/algorithms/PVT/libs/vtl_data.h | 1 + src/algorithms/PVT/libs/vtl_engine.cc | 57 +++++++++++++++--------- 4 files changed, 46 insertions(+), 25 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 94494f5ac..707367254 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1151,10 +1151,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_dts(0) = rx_position_and_time[3]; new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] - //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? //new_vtl_data.debug_print(); vtl_engine.vtl_loop(new_vtl_data); - + pvt_sol.rr[0]=new_vtl_data.kf_state(0);//rx_p + pvt_sol.rr[1]=new_vtl_data.kf_state(1);//rx_p + pvt_sol.rr[2]=new_vtl_data.kf_state(2);//rx_p + pvt_sol.rr[3]=new_vtl_data.kf_state(3);//rx_v + pvt_sol.rr[4]=new_vtl_data.kf_state(4);//rx_v + pvt_sol.rr[5]=new_vtl_data.kf_state(5);//rx_v //new_vtl_data.debug_print(); } // compute Ground speed and COG diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 2f4fccd83..4926aa2ee 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -45,6 +45,7 @@ void Vtl_Data::init_storage(int n_sats) rx_var = arma::vec(1); rx_pvt_var = arma::vec(8); kf_state = arma::vec(8); + kf_P = arma::mat(8,8); epoch_tow_s = 0; sample_counter = 0; } @@ -60,7 +61,7 @@ void Vtl_Data::debug_print() //sat_LOS.print("VTL SAT LOS"); // kf_state.print("EKF STATE"); - pr_m.print("Satellite Code pseudoranges [m]"); - doppler_hz.print("satellite Carrier Dopplers [Hz]"); + //pr_m.print("Satellite Code pseudoranges [m]"); + //doppler_hz.print("satellite Carrier Dopplers [Hz]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 15e403476..68d9dbcc1 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -55,6 +55,7 @@ public: arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] arma::colvec rx_var; // Receiver position and clock error variance [m^2] arma::colvec kf_state; // KF STATE + arma::mat kf_P; // KF STATE // time handling double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 5ecb947c1..cec5a725d 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -31,6 +31,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; + // ################## Kalman filter initialization ###################################### // covariances (static) kf_P_x = arma::eye(8, 8); //TODO: use a real value. @@ -55,17 +56,27 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_yerr = arma::zeros(2*new_data.sat_number, 1); kf_xerr = arma::zeros(8, 1); kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix - + // ################## Kalman Tracking ###################################### - // receiver solution from rtklib_solver - kf_x(0)=new_data.rx_p(0); - kf_x(1)=new_data.rx_p(1); - kf_x(2)=new_data.rx_p(2); - 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)=new_data.rx_dts(1); + static uint32_t counter=0; //counter + counter=counter+1; //uint64_t + cout << "counter" << counter; + + if(counter<500){ // + // receiver solution from rtklib_solver + kf_x(0)=new_data.rx_p(0); + kf_x(1)=new_data.rx_p(1); + kf_x(2)=new_data.rx_p(2); + 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)=new_data.rx_dts(1); + } + else{ + kf_x=new_data.kf_state; + kf_P_x=new_data.kf_P; + } for (int32_t i = 0; i < 8; i++) // State error Covariance Matrix Q (PVT) { @@ -74,9 +85,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } // Kalman state prediction (time update) - //kf_x.print(" KF RTKlib STATE"); - new_data.kf_state=kf_x; - //kf_x = kf_F * kf_x; // state prediction + // kf_x.print(" KF RTKlib STATE"); + new_data.kf_state=kf_x; + kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction //from error state variables to variables // From state variables definition @@ -160,16 +171,20 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_xerr = kf_K * (kf_yerr); // Error state estimation 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_P=kf_P_x; //update state estimation error covariance // States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) - kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; - kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; - new_data.pr_res.print(" pr RESIDUALS"); - //new_data.kf_state.print(" KF RTKlib STATE"); - //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; - //cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100; + // kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; + // kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; + + kf_x(6)=cdeltat_u/SPEED_OF_LIGHT_M_S; + kf_x(7)=cdeltatDot_u/SPEED_OF_LIGHT_M_S; + + //new_data.pr_res.print(" pr RESIDUALS"); + //!new_data.kf_state.print(" KF RTKlib STATE"); + //!cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; + //!cout << " KF posteriori STATE diference %1" << (kf_x-new_data.kf_state)/new_data.kf_state; // // ################## Geometric Transformation ######################################