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

fix: vtl_engine bug

Browse Source
This commit is contained in:
M.A. Gomez 2023-03-14 17:29:51 +01:00
parent b96526f1b8
commit 956142cbe2
1 changed files with 53 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -29,7 +29,6 @@ Vtl_Engine::Vtl_Engine()
kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; // TODO: use a real value.; kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; // TODO: use a real value.;
kf_x = arma::zeros(n_of_states, 1); kf_x = arma::zeros(n_of_states, 1);
} }
Vtl_Engine::~Vtl_Engine() Vtl_Engine::~Vtl_Engine()
@ -88,7 +87,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// ################## Kalman Tracking ###################################### // ################## Kalman Tracking ######################################
counter++; // uint64_t counter++; // uint64_t
if (counter>2500){ if (counter > 2500)
{
flag_time_cmd = true; flag_time_cmd = true;
} }
uint32_t closure_point = 3; uint32_t closure_point = 3;
@ -141,6 +141,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
else else
{ {
// receiver solution from previous KF step // receiver solution from previous KF step
kf_x = kf_x;
// acceleration model
double acc_x = 0; double acc_x = 0;
double acc_y = 0; double acc_y = 0;
double acc_z = 0; double acc_z = 0;
@ -163,7 +165,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); kf_H = arma::zeros(3 * new_data.sat_number, n_of_states);
kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt); kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt);
kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri * 0, new_data.pr_m, new_data.doppler_hz, kf_x); kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri * 0, new_data.pr_m, new_data.doppler_hz, kf_x);
//************************************** //**************************************
// Kalman filter update step // Kalman filter update step
//************************************** //**************************************
@ -233,7 +234,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
} }
else else
{ {
dump_vtl_file << "kf_state" dump_vtl_file << "kf_state"
<< "," << kf_x(0) << "," << kf_x(1) << "," << kf_x(2) << "," << kf_x(3) << "," << kf_x(4) << "," << kf_x(5) << "," << kf_x(6) << "," << kf_x(7) << "," << kf_x(8) << "," << kf_x(9) << "," << kf_x(10) << endl; << "," << kf_x(0) << "," << kf_x(1) << "," << kf_x(2) << "," << kf_x(3) << "," << kf_x(4) << "," << kf_x(5) << "," << kf_x(6) << "," << kf_x(7) << "," << kf_x(8) << "," << kf_x(9) << "," << kf_x(10) << endl;
dump_vtl_file << "kf_xerr" dump_vtl_file << "kf_xerr"