From f5be456f91e69bd25495e576d6016696df59f01a Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 21 Nov 2022 23:47:50 +0000 Subject: [PATCH] MOD: kf filtering --- src/algorithms/PVT/libs/rtklib_solver.cc | 15 ++++++++------- src/algorithms/PVT/libs/vtl_data.cc | 2 +- src/algorithms/PVT/libs/vtl_data.h | 5 +++-- src/algorithms/PVT/libs/vtl_engine.cc | 6 +++--- src/algorithms/PVT/libs/vtl_engine.h | 10 +++++----- 5 files changed, 20 insertions(+), 18 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 707367254..5c2427761 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1152,13 +1152,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] //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 + pvt_sol.rr[0]=vtl_engine.vtl_loop(new_vtl_data); + + // pvt_sol.rr[0]=new_vtl_data.rx_p(0); + // pvt_sol.rr[1]=new_vtl_data.rx_p(1); + // pvt_sol.rr[2]=new_vtl_data.rx_p(2); + // pvt_sol.rr[3]=new_vtl_data.rx_v(0); + // pvt_sol.rr[4]=new_vtl_data.rx_v(1); + // pvt_sol.rr[5]=new_vtl_data.rx_v(2); //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 4926aa2ee..f03f26df8 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -44,7 +44,7 @@ void Vtl_Data::init_storage(int n_sats) rx_dts = arma::mat(1, 2); rx_var = arma::vec(1); rx_pvt_var = arma::vec(8); - kf_state = arma::vec(8); + kf_state = arma::mat(8,1); kf_P = arma::mat(8,8); epoch_tow_s = 0; sample_counter = 0; diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 68d9dbcc1..787c69f13 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -54,9 +54,10 @@ public: arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] 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 + arma::mat kf_state; // KF STATE + arma::mat kf_P; // KF STATE covariance // time handling + double PV[6]; // position and Velocity double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] void debug_print(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index cec5a725d..d50c0a105 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -62,7 +62,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) counter=counter+1; //uint64_t cout << "counter" << counter; - if(counter<500){ // + if(counter<3000){ // // receiver solution from rtklib_solver kf_x(0)=new_data.rx_p(0); kf_x(1)=new_data.rx_p(1); @@ -161,8 +161,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 0.1; //TODO: fill with real values. - kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0; + kf_R(i, i) = 20.0; //TODO: fill with real values. + kf_R(i+new_data.sat_number, i+new_data.sat_number) = 10.0; } // Kalman filter update step diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index eb5ddc45d..88e564508 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -81,11 +81,11 @@ private: arma::mat kf_R; // measurement error covariance matrix arma::mat kf_Q; // system error covariance matrix - arma::colvec kf_x; // state vector - arma::colvec kf_x_pre; // predicted state vector - arma::colvec kf_y; // measurement vector - arma::colvec kf_yerr; // ERROR measurement vector - arma::colvec kf_xerr; // ERROR state vector + arma::mat kf_x; // state vector + arma::mat kf_x_pre; // predicted state vector + arma::mat kf_y; // measurement vector + arma::mat kf_yerr; // ERROR measurement vector + arma::mat kf_xerr; // ERROR state vector arma::mat kf_K; // Kalman gain matrix // Gaussian estimator