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

ADD: first KF close loop

Browse Source
This commit is contained in:
M.A.Gomez 2022-11-21 19:00:53 +01:00
parent 8bfdb1343e
commit f734f8ca0c
4 changed files with 46 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1151,10 +1151,14 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
new_vtl_data.rx_dts(0) = rx_position_and_time[3]; 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] 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(); //new_vtl_data.debug_print();
vtl_engine.vtl_loop(new_vtl_data); 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(); //new_vtl_data.debug_print();
} }
// compute Ground speed and COG // compute Ground speed and COG

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

@ -45,6 +45,7 @@ void Vtl_Data::init_storage(int n_sats)
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::vec(8);
kf_P = arma::mat(8,8);
epoch_tow_s = 0; epoch_tow_s = 0;
sample_counter = 0; sample_counter = 0;
} }
@ -60,7 +61,7 @@ void Vtl_Data::debug_print()
//sat_LOS.print("VTL SAT LOS"); //sat_LOS.print("VTL SAT LOS");
// kf_state.print("EKF STATE"); // kf_state.print("EKF STATE");
pr_m.print("Satellite Code pseudoranges [m]"); //pr_m.print("Satellite Code pseudoranges [m]");
doppler_hz.print("satellite Carrier Dopplers [Hz]"); //doppler_hz.print("satellite Carrier Dopplers [Hz]");
// carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
} }

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

@ -55,6 +55,7 @@ public:
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::colvec kf_state; // KF STATE
arma::mat kf_P; // KF STATE
// time handling // time handling
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]

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

@ -31,6 +31,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
{ {
//TODO: Implement main VTL loop here //TODO: Implement main VTL loop here
using arma::as_scalar; using arma::as_scalar;
// ################## Kalman filter initialization ###################################### // ################## Kalman filter initialization ######################################
// covariances (static) // covariances (static)
kf_P_x = arma::eye(8, 8); //TODO: use a real value. kf_P_x = arma::eye(8, 8); //TODO: use a real value.
@ -57,6 +58,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix
// ################## Kalman Tracking ###################################### // ################## Kalman Tracking ######################################
static uint32_t counter=0; //counter
counter=counter+1; //uint64_t
cout << "counter" << counter;
if(counter<500){ //
// 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);
@ -66,6 +72,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_x(5)=new_data.rx_v(2); kf_x(5)=new_data.rx_v(2);
kf_x(6)=new_data.rx_dts(0); kf_x(6)=new_data.rx_dts(0);
kf_x(7)=new_data.rx_dts(1); 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) 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) // Kalman state prediction (time update)
//kf_x.print(" KF RTKlib STATE"); // kf_x.print(" KF RTKlib STATE");
new_data.kf_state=kf_x; new_data.kf_state=kf_x;
//kf_x = kf_F * kf_x; // state prediction 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 kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction
//from error state variables to variables //from error state variables to variables
// From state variables definition // 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_xerr = kf_K * (kf_yerr); // Error state estimation
kf_x = kf_x + kf_xerr; // updated state estimation (a priori + error) 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 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) // 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(6) =kf_x(6) /SPEED_OF_LIGHT_M_S;
kf_x(7) =kf_x(7) /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"); kf_x(6)=cdeltat_u/SPEED_OF_LIGHT_M_S;
//cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; kf_x(7)=cdeltatDot_u/SPEED_OF_LIGHT_M_S;
//cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100;
//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 ###################################### // // ################## Geometric Transformation ######################################