mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-06-25 14:42:57 +00:00
Code Issues Releases Wiki Activity

fix: vtl_engine bug

Browse Source
This commit is contained in:
M.A. Gomez 2023-03-14 15:29:18 +01:00
parent edad601ed3
commit 2d4c5b4d7b
2 changed files with 64 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,8 +25,11 @@ Vtl_Engine::Vtl_Engine()
counter = 0;
refSampleCounter = 0;
n_of_states = 11;
delta_t_cmd = 0;
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);
}
Vtl_Engine::~Vtl_Engine()
@ -38,14 +41,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
//TODO: Implement main VTL loop here
using arma::as_scalar;
if (refSampleCounter = 0)
if (refSampleCounter == 0)
{
refSampleCounter = new_data.sample_counter;
}
double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0;
refSampleCounter = new_data.sample_counter;
static double delta_t_cmd = 0;
bool flag_cmd = false;
bool flag_time_cmd = false;
delta_t_cmd = delta_t_cmd + delta_t_vtl; // update timer for vtl trk command
if (delta_t_cmd >= 0.3)
{
@ -63,7 +67,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_Q = 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);
kf_F_fill(kf_F, kf_dt);
//kf_H = arma::zeros(3 * new_data.sat_number, n_of_states);
kf_y = arma::zeros(3 * new_data.sat_number, 1);
@ -74,6 +78,10 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
;
// ################## Kalman Tracking ######################################
counter++; //uint64_t
if (counter>2500){
flag_time_cmd = true;
}
//new_data.kf_state.print("new_data kf initial");
uint32_t closure_point = 3;
@ -102,7 +110,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
double acc_x = 0;
double acc_y = 0;
double acc_z = 0;
test = model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter);
// model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter);
kf_x(6) = acc_x;
kf_x(7) = acc_y;
kf_x(8) = acc_z;
@ -161,7 +169,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
a_y = arma::zeros(new_data.sat_number, 1);
a_z = arma::zeros(new_data.sat_number, 1);
// 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);
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
{
new_data.sat_LOS(i, 0) = a_x(i);
@ -170,14 +178,14 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
}
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);
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
{
// rhoDot2_pri(i)=(rhoDot_pri(i)-rhoDot_pri_old(i))/kf_dt;
}
// 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);
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
// Kalman filter update step
@ -193,7 +201,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error)
// kf_x.print("state of kalman: ");
// // ################## 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);
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
{
@ -202,7 +210,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
//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);
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
kf_yerr = kf_H * kf_xerr;
// Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
@ -224,6 +232,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
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;
if (flag_time_cmd)
{
if (flag_cmd)
{
trk_cmd.enable_carrier_nco_cmd = true;
@ -232,16 +242,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
{
trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue
}
if (counter < 1500)
{
// std::cout<<"yet to soon"<<std::endl;
trk_cmd.enable_code_nco_cmd = false; // do NOT apply corrections! initial convergence issue
}
else
{
trk_cmd.enable_code_nco_cmd = true;
trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue
}
trk_cmd.sample_counter = new_data.sample_counter;
trk_cmd.channel_id = channel;
trk_cmd_outs.push_back(trk_cmd);
@ -253,40 +260,26 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// << " \n";
// }
}
// fstream dump_vtl_file;
// dump_vtl_file.open("dump_vtl_file.csv", ios::out | ios::app);
// dump_vtl_file.precision(15);
// if (!dump_vtl_file)
// {
// cout << "File not created!";
// }
// else
// {
fstream dump_vtl_file;
dump_vtl_file.open("dump_vtl_file.csv", ios::out | ios::app);
dump_vtl_file.precision(15);
if (!dump_vtl_file)
{
cout << "File not created!";
}
else
{
// if(new_data.sat_number>5){
// // dump_vtl_file << "pr_m"
// // << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2)
// // << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)<<endl;
// // dump_vtl_file << "prDot_m"
// // << "," << kf_yerr(new_data.sat_number)<< "," << kf_yerr(new_data.sat_number+1)<< "," << kf_yerr(new_data.sat_number+2)
// // << "," << kf_yerr(new_data.sat_number+3) << "," << kf_yerr(new_data.sat_number+4)<< "," << kf_yerr(new_data.sat_number+5)<< endl;
// // // dump_vtl_file << "K_column"
// // // << "," << kf_K.row(5)(6) << "," << kf_K.row(5)(7) << "," <<kf_K.row(5)(8)<< "," << kf_K.row(5)(9)
// // // << "," << kf_K.row(5)(10) << "," << kf_K.row(5)(11) << endl;
// // // dump_vtl_file << "K_column2"
// // // << "," << kf_K.row(5)(0) << "," << kf_K.row(5)(1) << "," <<kf_K.row(5)(2)<< "," << kf_K.row(5)(3)
// // // << "," << kf_K.row(5)(4) << "," << kf_K.row(5)(5) << endl;
// }
// 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;
// dump_vtl_file << "kf_xerr"
// << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) << "," << kf_xerr(6) << "," << kf_xerr(7)<< "," << kf_xerr(8) <<"," << kf_xerr(9) <<"," << kf_xerr(10)<< endl;
// dump_vtl_file << "rtklib_state"
// << "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << "," << delta_t_vtl << endl;
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;
dump_vtl_file << "kf_xerr"
<< "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) << "," << kf_xerr(6) << "," << kf_xerr(7)<< "," << kf_xerr(8) <<"," << kf_xerr(9) <<"," << kf_xerr(10)<< endl;
dump_vtl_file << "rtklib_state"
<< "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << "," << delta_t_vtl << endl;
// dump_vtl_file << "filt_dopp_sat"
// << "," << doppler_hz_filt(0) << "," << doppler_hz_filt(1) << "," << doppler_hz_filt(2) << "," << doppler_hz_filt(3) << "," << doppler_hz_filt(4) <<endl;
// dump_vtl_file.close();
// }
dump_vtl_file.close();
}
return true;
}
@ -306,7 +299,7 @@ void Vtl_Engine::configure(Vtl_Conf config_)
//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)
void 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
{
@ -333,11 +326,9 @@ bool Vtl_Engine::kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arm
kf_H(i + 2 * sat_number, 8) = az(i);
kf_H(i + 2 * sat_number, 10) = kf_dt;
}
return -1;
}
bool Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt)
void 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;
@ -351,11 +342,9 @@ bool Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt)
kf_F(5, 8) = kf_dt;
kf_F(9, 10) = kf_dt;
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)
void 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
{
@ -375,10 +364,9 @@ bool Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colv
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;
}
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)
void 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
{
@ -386,10 +374,9 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r
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);
}
return -1;
}
bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter)
void 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 acc_vec;
@ -411,7 +398,7 @@ bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma
if (u > 6)
{
t_disparo = t_disparo + dt;
std::cout << "u : " << u << endl;
// std::cout << "u : " << u << endl;
double diam_cohete = 120.0e-3; // 120 mm
double mass_rocket = 50.0; //50Kg
@ -438,7 +425,7 @@ bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma
// acc_vec.print("acc_vec");
// % return
cout << "modelo 3Dof actuando,t:" << t_disparo << endl;
// cout << "modelo 3Dof actuando,t:" << t_disparo << endl;
acc_x = acc_vec(0);
acc_y = acc_vec(1);
acc_z = acc_vec(2);
@ -459,8 +446,6 @@ bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma
acc_y = 0;
acc_z = 0;
}
return -1;
}
double Vtl_Engine::EmpujeLkTable(double t_disparo)

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

@ -100,12 +100,13 @@ private:
uint32_t counter;
int n_of_states;
uint64_t refSampleCounter;
double delta_t_cmd = 0;
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 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 model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);
void 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
void kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor
void 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
void 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);
void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);
double EmpujeLkTable(double t_disparo);
};