mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 22:34:58 +00:00
Code Issues Releases Wiki Activity

MOD: kf filtering

Browse Source
This commit is contained in:
M.A.Gomez 2022-11-21 23:47:50 +00:00
parent f734f8ca0c
commit f5be456f91
5 changed files with 20 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -1152,13 +1152,14 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s]
//new_vtl_data.debug_print(); //new_vtl_data.debug_print();
vtl_engine.vtl_loop(new_vtl_data); pvt_sol.rr[0]=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[0]=new_vtl_data.rx_p(0);
pvt_sol.rr[2]=new_vtl_data.kf_state(2);//rx_p // pvt_sol.rr[1]=new_vtl_data.rx_p(1);
pvt_sol.rr[3]=new_vtl_data.kf_state(3);//rx_v // pvt_sol.rr[2]=new_vtl_data.rx_p(2);
pvt_sol.rr[4]=new_vtl_data.kf_state(4);//rx_v // pvt_sol.rr[3]=new_vtl_data.rx_v(0);
pvt_sol.rr[5]=new_vtl_data.kf_state(5);//rx_v // 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(); //new_vtl_data.debug_print();
} }
// compute Ground speed and COG // compute Ground speed and COG

2
src/algorithms/PVT/libs/vtl_data.cc
View File

@ -44,7 +44,7 @@ void Vtl_Data::init_storage(int n_sats)
rx_dts = arma::mat(1, 2); rx_dts = arma::mat(1, 2);
rx_var = arma::vec(1); rx_var = arma::vec(1);
rx_pvt_var = arma::vec(8); rx_pvt_var = arma::vec(8);
kf_state = arma::vec(8); kf_state = arma::mat(8,1);
kf_P = arma::mat(8,8); kf_P = arma::mat(8,8);
epoch_tow_s = 0; epoch_tow_s = 0;
sample_counter = 0; sample_counter = 0;

5
src/algorithms/PVT/libs/vtl_data.h
View File

@ -54,9 +54,10 @@ public:
arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [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::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]
arma::colvec kf_state; // KF STATE arma::mat kf_state; // KF STATE
arma::mat kf_P; // KF STATE arma::mat kf_P; // KF STATE covariance
// time handling // time handling
double PV[6]; // position and Velocity
double epoch_tow_s; // current observation RX time [s] double epoch_tow_s; // current observation RX time [s]
uint64_t sample_counter; // current sample counter associated with RX time [samples from start] uint64_t sample_counter; // current sample counter associated with RX time [samples from start]
void debug_print(); void debug_print();

6
src/algorithms/PVT/libs/vtl_engine.cc
View File

@ -62,7 +62,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
counter=counter+1; //uint64_t counter=counter+1; //uint64_t
cout << "counter" << counter; cout << "counter" << counter;
if(counter<500){ // if(counter<3000){ //
// receiver solution from rtklib_solver // receiver solution from rtklib_solver
kf_x(0)=new_data.rx_p(0); kf_x(0)=new_data.rx_p(0);
kf_x(1)=new_data.rx_p(1); 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 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) // 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, i) = 20.0; //TODO: fill with real values.
kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0; kf_R(i+new_data.sat_number, i+new_data.sat_number) = 10.0;
} }
// Kalman filter update step // Kalman filter update step

10
src/algorithms/PVT/libs/vtl_engine.h
View File

@ -81,11 +81,11 @@ private:
arma::mat kf_R; // measurement error covariance matrix arma::mat kf_R; // measurement error covariance matrix
arma::mat kf_Q; // system error covariance matrix arma::mat kf_Q; // system error covariance matrix
arma::colvec kf_x; // state vector arma::mat kf_x; // state vector
arma::colvec kf_x_pre; // predicted state vector arma::mat kf_x_pre; // predicted state vector
arma::colvec kf_y; // measurement vector arma::mat kf_y; // measurement vector
arma::colvec kf_yerr; // ERROR measurement vector arma::mat kf_yerr; // ERROR measurement vector
arma::colvec kf_xerr; // ERROR state vector arma::mat kf_xerr; // ERROR state vector
arma::mat kf_K; // Kalman gain matrix arma::mat kf_K; // Kalman gain matrix
// Gaussian estimator // Gaussian estimator