From c34782762c25235b181cc9d6036e7ee56248f47d Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 10 Oct 2022 12:00:33 +0200 Subject: [PATCH] ADD: vtl variables and Q estimation. --- src/algorithms/PVT/libs/vtl_data.h | 4 +++- src/algorithms/PVT/libs/vtl_engine.cc | 10 ++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index d63dc865b..e8f6519f3 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -38,6 +38,7 @@ public: arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] arma::colvec sat_var; // sat position and clock error variance [m^2] arma::colvec sat_health_flag; // sat health flag (0 is ok) + arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz int sat_number; // on-view sat number arma::colvec pr_m; // Satellite Code pseudoranges [m] @@ -46,8 +47,9 @@ public: arma::mat rx_p; // Receiver ENU Position [m] arma::mat rx_v; // Receiver Velocity [m/s] + 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 rx_var; // Receiver position and clock error variance [m^2] // 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 ea8abf83a..10fb0f831 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -64,6 +64,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x(6)=new_data.rx_dts(0); kf_x(7)=new_data.rx_dts(1); + for (int32_t i = 0; i < 8; i++) // State error Covariance Matrix Q (PVT) + { + // It is diagonal 8x8 matrix + kf_Q(i, i) = new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate. + } + // // Kalman state prediction (time update) 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 @@ -128,7 +134,7 @@ 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) = 1.0; //TODO: use a real value. + kf_R(i, i) = 1.0; //TODO: use a valid value. kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0; } @@ -143,7 +149,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //} //kf_delta_x = kf_K * kf_delta_y; // updated error state estimation - kf_x = kf_K * (kf_y-dot(kf_H,kf_x)); // updated error state estimation + kf_x = kf_x + kf_K * (kf_y-dot(kf_H,kf_x)); // updated state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?)