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

FORMAT: clang-format applied

Browse Source
This commit is contained in:
M.A.Gomez 2023-03-11 19:32:16 +00:00
parent fd69416f4e
commit b815ee4d9d
6 changed files with 489 additions and 471 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/algorithms/PVT/libs/rtklib_solver.cc Executable file → Normal file
View File

@ -168,7 +168,7 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
try try
{ {
d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit);
uint end_filename = d_dump_filename.length()-4; uint end_filename = d_dump_filename.length() - 4;
d_vtl_dump_filename = d_dump_filename; d_vtl_dump_filename = d_dump_filename;
d_vtl_dump_filename = d_vtl_dump_filename.insert(end_filename, "_vtl"); d_vtl_dump_filename = d_vtl_dump_filename.insert(end_filename, "_vtl");
d_vtl_dump_file.open(d_vtl_dump_filename, std::ios::out | std::ios::binary); d_vtl_dump_file.open(d_vtl_dump_filename, std::ios::out | std::ios::binary);
@ -218,7 +218,7 @@ Rtklib_Solver::~Rtklib_Solver()
LOG(WARNING) << "Exception in destructor saving the PVT .mat dump file " << ex.what(); LOG(WARNING) << "Exception in destructor saving the PVT .mat dump file " << ex.what();
} }
} }
if (d_flag_dump_mat_enabled) if (d_flag_dump_mat_enabled)
{ {
try try
{ {
@ -353,7 +353,6 @@ bool Rtklib_Solver::save_vtl_matfile() const
matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73);
if (reinterpret_cast<int64_t *>(matfp) != nullptr) if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{ {
std::array<size_t, 2> dims{1, static_cast<size_t>(num_epoch)}; std::array<size_t, 2> dims{1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
@ -474,7 +473,6 @@ bool Rtklib_Solver::save_vtl_matfile() const
matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0); matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
} }
Mat_Close(matfp); Mat_Close(matfp);
@ -1893,7 +1891,6 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s]
vtl_engine.vtl_loop(vtl_data); vtl_engine.vtl_loop(vtl_data);
} }
else else
{ {
@ -2088,7 +2085,6 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
try try
{ {
double tmp_double; double tmp_double;
uint32_t tmp_uint32; uint32_t tmp_uint32;
// TOW // TOW
@ -2114,7 +2110,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
tmp_double = p_vec_m[2]; tmp_double = p_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_vec_m = vtl_engine.get_velocity_ecef_m_s(); std::vector<double> v_vec_m = vtl_engine.get_velocity_ecef_m_s();
tmp_double = v_vec_m[0]; tmp_double = v_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_vec_m[1]; tmp_double = v_vec_m[1];
@ -2122,7 +2118,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
tmp_double = v_vec_m[2]; tmp_double = v_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> a_vec_m = vtl_engine.get_accel_ecef_m_s2(); std::vector<double> a_vec_m = vtl_engine.get_accel_ecef_m_s2();
tmp_double = a_vec_m[0]; tmp_double = a_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_vec_m[1]; tmp_double = a_vec_m[1];
@ -2132,7 +2128,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// position/velocity/acceleration variance/ (units^2) (9 x double) // position/velocity/acceleration variance/ (units^2) (9 x double)
std::vector<double> p_var_vec_m = vtl_engine.get_position_var_ecef_m(); std::vector<double> p_var_vec_m = vtl_engine.get_position_var_ecef_m();
tmp_double = p_var_vec_m[0]; tmp_double = p_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_var_vec_m[1]; tmp_double = p_var_vec_m[1];
@ -2141,7 +2137,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); std::vector<double> v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s();
tmp_double = v_var_vec_m[0]; tmp_double = v_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_var_vec_m[1]; tmp_double = v_var_vec_m[1];
@ -2149,7 +2145,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
tmp_double = v_var_vec_m[2]; tmp_double = v_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
vector<double> a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); vector<double> a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2();
tmp_double = a_var_vec_m[0]; tmp_double = a_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_var_vec_m[1]; tmp_double = a_var_vec_m[1];
@ -2158,13 +2154,13 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Latitude [deg] // GEO user position Latitude [deg]
tmp_double = this->get_latitude(); tmp_double = this->get_latitude();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Longitude [deg] // GEO user position Longitude [deg]
tmp_double = this->get_longitude(); tmp_double = this->get_longitude();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Height [m] // GEO user position Height [m]
tmp_double = this->get_height(); tmp_double = this->get_height();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// NUMBER OF VALID SATS // NUMBER OF VALID SATS

4
src/algorithms/PVT/libs/vtl_data.cc Executable file → Normal file
View File

@ -16,8 +16,8 @@
#include "vtl_data.h" #include "vtl_data.h"
#include "vector"
#include "armadillo" #include "armadillo"
#include "vector"
Vtl_Data::Vtl_Data() Vtl_Data::Vtl_Data()
{ {
@ -66,5 +66,3 @@ void Vtl_Data::debug_print()
//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]");
} }

28
src/algorithms/PVT/libs/vtl_data.h Executable file → Normal file
View File

@ -36,29 +36,29 @@ public:
Vtl_Data(); Vtl_Data();
void init_storage(int n_sats); void init_storage(int n_sats);
arma::mat sat_p; // Satellite ECEF Position [m] arma::mat sat_p; // Satellite ECEF Position [m]
arma::mat sat_v; // Satellite Velocity [m/s] arma::mat sat_v; // Satellite Velocity [m/s]
arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] 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_var; // sat position and clock error variance [m^2]
arma::colvec sat_health_flag; // sat health flag (0 is ok) arma::colvec sat_health_flag; // sat health flag (0 is ok)
arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz
arma::mat sat_LOS; // sat LOS arma::mat sat_LOS; // sat LOS
int sat_number; // on-view sat number int sat_number; // on-view sat number
arma::colvec pr_m; // Satellite Code pseudoranges [m] arma::colvec pr_m; // Satellite Code pseudoranges [m]
arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz] arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz]
arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads] arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads]
arma::colvec pr_res; // pseudorange residual arma::colvec pr_res; // pseudorange residual
arma::mat rx_p; // Receiver ENU Position [m] arma::mat rx_p; // Receiver ENU Position [m]
arma::mat rx_v; // Receiver Velocity [m/s] arma::mat rx_v; // Receiver Velocity [m/s]
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::mat kf_state; // KF STATE arma::mat kf_state; // KF STATE
arma::mat kf_P; // KF STATE covariance arma::mat kf_P; // KF STATE covariance
// time handling // time handling
double PV[6]; // position and Velocity 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();

713
src/algorithms/PVT/libs/vtl_engine.cc Executable file → Normal file
View File

@ -22,9 +22,9 @@ using namespace std;
Vtl_Engine::Vtl_Engine() Vtl_Engine::Vtl_Engine()
{ {
counter=0; counter = 0;
refSampleCounter=0; refSampleCounter = 0;
n_of_states=11; n_of_states = 11;
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);
} }
@ -38,19 +38,20 @@ 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;
if (refSampleCounter=0) if (refSampleCounter = 0)
{ {
refSampleCounter=new_data.sample_counter; refSampleCounter = new_data.sample_counter;
} }
double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0;
refSampleCounter = new_data.sample_counter; refSampleCounter = new_data.sample_counter;
static double delta_t_cmd = 0; static double delta_t_cmd = 0;
bool flag_cmd = false; bool flag_cmd = false;
delta_t_cmd = delta_t_cmd+delta_t_vtl; // update timer for vtl trk command delta_t_cmd = delta_t_cmd + delta_t_vtl; // update timer for vtl trk command
if(delta_t_cmd>=0.3){ if (delta_t_cmd >= 0.3)
flag_cmd = true; {
delta_t_cmd = 0; // reset timer for vtl trk command flag_cmd = true;
} delta_t_cmd = 0; // reset timer for vtl trk command
}
// ################## Kalman filter initialization ###################################### // ################## Kalman filter initialization ######################################
//State variables //State variables
@ -58,27 +59,28 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// covariances (static) // covariances (static)
kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number);
double kf_dt = delta_t_vtl; //0.05; double kf_dt = delta_t_vtl; //0.05;
kf_Q = arma::eye(n_of_states, n_of_states); kf_Q = arma::eye(n_of_states, n_of_states);
kf_F = arma::eye(n_of_states, n_of_states); kf_F = arma::eye(n_of_states, n_of_states);
bool test = kf_F_fill(kf_F,kf_dt); bool test = kf_F_fill(kf_F, kf_dt);
//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_y = arma::zeros(3 * new_data.sat_number, 1); kf_y = arma::zeros(3 * new_data.sat_number, 1);
kf_yerr = arma::zeros(3 * new_data.sat_number, 1); kf_yerr = arma::zeros(3 * new_data.sat_number, 1);
kf_xerr = arma::zeros(n_of_states, 1); kf_xerr = arma::zeros(n_of_states, 1);
kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix
kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); ; kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number);
;
// ################## Kalman Tracking ###################################### // ################## Kalman Tracking ######################################
counter++; //uint64_t counter++; //uint64_t
//new_data.kf_state.print("new_data kf initial"); //new_data.kf_state.print("new_data kf initial");
uint32_t closure_point=3; uint32_t closure_point = 3;
if (counter < closure_point) if (counter < closure_point)
{ {
// // receiver solution from rtklib_solver // // receiver solution from rtklib_solver
kf_dx=kf_x; kf_dx = kf_x;
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);
kf_x(2) = new_data.rx_p(2); kf_x(2) = new_data.rx_p(2);
@ -91,7 +93,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S;
kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S;
kf_dx = kf_x-kf_dx; kf_dx = kf_x - kf_dx;
kf_dx = kf_F * kf_dx; // state prediction kf_dx = kf_F * kf_dx; // state prediction
} }
else else
@ -100,7 +102,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
double acc_x = 0; double acc_x = 0;
double acc_y = 0; double acc_y = 0;
double acc_z = 0; double acc_z = 0;
test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt,counter); test = model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter);
kf_x(6) = acc_x; kf_x(6) = acc_x;
kf_x(7) = acc_y; kf_x(7) = acc_y;
kf_x(8) = acc_z; kf_x(8) = acc_z;
@ -127,9 +129,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
for (int32_t i = 0; i < new_data.sat_number; i++) for (int32_t i = 0; i < new_data.sat_number; i++)
{ {
// 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) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. kf_R(i, i) = 80.0; //*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values.
kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; //*50.0/new_data.sat_CN0_dB_hz(i);
kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 400.0;//*50.0/new_data.sat_CN0_dB_hz(i); kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; //*50.0/new_data.sat_CN0_dB_hz(i);
// if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ // if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){
// kf_R(i, i) = 10e4; // kf_R(i, i) = 10e4;
@ -159,7 +161,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
a_y = arma::zeros(new_data.sat_number, 1); a_y = arma::zeros(new_data.sat_number, 1);
a_z = arma::zeros(new_data.sat_number, 1); a_z = arma::zeros(new_data.sat_number, 1);
// cout<<"llegado aqui tambien"<<endl; // cout<<"llegado aqui tambien"<<endl;
test = obsv_calc(rho_pri,rhoDot_pri,a_x, a_y, a_z,new_data.sat_number,new_data.sat_p,new_data.sat_v,kf_x); test = obsv_calc(rho_pri, rhoDot_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x);
for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities
{ {
new_data.sat_LOS(i, 0) = a_x(i); new_data.sat_LOS(i, 0) = a_x(i);
@ -168,39 +170,39 @@ 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);
test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt); test = kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt);
for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities
{ {
// rhoDot2_pri(i)=(rhoDot_pri(i)-rhoDot_pri_old(i))/kf_dt; // rhoDot2_pri(i)=(rhoDot_pri(i)-rhoDot_pri_old(i))/kf_dt;
} }
// Kalman estimation (measurement update) // Kalman estimation (measurement update)
test = 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); test = 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_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
// Kalman filter update step // Kalman filter update step
kf_S = kf_H * kf_P_x * kf_H.t() + kf_R; // innovation covariance matrix (S) kf_S = kf_H * kf_P_x * kf_H.t() + kf_R; // innovation covariance matrix (S)
arma::mat B= (kf_P_x * kf_H.t()) ; arma::mat B = (kf_P_x * kf_H.t());
kf_K = B * arma::inv(kf_S); // Kalman gain kf_K = B * arma::inv(kf_S); // Kalman gain
kf_xerr = kf_K * (kf_yerr); // Error state estimation kf_xerr = kf_K * (kf_yerr); // Error state estimation
//kf_xerr.row(5)=kf_K.row(5)*kf_yerr; //kf_xerr.row(5)=kf_K.row(5)*kf_yerr;
arma::mat A = (arma::eye(size(kf_P_x)) - kf_K * kf_H); arma::mat A = (arma::eye(size(kf_P_x)) - kf_K * kf_H);
kf_P_x = A * kf_P_x * A.t() + kf_K * kf_R * kf_K.t() ; // update state estimation error covariance matrix kf_P_x = A * kf_P_x * A.t() + kf_K * kf_R * kf_K.t(); // update state estimation error covariance matrix
kf_dx=kf_x; kf_dx = kf_x;
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_x.print("state of kalman: "); // kf_x.print("state of kalman: ");
// // ################## Geometric Transformation ###################################### // // ################## Geometric Transformation ######################################
test = obsv_calc(rho_pri,rhoDot_pri,a_x, a_y, a_z,new_data.sat_number,new_data.sat_p,new_data.sat_v,kf_x); test = obsv_calc(rho_pri, rhoDot_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x);
for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities
{ {
//acc_effect(i)=(a_x(i)*kf_state(7,t)+a_y(chan,t)*kf_state(8,t)+a_z(chan,t)*kf_state(9,t)); //acc_effect(i)=(a_x(i)*kf_state(7,t)+a_y(chan,t)*kf_state(8,t)+a_z(chan,t)*kf_state(9,t));
//rhoDot2_pri(chan,t)=(rhoDot_pri(chan,t)-rhoDot_pri(chan,t-1))/kf_dt; //rhoDot2_pri(chan,t)=(rhoDot_pri(chan,t)-rhoDot_pri(chan,t-1))/kf_dt;
//rhoDot2_pri(chan,t)=-acc_effect(chan,t); //rhoDot2_pri(chan,t)=-acc_effect(chan,t);
} }
test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt); test = kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt);
// Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x // Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x
kf_yerr = kf_H * kf_xerr; kf_yerr = kf_H * kf_xerr;
// Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): // Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
@ -209,7 +211,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector
{ {
rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered
rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1) + kf_yerr(channel + new_data.sat_number); // now filtered rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1) + kf_yerr(channel + new_data.sat_number); // now filtered
doppler_hz_filt(channel) = (rhoDot_pri_filt(channel)) / Lambda_GPS_L1; doppler_hz_filt(channel) = (rhoDot_pri_filt(channel)) / Lambda_GPS_L1;
//TODO: Fill the tracking commands outputs //TODO: Fill the tracking commands outputs
@ -217,23 +219,29 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// sample code // sample code
trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_phase_rads = 0; // difficult of calculation
trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction
trk_cmd.carrier_freq_rate_hz_s =-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8)) / Lambda_GPS_L1; trk_cmd.carrier_freq_rate_hz_s = -(a_x(channel) * kf_x(6) + a_y(channel) * kf_x(7) + a_z(channel) * kf_x(8)) / Lambda_GPS_L1;
trk_cmd.code_phase_chips = 0;//kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; trk_cmd.code_phase_chips = 0; //kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3;
if (flag_cmd){ if (flag_cmd)
trk_cmd.enable_carrier_nco_cmd = true; {
}else{ trk_cmd.enable_carrier_nco_cmd = true;
trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue }
} else
{
trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue
}
if(counter<1500){ if (counter < 1500)
// std::cout<<"yet to soon"<<std::endl; {
trk_cmd.enable_code_nco_cmd = false; // do NOT apply corrections! initial convergence issue // std::cout<<"yet to soon"<<std::endl;
}else{ trk_cmd.enable_code_nco_cmd = false; // do NOT apply corrections! initial convergence issue
trk_cmd.enable_code_nco_cmd = true; }
} else
{
trk_cmd.enable_code_nco_cmd = true;
}
trk_cmd.sample_counter = new_data.sample_counter; trk_cmd.sample_counter = new_data.sample_counter;
trk_cmd.channel_id = channel; trk_cmd.channel_id = channel;
trk_cmd_outs.push_back(trk_cmd); trk_cmd_outs.push_back(trk_cmd);
@ -298,42 +306,45 @@ void Vtl_Engine::configure(Vtl_Conf config_)
//TODO: initialize internal variables //TODO: initialize internal variables
} }
bool Vtl_Engine::kf_H_fill(arma::mat &kf_H,int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt) bool Vtl_Engine::kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt)
{ {
for (int32_t i = 0; i < sat_number; i++) // Measurement matrix H assembling for (int32_t i = 0; i < sat_number; i++) // Measurement matrix H assembling
{ {
// It has n_of_states columns (n_of_states states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) // It has n_of_states columns (n_of_states states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(i, 0) = ax(i); kf_H(i, 0) = ax(i);
kf_H(i, 1) = ay(i); kf_H(i, 1) = ay(i);
kf_H(i, 2) = az(i); kf_H(i, 2) = az(i);
kf_H(i, 9) = 1.0; kf_H(i, 9) = 1.0;
kf_H(i,10) = kf_dt; kf_H(i, 10) = kf_dt;
kf_H(i + sat_number, 3) = ax(i); kf_H(i + sat_number, 3) = ax(i);
kf_H(i + sat_number, 4) = ay(i); kf_H(i + sat_number, 4) = ay(i);
kf_H(i + sat_number, 5) = az(i); kf_H(i + sat_number, 5) = az(i);
kf_H(i + sat_number, 6) = ax(i)*kf_dt; kf_H(i + sat_number, 6) = ax(i) * kf_dt;
kf_H(i + sat_number, 7) = ay(i)*kf_dt; kf_H(i + sat_number, 7) = ay(i) * kf_dt;
kf_H(i + sat_number, 8) = az(i)*kf_dt; kf_H(i + sat_number, 8) = az(i) * kf_dt;
kf_H(i + sat_number, 10) = 1.0; kf_H(i + sat_number, 10) = 1.0;
kf_H(i + 2*sat_number, 3) = 0;//ax(i); kf_H(i + 2 * sat_number, 3) = 0; //ax(i);
kf_H(i + 2*sat_number, 4) = 0;//ay(i); kf_H(i + 2 * sat_number, 4) = 0; //ay(i);
kf_H(i + 2*sat_number, 5) = 0;//az(i); kf_H(i + 2 * sat_number, 5) = 0; //az(i);
kf_H(i + 2*sat_number, 6) = ax(i); kf_H(i + 2 * sat_number, 6) = ax(i);
kf_H(i + 2*sat_number, 7) = ay(i); kf_H(i + 2 * sat_number, 7) = ay(i);
kf_H(i + 2*sat_number, 8) = az(i); kf_H(i + 2 * sat_number, 8) = az(i);
kf_H(i + 2*sat_number, 10) = kf_dt; kf_H(i + 2 * sat_number, 10) = kf_dt;
} }
return -1; return -1;
} }
bool Vtl_Engine::kf_F_fill(arma::mat &kf_F,double kf_dt) bool Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt)
{ {
kf_F(0, 3) = kf_dt; kf_F(0, 6) = kf_dt*kf_dt/2; kf_F(0, 3) = kf_dt;
kf_F(1, 4) = kf_dt; kf_F(1, 7) = kf_dt*kf_dt/2; kf_F(0, 6) = kf_dt * kf_dt / 2;
kf_F(2, 5) = kf_dt; kf_F(2, 8) = kf_dt*kf_dt/2; kf_F(1, 4) = kf_dt;
kf_F(1, 7) = kf_dt * kf_dt / 2;
kf_F(2, 5) = kf_dt;
kf_F(2, 8) = kf_dt * kf_dt / 2;
kf_F(3, 6) = kf_dt; kf_F(3, 6) = kf_dt;
kf_F(4, 7) = kf_dt; kf_F(4, 7) = kf_dt;
@ -344,49 +355,49 @@ bool Vtl_Engine::kf_F_fill(arma::mat &kf_F,double kf_dt)
return -1; return -1;
} }
bool Vtl_Engine::obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x) bool Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x)
{ {
for (int32_t i = 0; i < sat_number; i++) //neccesary quantities for (int32_t i = 0; i < sat_number; i++) //neccesary quantities
{ {
//d(i) is the distance sat(i) to receiver //d(i) is the distance sat(i) to receiver
d(i) = (sat_p(i, 0) - kf_x(0)) * (sat_p(i, 0) - kf_x(0)); d(i) = (sat_p(i, 0) - kf_x(0)) * (sat_p(i, 0) - kf_x(0));
d(i) = d(i) + (sat_p(i, 1) - kf_x(1)) * (sat_p(i, 1) - kf_x(1)); d(i) = d(i) + (sat_p(i, 1) - kf_x(1)) * (sat_p(i, 1) - kf_x(1));
d(i) = d(i) + (sat_p(i, 2) - kf_x(2)) * (sat_p(i, 2) - kf_x(2)); d(i) = d(i) + (sat_p(i, 2) - kf_x(2)) * (sat_p(i, 2) - kf_x(2));
d(i) = sqrt(d(i)); d(i) = sqrt(d(i));
//compute pseudorange estimation //compute pseudorange estimation
rho_pri(i) = d(i) + kf_x(9); rho_pri(i) = d(i) + kf_x(9);
//compute LOS sat-receiver vector componentsx //compute LOS sat-receiver vector componentsx
ax(i) = -(sat_p(i, 0) - kf_x(0)) / d(i); ax(i) = -(sat_p(i, 0) - kf_x(0)) / d(i);
ay(i) = -(sat_p(i, 1) - kf_x(1)) / d(i); ay(i) = -(sat_p(i, 1) - kf_x(1)) / d(i);
az(i) = -(sat_p(i, 2) - kf_x(2)) / d(i); az(i) = -(sat_p(i, 2) - kf_x(2)) / d(i);
//compute pseudorange rate estimation //compute pseudorange rate estimation
rhoDot_pri(i) = (sat_v(i, 0) - kf_x(3)) * a_x(i) + (sat_v(i, 1) - kf_x(4)) * a_y(i) + (sat_v(i, 2) - kf_x(5)) * a_z(i); rhoDot_pri(i) = (sat_v(i, 0) - kf_x(3)) * a_x(i) + (sat_v(i, 1) - kf_x(4)) * a_y(i) + (sat_v(i, 2) - kf_x(5)) * a_z(i);
//rhoDot_pri(i) = rhoDot_pri(i) + a_x(i)*xDot2_u*kf_dt+a_y(i)*yDot2_u*kf_dt+a_z(i)*zDot2_u*kf_dt; //rhoDot_pri(i) = rhoDot_pri(i) + a_x(i)*xDot2_u*kf_dt+a_y(i)*yDot2_u*kf_dt+a_z(i)*zDot2_u*kf_dt;
} }
return -1; return -1;
} }
bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x) bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x)
{ {
for (int32_t i = 0; i < sat_number; i++) // Measurement vector for (int32_t i = 0; i < sat_number; i++) // Measurement vector
{ {
kf_yerr(i) = rho_pri(i) - pr_m(i); kf_yerr(i) = rho_pri(i) - pr_m(i);
kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1+kf_x(10)) - rhoDot_pri(i); kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1 + kf_x(10)) - rhoDot_pri(i);
kf_yerr(i + 2*sat_number) = -rhoDot2_pri(i); kf_yerr(i + 2 * sat_number) = -rhoDot2_pri(i);
} }
return -1; return -1;
} }
bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt, int counter) bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter)
{ {
arma::colvec u_vec; arma::colvec u_vec;
arma::colvec acc_vec; arma::colvec acc_vec;
arma::colvec u_dir; arma::colvec u_dir;
arma::colvec gravity_ECEF = {-7.826024, 0.8616969, -5.833042}; //lat=36.533333 lon=-6.283333 arma::colvec gravity_ECEF = {-7.826024, 0.8616969, -5.833042}; //lat=36.533333 lon=-6.283333
static double t_disparo=0; static double t_disparo = 0;
double Empuje; double Empuje;
double densidad=1.0; double densidad = 1.0;
double ballistic_coef = 0.007; double ballistic_coef = 0.007;
//vector velocidad //vector velocidad
@ -395,257 +406,268 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
//modulo de la velocidad //modulo de la velocidad
double u = norm(u_vec, 2); double u = norm(u_vec, 2);
if(counter>1500){ if (counter > 1500)
if(u>6){ {
t_disparo=t_disparo+dt; if (u > 6)
std::cout<<"u : "<<u<<endl; {
double diam_cohete=120.0e-3;// 120 mm t_disparo = t_disparo + dt;
double mass_rocket=50.0; //50Kg std::cout << "u : " << u << endl;
double diam_cohete = 120.0e-3; // 120 mm
double mass_rocket = 50.0; //50Kg
if(t_disparo<.2){ if (t_disparo < .2)
u_dir={.90828, -.13984, -.388756}; {
}else{ u_dir = {.90828, -.13984, -.388756};
u_dir = u_vec / u; }
} else
// u_dir.print("u_dir"); {
// lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]); u_dir = u_vec / u;
// [T, sound_v, P, densidad] = atmosisa(lla(3)); }
// sound_v=320;% @ 5km and -17.5C // u_dir.print("u_dir");
// Mach=u/sound_v; // lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]);
// CD0 = Cd0_M_LookTable(Mach); // [T, sound_v, P, densidad] = atmosisa(lla(3));
// % ballistic_coef is Cd0/mass_rocket; // sound_v=320;% @ 5km and -17.5C
// ballistic_coef=CD0/mass_rocket; // Mach=u/sound_v;
Empuje = EmpujeLkTable(t_disparo); // CD0 = Cd0_M_LookTable(Mach);
// cout<<"Empuje: "<<Empuje<<endl; // % ballistic_coef is Cd0/mass_rocket;
// ballistic_coef=CD0/mass_rocket;
Empuje = EmpujeLkTable(t_disparo);
// cout<<"Empuje: "<<Empuje<<endl;
acc_vec = -(GNSS_PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir acc_vec = -(GNSS_PI * densidad * diam_cohete * diam_cohete / 8) * ballistic_coef * u * u_dir + gravity_ECEF + Empuje * u_dir;
+gravity_ECEF+Empuje*u_dir;
// acc_vec.print("acc_vec"); // acc_vec.print("acc_vec");
// % return // % return
cout<<"modelo 3Dof actuando,t:"<<t_disparo<<endl; cout << "modelo 3Dof actuando,t:" << t_disparo << endl;
acc_x = acc_vec(0); acc_x = acc_vec(0);
acc_y = acc_vec(1); acc_y = acc_vec(1);
acc_z = acc_vec(2); acc_z = acc_vec(2);
}else{ }
t_disparo=0; else
{
t_disparo = 0;
// % return
acc_x = 0;
acc_y = 0;
acc_z = 0;
}
}
else
{
// % return // % return
acc_x = 0; acc_x = 0;
acc_y = 0; acc_y = 0;
acc_z = 0; acc_z = 0;
} }
}else{
// % return return -1;
acc_x = 0;
acc_y = 0;
acc_z = 0;
}
return -1;
} }
double Vtl_Engine::EmpujeLkTable(double t_disparo) double Vtl_Engine::EmpujeLkTable(double t_disparo)
{ {
double E; double E;
arma::mat LkTable={ arma::mat LkTable = {
{0.0, 391.083112445424}, {0.0, 391.083112445424},
{0.0100578034682081, 385.626317230813}, {0.0100578034682081, 385.626317230813},
{0.0201156069364162, 379.253652903964}, {0.0201156069364162, 379.253652903964},
{0.0301734104046243, 372.850418310078}, {0.0301734104046243, 372.850418310078},
{0.0402312138728324, 366.435105395212}, {0.0402312138728324, 366.435105395212},
{0.0502890173410405, 359.948724887310}, {0.0502890173410405, 359.948724887310},
{0.0603468208092486, 353.452370826679}, {0.0603468208092486, 353.452370826679},
{0.0704046242774566, 346.915160536406}, {0.0704046242774566, 346.915160536406},
{0.0804624277456647, 340.353374212744}, {0.0804624277456647, 340.353374212744},
{0.0905202312138728, 339.982366920698}, {0.0905202312138728, 339.982366920698},
{0.100578034682081, 339.649644036322}, {0.100578034682081, 339.649644036322},
{0.110635838150289, 339.313119301332}, {0.110635838150289, 339.313119301332},
{0.120693641618497, 338.971249841340}, {0.120693641618497, 338.971249841340},
{0.130751445086705, 338.626092336370}, {0.130751445086705, 338.626092336370},
{0.140809248554913, 338.277926096280}, {0.140809248554913, 338.277926096280},
{0.150867052023121, 337.923860794114}, {0.150867052023121, 337.923860794114},
{0.160924855491329, 337.564686821652}, {0.160924855491329, 337.564686821652},
{0.170982658959538, 337.204565166310}, {0.170982658959538, 337.204565166310},
{0.181040462427746, 336.836327593982}, {0.181040462427746, 336.836327593982},
{0.191098265895954, 337.612574596978}, {0.191098265895954, 337.612574596978},
{0.201156069364162, 338.389812277202}, {0.201156069364162, 338.389812277202},
{0.211213872832370, 339.172197383571}, {0.211213872832370, 339.172197383571},
{0.221271676300578, 339.950864133333}, {0.221271676300578, 339.950864133333},
{0.231329479768786, 340.734286946588}, {0.231329479768786, 340.734286946588},
{0.241387283236994, 341.513599696996}, {0.241387283236994, 341.513599696996},
{0.251445086705202, 342.296487929689}, {0.251445086705202, 342.296487929689},
{0.261502890173410, 343.077730186615}, {0.261502890173410, 343.077730186615},
{0.271560693641619, 343.862401553296}, {0.271560693641619, 343.862401553296},
{0.281618497109827, 344.645045182721}, {0.281618497109827, 344.645045182721},
{0.291676300578035, 345.430222609429}, {0.291676300578035, 345.430222609429},
{0.301734104046243, 346.216363751051}, {0.301734104046243, 346.216363751051},
{0.311791907514451, 347.003086022300}, {0.311791907514451, 347.003086022300},
{0.321849710982659, 347.790403464500}, {0.321849710982659, 347.790403464500},
{0.331907514450867, 348.576632890101}, {0.331907514450867, 348.576632890101},
{0.341965317919075, 349.365041767204}, {0.341965317919075, 349.365041767204},
{0.352023121387283, 350.154345504950}, {0.352023121387283, 350.154345504950},
{0.362080924855491, 350.943780688588}, {0.362080924855491, 350.943780688588},
{0.372138728323699, 351.735714016931}, {0.372138728323699, 351.735714016931},
{0.382196531791908, 352.523676077225}, {0.382196531791908, 352.523676077225},
{0.392254335260116, 353.317052045327}, {0.392254335260116, 353.317052045327},
{0.402312138728324, 354.110411623268}, {0.402312138728324, 354.110411623268},
{0.412369942196532, 354.903279067692}, {0.412369942196532, 354.903279067692},
{0.422427745664740, 355.692559158889}, {0.422427745664740, 355.692559158889},
{0.432485549132948, 356.490274978154}, {0.432485549132948, 356.490274978154},
{0.442543352601156, 357.283451900588}, {0.442543352601156, 357.283451900588},
{0.452601156069364, 358.078507291348}, {0.452601156069364, 358.078507291348},
{0.462658959537572, 358.871323078342}, {0.462658959537572, 358.871323078342},
{0.472716763005780, 359.668187019647}, {0.472716763005780, 359.668187019647},
{0.482774566473988, 360.465893772213}, {0.482774566473988, 360.465893772213},
{0.492832369942197, 361.264486481857}, {0.492832369942197, 361.264486481857},
{0.502890173410405, 362.057713788838}, {0.502890173410405, 362.057713788838},
{0.512947976878613, 362.855398652135}, {0.512947976878613, 362.855398652135},
{0.523005780346821, 363.651958513907}, {0.523005780346821, 363.651958513907},
{0.533063583815029, 364.453139119421}, {0.533063583815029, 364.453139119421},
{0.543121387283237, 365.248494477390}, {0.543121387283237, 365.248494477390},
{0.553179190751445, 366.047440571574}, {0.553179190751445, 366.047440571574},
{0.563236994219653, 366.844541693072}, {0.563236994219653, 366.844541693072},
{0.573294797687861, 367.645722991578}, {0.573294797687861, 367.645722991578},
{0.583352601156069, 368.431281642859}, {0.583352601156069, 368.431281642859},
{0.593410404624277, 369.218032671753}, {0.593410404624277, 369.218032671753},
{0.603468208092486, 370.005763838889}, {0.603468208092486, 370.005763838889},
{0.613526011560694, 370.793449087224}, {0.613526011560694, 370.793449087224},
{0.623583815028902, 371.577331994297}, {0.623583815028902, 371.577331994297},
{0.633641618497110, 372.361872668242}, {0.633641618497110, 372.361872668242},
{0.643699421965318, 373.139234321453}, {0.643699421965318, 373.139234321453},
{0.653757225433526, 373.922395308171}, {0.653757225433526, 373.922395308171},
{0.663815028901734, 374.698638233448}, {0.663815028901734, 374.698638233448},
{0.673872832369942, 375.477674739791}, {0.673872832369942, 375.477674739791},
{0.683930635838150, 376.256375688174}, {0.683930635838150, 376.256375688174},
{0.693988439306358, 377.561519397833}, {0.693988439306358, 377.561519397833},
{0.704046242774567, 378.859304679191}, {0.704046242774567, 378.859304679191},
{0.714104046242775, 380.167718611141}, {0.714104046242775, 380.167718611141},
{0.724161849710983, 381.477582211590}, {0.724161849710983, 381.477582211590},
{0.734219653179191, 382.785006750526}, {0.734219653179191, 382.785006750526},
{0.744277456647399, 384.092555921096}, {0.744277456647399, 384.092555921096},
{0.754335260115607, 385.403347233778}, {0.754335260115607, 385.403347233778},
{0.764393063583815, 386.680222025901}, {0.764393063583815, 386.680222025901},
{0.774450867052023, 387.960348600215}, {0.774450867052023, 387.960348600215},
{0.784508670520231, 389.241465657245}, {0.784508670520231, 389.241465657245},
{0.794566473988439, 390.517609695133}, {0.794566473988439, 390.517609695133},
{0.804624277456647, 391.778341124162}, {0.804624277456647, 391.778341124162},
{0.814682080924856, 393.038366474572}, {0.814682080924856, 393.038366474572},
{0.824739884393064, 394.291210028831}, {0.824739884393064, 394.291210028831},
{0.834797687861272, 395.545183108074}, {0.834797687861272, 395.545183108074},
{0.844855491329480, 396.779476009029}, {0.844855491329480, 396.779476009029},
{0.854913294797688, 398.005131564908}, {0.854913294797688, 398.005131564908},
{0.864971098265896, 399.217607990930}, {0.864971098265896, 399.217607990930},
{0.875028901734104, 400.433585037519}, {0.875028901734104, 400.433585037519},
{0.885086705202312, 401.639603766860}, {0.885086705202312, 401.639603766860},
{0.895144508670520, 402.824960888722}, {0.895144508670520, 402.824960888722},
{0.905202312138728, 403.999818917039}, {0.905202312138728, 403.999818917039},
{0.915260115606936, 405.164413592803}, {0.915260115606936, 405.164413592803},
{0.925317919075145, 406.332094769783}, {0.925317919075145, 406.332094769783},
{0.935375722543353, 407.489826389568}, {0.935375722543353, 407.489826389568},
{0.945433526011561, 408.638695158694}, {0.945433526011561, 408.638695158694},
{0.955491329479769, 409.788208430892}, {0.955491329479769, 409.788208430892},
{0.965549132947977, 410.920696471959}, {0.965549132947977, 410.920696471959},
{0.975606936416185, 412.071117188526}, {0.975606936416185, 412.071117188526},
{0.985664739884393, 413.227362102269}, {0.985664739884393, 413.227362102269},
{0.995722543352601, 414.376295532213}, {0.995722543352601, 414.376295532213},
{1.00578034682081, 415.517208260982}, {1.00578034682081, 415.517208260982},
{1.01583815028902, 416.685111645473}, {1.01583815028902, 416.685111645473},
{1.02589595375723, 417.874034388355}, {1.02589595375723, 417.874034388355},
{1.03595375722543, 419.050152356493}, {1.03595375722543, 419.050152356493},
{1.04601156069364, 420.226540416000}, {1.04601156069364, 420.226540416000},
{1.05606936416185, 421.431674949807}, {1.05606936416185, 421.431674949807},
{1.06612716763006, 422.651832857732}, {1.06612716763006, 422.651832857732},
{1.07618497109827, 423.879432015756}, {1.07618497109827, 423.879432015756},
{1.08624277456647, 425.096859052146}, {1.08624277456647, 425.096859052146},
{1.09630057803468, 426.370514159926}, {1.09630057803468, 426.370514159926},
{1.10635838150289, 427.681420607676}, {1.10635838150289, 427.681420607676},
{1.11641618497110, 428.990653815442}, {1.11641618497110, 428.990653815442},
{1.12647398843931, 430.308007852515}, {1.12647398843931, 430.308007852515},
{1.13653179190751, 431.623136377095}, {1.13653179190751, 431.623136377095},
{1.14658959537572, 432.942354714805}, {1.14658959537572, 432.942354714805},
{1.15664739884393, 434.260396644686}, {1.15664739884393, 434.260396644686},
{1.16670520231214, 435.580636665589}, {1.16670520231214, 435.580636665589},
{1.17676300578035, 436.903819649131}, {1.17676300578035, 436.903819649131},
{1.18682080924855, 438.235977095674}, {1.18682080924855, 438.235977095674},
{1.19687861271676, 439.760282970165}, {1.19687861271676, 439.760282970165},
{1.20693641618497, 441.285566201751}, {1.20693641618497, 441.285566201751},
{1.21699421965318, 442.823791210454}, {1.21699421965318, 442.823791210454},
{1.22705202312139, 444.354330708051}, {1.22705202312139, 444.354330708051},
{1.23710982658960, 445.893589623130}, {1.23710982658960, 445.893589623130},
{1.24716763005780, 447.437414139676}, {1.24716763005780, 447.437414139676},
{1.25722543352601, 449.016306037988}, {1.25722543352601, 449.016306037988},
{1.26728323699422, 450.605733417807}, {1.26728323699422, 450.605733417807},
{1.27734104046243, 452.197570123945}, {1.27734104046243, 452.197570123945},
{1.28739884393064, 453.799041921642}, {1.28739884393064, 453.799041921642},
{1.29745664739884, 455.395152193126}, {1.29745664739884, 455.395152193126},
{1.30751445086705, 457.004209080179}, {1.30751445086705, 457.004209080179},
{1.31757225433526, 458.623015526914}, {1.31757225433526, 458.623015526914},
{1.32763005780347, 460.238728741943}, {1.32763005780347, 460.238728741943},
{1.33768786127168, 461.860881948233}, {1.33768786127168, 461.860881948233},
{1.34774566473988, 463.495649237401}, {1.34774566473988, 463.495649237401},
{1.35780346820809, 465.129826626932}, {1.35780346820809, 465.129826626932},
{1.36786127167630, 466.767033528504}, {1.36786127167630, 466.767033528504},
{1.37791907514451, 468.408766204263}, {1.37791907514451, 468.408766204263},
{1.38797687861272, 470.059173838784}, {1.38797687861272, 470.059173838784},
{1.39803468208092, 471.721126584341}, {1.39803468208092, 471.721126584341},
{1.40809248554913, 473.414912587257}, {1.40809248554913, 473.414912587257},
{1.41815028901734, 475.097553575053}, {1.41815028901734, 475.097553575053},
{1.42820809248555, 476.805256760032}, {1.42820809248555, 476.805256760032},
{1.43826589595376, 478.512506689219}, {1.43826589595376, 478.512506689219},
{1.44832369942197, 480.230235676597}, {1.44832369942197, 480.230235676597},
{1.45838150289017, 481.950516788809}, {1.45838150289017, 481.950516788809},
{1.46843930635838, 483.675596525192}, {1.46843930635838, 483.675596525192},
{1.47849710982659, 485.408520318711}, {1.47849710982659, 485.408520318711},
{1.48855491329480, 487.160694244723}, {1.48855491329480, 487.160694244723},
{1.49861271676301, 488.912737035966}, {1.49861271676301, 488.912737035966},
{1.50867052023121, 490.676335211920}, {1.50867052023121, 490.676335211920},
{1.51872832369942, 492.446193358219}, {1.51872832369942, 492.446193358219},
{1.52878612716763, 494.230761014528}, {1.52878612716763, 494.230761014528},
{1.53884393063584, 496.014213821540}, {1.53884393063584, 496.014213821540},
{1.54890173410405, 497.800012215749}, {1.54890173410405, 497.800012215749},
{1.55895953757225, 499.586844428474}, {1.55895953757225, 499.586844428474},
{1.56901734104046, 501.389791490613}, {1.56901734104046, 501.389791490613},
{1.57907514450867, 503.197873321221}, {1.57907514450867, 503.197873321221},
{1.58913294797688, 505.017976823598}, {1.58913294797688, 505.017976823598},
{1.59919075144509, 506.842077493572}, {1.59919075144509, 506.842077493572},
{1.60924855491329, 508.681008225194}, {1.60924855491329, 508.681008225194},
{1.61930635838150, 510.531745060971}, {1.61930635838150, 510.531745060971},
{1.62936416184971, 512.383464849326}, {1.62936416184971, 512.383464849326},
{1.63942196531792, 514.249250804510}, {1.63942196531792, 514.249250804510},
{1.64947976878613, 516.119098877381}, {1.64947976878613, 516.119098877381},
{1.65953757225434, 518.005308788433}, {1.65953757225434, 518.005308788433},
{1.66959537572254, 519.910761252352}, {1.66959537572254, 519.910761252352},
{1.67965317919075, 521.812275649970}, {1.67965317919075, 521.812275649970},
{1.68971098265896, 523.727747256148}, {1.68971098265896, 523.727747256148},
{1.69976878612717, 526.524959987653}, {1.69976878612717, 526.524959987653},
{1.70982658959538, 529.326074922632}, {1.70982658959538, 529.326074922632},
{1.71988439306358, 532.152158438731}, {1.71988439306358, 532.152158438731},
{1.72994219653179, 534.995939192065}, {1.72994219653179, 534.995939192065},
{1.74000000000000, 537.866310625605},}; {1.74000000000000, 537.866310625605},
};
//encuentra el mas cercano justo anterior. //encuentra el mas cercano justo anterior.
// int index_E = LkTable.elem(find(LkTable<=t_disparo)).max(); // int index_E = LkTable.elem(find(LkTable<=t_disparo)).max();
arma::uvec index_E = find(LkTable<=t_disparo, 1, "last"); arma::uvec index_E = find(LkTable <= t_disparo, 1, "last");
// index_E.print("indice E: "); // index_E.print("indice E: ");
// uint kk = index_E(0); // uint kk = index_E(0);
if (index_E(0)<(LkTable.n_rows-1)){ if (index_E(0) < (LkTable.n_rows - 1))
double tdisparo1=LkTable(index_E(0),0); {
double tdisparo2=LkTable(index_E(0)+1,0); double tdisparo1 = LkTable(index_E(0), 0);
double E1=LkTable(index_E(0),1); double tdisparo2 = LkTable(index_E(0) + 1, 0);
double E2=LkTable(index_E(0)+1,1); double E1 = LkTable(index_E(0), 1);
double E2 = LkTable(index_E(0) + 1, 1);
E=(t_disparo-tdisparo1)*(E2-E1)/(tdisparo2-tdisparo1)+E1; E = (t_disparo - tdisparo1) * (E2 - E1) / (tdisparo2 - tdisparo1) + E1;
}else{ }
E=0; else
} {
E = 0;
}
return E; return E;
} }
std::vector<double> Vtl_Engine::get_position_ecef_m() std::vector<double> Vtl_Engine::get_position_ecef_m()
{ {
std::vector<double> temp = {42,42,42}; std::vector<double> temp = {42, 42, 42};
temp[0] = kf_x[0]; temp[0] = kf_x[0];
temp[1] = kf_x[1]; temp[1] = kf_x[1];
temp[2] = kf_x[2]; temp[2] = kf_x[2];
@ -655,7 +677,7 @@ std::vector<double> Vtl_Engine::get_position_ecef_m()
std::vector<double> Vtl_Engine::get_velocity_ecef_m_s() std::vector<double> Vtl_Engine::get_velocity_ecef_m_s()
{ {
std::vector<double> temp = {42,42,42}; std::vector<double> temp = {42, 42, 42};
temp[0] = kf_x[3]; temp[0] = kf_x[3];
temp[1] = kf_x[4]; temp[1] = kf_x[4];
temp[2] = kf_x[5]; temp[2] = kf_x[5];
@ -665,7 +687,7 @@ std::vector<double> Vtl_Engine::get_velocity_ecef_m_s()
std::vector<double> Vtl_Engine::get_accel_ecef_m_s2() std::vector<double> Vtl_Engine::get_accel_ecef_m_s2()
{ {
std::vector<double> temp = {42,42,42}; std::vector<double> temp = {42, 42, 42};
temp[0] = kf_x[6]; temp[0] = kf_x[6];
temp[1] = kf_x[7]; temp[1] = kf_x[7];
temp[2] = kf_x[8]; temp[2] = kf_x[8];
@ -674,44 +696,41 @@ std::vector<double> Vtl_Engine::get_accel_ecef_m_s2()
} }
std::vector<double> Vtl_Engine::get_position_var_ecef_m() std::vector<double> Vtl_Engine::get_position_var_ecef_m()
{ {
std::vector<double> temp = {42,42,42}; std::vector<double> temp = {42, 42, 42};
temp[0] = kf_P_x(0,0); temp[0] = kf_P_x(0, 0);
temp[1] = kf_P_x(1,1); temp[1] = kf_P_x(1, 1);
temp[2] = kf_P_x(2,2); temp[2] = kf_P_x(2, 2);
return temp; return temp;
} }
std::vector<double> Vtl_Engine::get_velocity_var_ecef_m_s() std::vector<double> Vtl_Engine::get_velocity_var_ecef_m_s()
{ {
std::vector<double> temp = {42,42,42}; std::vector<double> temp = {42, 42, 42};
temp[0] = kf_P_x(3,3); temp[0] = kf_P_x(3, 3);
temp[1] = kf_P_x(4,4); temp[1] = kf_P_x(4, 4);
temp[2] = kf_P_x(5,5); temp[2] = kf_P_x(5, 5);
return temp; return temp;
} }
std::vector<double> Vtl_Engine::get_accel_var_ecef_m_s2() std::vector<double> Vtl_Engine::get_accel_var_ecef_m_s2()
{ {
std::vector<double> temp = {42,42,42}; std::vector<double> temp = {42, 42, 42};
temp[0] = kf_P_x(6,6); temp[0] = kf_P_x(6, 6);
temp[1] = kf_P_x(7,7); temp[1] = kf_P_x(7, 7);
temp[2] = kf_P_x(8,8); temp[2] = kf_P_x(8, 8);
return temp; return temp;
} }
double Vtl_Engine::get_latitude() double Vtl_Engine::get_latitude()
{ {
return -1.0; return -1.0;
} }
double Vtl_Engine::get_longitude() double Vtl_Engine::get_longitude()
{ {
return -1.0; return -1.0;
} }
@ -722,7 +741,7 @@ double Vtl_Engine::get_height()
double Vtl_Engine::get_user_clock_offset_s() double Vtl_Engine::get_user_clock_offset_s()
{ {
double temp=0; double temp = 0;
temp = kf_x[9]; temp = kf_x[9];
return temp; return temp;

36
src/algorithms/PVT/libs/vtl_engine.h Executable file → Normal file
View File

@ -17,10 +17,10 @@
#ifndef GNSS_SDR_VTL_ENGINE_H #ifndef GNSS_SDR_VTL_ENGINE_H
#define GNSS_SDR_VTL_ENGINE_H #define GNSS_SDR_VTL_ENGINE_H
#include "MATH_CONSTANTS.h"
#include "trackingcmd.h" #include "trackingcmd.h"
#include "vtl_conf.h" #include "vtl_conf.h"
#include "vtl_data.h" #include "vtl_data.h"
#include "MATH_CONSTANTS.h"
#include <armadillo> #include <armadillo>
#include <cstdint> #include <cstdint>
#include <string> #include <string>
@ -46,17 +46,17 @@ public:
void reset(); // reset all internal states void reset(); // reset all internal states
void debug_print(); // print debug information void debug_print(); // print debug information
std::vector<TrackingCmd> trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs std::vector<TrackingCmd> trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs
std::vector<double> get_position_ecef_m(); // get_position_ecef_m std::vector<double> get_position_ecef_m(); // get_position_ecef_m
std::vector<double> get_velocity_ecef_m_s(); // get_velocity_ecef_m_s std::vector<double> get_velocity_ecef_m_s(); // get_velocity_ecef_m_s
std::vector<double> get_accel_ecef_m_s2(); // get_accel_ecef_m_s2 std::vector<double> get_accel_ecef_m_s2(); // get_accel_ecef_m_s2
std::vector<double> get_position_var_ecef_m(); // get_position_var_ecef_m std::vector<double> get_position_var_ecef_m(); // get_position_var_ecef_m
std::vector<double> get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s std::vector<double> get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s
std::vector<double> get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 std::vector<double> get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2
double get_latitude(); // get_latitude double get_latitude(); // get_latitude
double get_longitude(); // get_longitude double get_longitude(); // get_longitude
double get_height(); // get_height double get_height(); // get_height
double get_user_clock_offset_s(); // get_user_clock_offset_s; double get_user_clock_offset_s(); // get_user_clock_offset_s;
private: private:
Vtl_Conf config; Vtl_Conf config;
@ -79,7 +79,7 @@ private:
// arma::mat kf_P_x; // state error covariance matrix // arma::mat kf_P_x; // state error covariance matrix
arma::mat kf_P_x_pre; // Predicted state error covariance matrix arma::mat kf_P_x_pre; // Predicted state error covariance matrix
arma::mat kf_P_x; arma::mat kf_P_x;
arma::mat kf_S; // innovation covariance matrix arma::mat kf_S; // innovation covariance matrix
arma::mat kf_F; // state transition matrix arma::mat kf_F; // state transition matrix
arma::mat kf_H; // system matrix arma::mat kf_H; // system matrix
@ -91,7 +91,7 @@ private:
arma::mat kf_y; // measurement vector arma::mat kf_y; // measurement vector
arma::mat kf_yerr; // ERROR measurement vector arma::mat kf_yerr; // ERROR measurement vector
arma::mat 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
arma::mat kf_R_est; // measurement error covariance arma::mat kf_R_est; // measurement error covariance
@ -101,11 +101,11 @@ private:
int n_of_states; int n_of_states;
uint64_t refSampleCounter; uint64_t refSampleCounter;
bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor
bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor bool kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor
bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation bool obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation
bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
bool model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt, int counter); bool model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);
double EmpujeLkTable(double t_disparo); double EmpujeLkTable(double t_disparo);
}; };

73
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
View File

@ -49,16 +49,16 @@
#include <matio.h> // for Mat_VarCreate #include <matio.h> // for Mat_VarCreate
#include <pmt/pmt_sugar.h> // for mp #include <pmt/pmt_sugar.h> // for mp
#include <volk_gnsssdr/volk_gnsssdr.h> #include <volk_gnsssdr/volk_gnsssdr.h>
#include "iostream"
#include <algorithm> // for fill_n #include <algorithm> // for fill_n
#include <array> #include <array>
#include <cmath> // for fmod, round, floor #include <cmath> // for fmod, round, floor
#include <exception> // for exception #include <exception> // for exception
#include <iostream> // for cout, cerr #include <fstream>
#include <iostream> // for cout, cerr
#include <map> #include <map>
#include <numeric> #include <numeric>
#include <vector> #include <vector>
#include "iostream"
#include <fstream>
#if HAS_GENERIC_LAMBDA #if HAS_GENERIC_LAMBDA
#else #else
@ -637,7 +637,6 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
//std::cout<< "test cast CH "<<cmd->sample_counter <<"\n"; //std::cout<< "test cast CH "<<cmd->sample_counter <<"\n";
if (cmd->channel_id == this->d_channel) if (cmd->channel_id == this->d_channel)
{ {
arma::vec x_tmp; arma::vec x_tmp;
arma::mat F_tmp; arma::mat F_tmp;
@ -657,28 +656,34 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
double old_doppler_rate = d_x_old_old(3); double old_doppler_rate = d_x_old_old(3);
double old_code_phase_chips = d_x_old_old(0); double old_code_phase_chips = d_x_old_old(0);
if(cmd->enable_carrier_nco_cmd){ if (cmd->enable_carrier_nco_cmd)
if(cmd->enable_code_nco_cmd){ {
if(abs(d_x_old_old(2) - tmp_x(2))>50){ if (cmd->enable_code_nco_cmd)
std::cout <<"channel: "<< this->d_channel {
<< " tracking_cmd TOO FAR: " if (abs(d_x_old_old(2) - tmp_x(2)) > 50)
<< abs(d_x_old_old(2) - tmp_x(2))<< "Hz" {
<< " \n"; std::cout << "channel: " << this->d_channel
}else{ << " tracking_cmd TOO FAR: "
std::cout <<"channel: "<< this->d_channel << abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " tracking_cmd NEAR: " << " \n";
<< abs(d_x_old_old(2) - tmp_x(2))<< "Hz" }
<< " \n"; else
} {
d_x_old_old(2) = tmp_x(2); //replace DOPPLER std::cout << "channel: " << this->d_channel
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE << " tracking_cmd NEAR: "
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
}else{ << " \n";
// std::cout<<"yet to soon"<<std::endl; }
//d_x_old_old(2) = tmp_x(2); //replace DOPPLER d_x_old_old(2) = tmp_x(2); //replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
}
else
{
// std::cout<<"yet to soon"<<std::endl;
//d_x_old_old(2) = tmp_x(2); //replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
}
} }
}
// set vtl corrections flag to inform VTL from gnss_synchro object // set vtl corrections flag to inform VTL from gnss_synchro object
d_vtl_cmd_applied_now = true; d_vtl_cmd_applied_now = true;
@ -702,15 +707,15 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
dump_tracking_file.open("dump_trk_file.csv", std::ios::out | std::ios::app); dump_tracking_file.open("dump_trk_file.csv", std::ios::out | std::ios::app);
dump_tracking_file.precision(15); dump_tracking_file.precision(15);
if (!dump_tracking_file) if (!dump_tracking_file)
{ {
std::cout << "dump_tracking_file not created!"; std::cout << "dump_tracking_file not created!";
} }
else else
{ {
dump_tracking_file << "doppler_corr" dump_tracking_file << "doppler_corr"
<< ","<< this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3)<< "," << old_doppler_rate << "\n"; << "," << this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3) << "," << old_doppler_rate << "\n";
dump_tracking_file.close(); dump_tracking_file.close();
} }
} }
} }
else else
@ -1252,7 +1257,7 @@ void kf_tracking::run_Kf()
// new code phase estimation // new code phase estimation
d_code_error_kf_chips = d_x_new_new(0); d_code_error_kf_chips = d_x_new_new(0);
d_x_new_new(0)=0; // reset error estimation because the NCO corrects the code phase d_x_new_new(0) = 0; // reset error estimation because the NCO corrects the code phase
// new carrier phase estimation // new carrier phase estimation
d_carrier_phase_kf_rad = d_x_new_new(1); d_carrier_phase_kf_rad = d_x_new_new(1);