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

6
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
{
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_vtl_dump_filename.insert(end_filename, "_vtl");
d_vtl_dump_file.open(d_vtl_dump_filename, std::ios::out | std::ios::binary);
@ -353,7 +353,6 @@ bool Rtklib_Solver::save_vtl_matfile() const
matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73);
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
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);
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);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
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_engine.vtl_loop(vtl_data);
}
else
{
@ -2088,7 +2085,6 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
try
{
double tmp_double;
uint32_t tmp_uint32;
// TOW

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

@ -16,8 +16,8 @@
#include "vtl_data.h"
#include "vector"
#include "armadillo"
#include "vector"
Vtl_Data::Vtl_Data()
{
@ -66,5 +66,3 @@ void Vtl_Data::debug_print()
//doppler_hz.print("satellite Carrier Dopplers [Hz]");
// carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
}

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

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

@ -22,9 +22,9 @@ using namespace std;
Vtl_Engine::Vtl_Engine()
{
counter=0;
refSampleCounter=0;
n_of_states=11;
counter = 0;
refSampleCounter = 0;
n_of_states = 11;
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);
}
@ -38,16 +38,17 @@ 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;
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;
delta_t_cmd = delta_t_cmd+delta_t_vtl; // update timer for vtl trk command
if(delta_t_cmd>=0.3){
delta_t_cmd = delta_t_cmd + delta_t_vtl; // update timer for vtl trk command
if (delta_t_cmd >= 0.3)
{
flag_cmd = true;
delta_t_cmd = 0; // reset timer for vtl trk command
}
@ -62,23 +63,24 @@ 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);
bool test = 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);
kf_yerr = arma::zeros(3 * new_data.sat_number, 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_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 ######################################
counter++; //uint64_t
//new_data.kf_state.print("new_data kf initial");
uint32_t closure_point=3;
uint32_t closure_point = 3;
if (counter < closure_point)
{
// // receiver solution from rtklib_solver
kf_dx=kf_x;
kf_dx = kf_x;
kf_x(0) = new_data.rx_p(0);
kf_x(1) = new_data.rx_p(1);
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(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
}
else
@ -100,7 +102,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);
test = 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;
@ -127,9 +129,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
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)
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 + 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, 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 + 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){
// 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_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);
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
{
new_data.sat_LOS(i, 0) = a_x(i);
@ -168,30 +170,30 @@ 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);
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
{
// 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);
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
// Kalman filter update step
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_xerr = kf_K * (kf_yerr); // Error state estimation
//kf_xerr.row(5)=kf_K.row(5)*kf_yerr;
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_dx=kf_x;
kf_x = kf_x-kf_xerr; // updated state estimation (a priori + error)
// kf_x.print("state of kalman: ");
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_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);
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
{
@ -200,7 +202,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);
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
kf_yerr = kf_H * kf_xerr;
// Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
@ -219,19 +221,25 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
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_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.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_cmd){
if (flag_cmd)
{
trk_cmd.enable_carrier_nco_cmd = true;
}else{
}
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
}else{
}
else
{
trk_cmd.enable_code_nco_cmd = true;
}
trk_cmd.sample_counter = new_data.sample_counter;
@ -298,7 +306,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)
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
{
@ -307,33 +315,36 @@ bool Vtl_Engine::kf_H_fill(arma::mat &kf_H,int sat_number, arma::colvec ax, arma
kf_H(i, 1) = ay(i);
kf_H(i, 2) = az(i);
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, 4) = ay(i);
kf_H(i + sat_number, 5) = az(i);
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, 8) = az(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, 8) = az(i) * kf_dt;
kf_H(i + sat_number, 10) = 1.0;
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, 5) = 0;//az(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, 8) = az(i);
kf_H(i + 2*sat_number, 10) = kf_dt;
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, 5) = 0; //az(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, 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)
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(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(0, 3) = kf_dt;
kf_F(0, 6) = 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(4, 7) = kf_dt;
@ -344,7 +355,7 @@ bool Vtl_Engine::kf_F_fill(arma::mat &kf_F,double 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)
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
{
@ -372,21 +383,21 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r
for (int32_t i = 0; i < sat_number; i++) // Measurement vector
{
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 + 2*sat_number) = -rhoDot2_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);
}
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 acc_vec;
arma::colvec u_dir;
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 densidad=1.0;
double densidad = 1.0;
double ballistic_coef = 0.007;
//vector velocidad
@ -395,16 +406,21 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
//modulo de la velocidad
double u = norm(u_vec, 2);
if(counter>1500){
if(u>6){
t_disparo=t_disparo+dt;
std::cout<<"u : "<<u<<endl;
double diam_cohete=120.0e-3;// 120 mm
double mass_rocket=50.0; //50Kg
if (counter > 1500)
{
if (u > 6)
{
t_disparo = t_disparo + dt;
std::cout << "u : " << u << endl;
double diam_cohete = 120.0e-3; // 120 mm
double mass_rocket = 50.0; //50Kg
if(t_disparo<.2){
u_dir={.90828, -.13984, -.388756};
}else{
if (t_disparo < .2)
{
u_dir = {.90828, -.13984, -.388756};
}
else
{
u_dir = u_vec / u;
}
// u_dir.print("u_dir");
@ -418,24 +434,26 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
Empuje = EmpujeLkTable(t_disparo);
// cout<<"Empuje: "<<Empuje<<endl;
acc_vec = -(GNSS_PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir
+gravity_ECEF+Empuje*u_dir;
acc_vec = -(GNSS_PI * densidad * diam_cohete * diam_cohete / 8) * ballistic_coef * u * u_dir + gravity_ECEF + Empuje * u_dir;
// 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);
}else{
t_disparo=0;
}
else
{
t_disparo = 0;
// % return
acc_x = 0;
acc_y = 0;
acc_z = 0;
}
}else{
}
else
{
// % return
acc_x = 0;
acc_y = 0;
@ -448,7 +466,7 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
double Vtl_Engine::EmpujeLkTable(double t_disparo)
{
double E;
arma::mat LkTable={
arma::mat LkTable = {
{0.0, 391.083112445424},
{0.0100578034682081, 385.626317230813},
{0.0201156069364162, 379.253652903964},
@ -622,22 +640,26 @@ double Vtl_Engine::EmpujeLkTable(double t_disparo)
{1.70982658959538, 529.326074922632},
{1.71988439306358, 532.152158438731},
{1.72994219653179, 534.995939192065},
{1.74000000000000, 537.866310625605},};
{1.74000000000000, 537.866310625605},
};
//encuentra el mas cercano justo anterior.
// 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: ");
// uint kk = index_E(0);
if (index_E(0)<(LkTable.n_rows-1)){
double tdisparo1=LkTable(index_E(0),0);
double tdisparo2=LkTable(index_E(0)+1,0);
double E1=LkTable(index_E(0),1);
double E2=LkTable(index_E(0)+1,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 E1 = LkTable(index_E(0), 1);
double E2 = LkTable(index_E(0) + 1, 1);
E=(t_disparo-tdisparo1)*(E2-E1)/(tdisparo2-tdisparo1)+E1;
}else{
E=0;
E = (t_disparo - tdisparo1) * (E2 - E1) / (tdisparo2 - tdisparo1) + E1;
}
else
{
E = 0;
}
return E;
@ -645,7 +667,7 @@ double Vtl_Engine::EmpujeLkTable(double t_disparo)
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[1] = kf_x[1];
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> temp = {42,42,42};
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_x[3];
temp[1] = kf_x[4];
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> temp = {42,42,42};
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_x[6];
temp[1] = kf_x[7];
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> temp = {42,42,42};
temp[0] = kf_P_x(0,0);
temp[1] = kf_P_x(1,1);
temp[2] = kf_P_x(2,2);
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_P_x(0, 0);
temp[1] = kf_P_x(1, 1);
temp[2] = kf_P_x(2, 2);
return temp;
}
std::vector<double> Vtl_Engine::get_velocity_var_ecef_m_s()
{
std::vector<double> temp = {42,42,42};
temp[0] = kf_P_x(3,3);
temp[1] = kf_P_x(4,4);
temp[2] = kf_P_x(5,5);
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_P_x(3, 3);
temp[1] = kf_P_x(4, 4);
temp[2] = kf_P_x(5, 5);
return temp;
}
std::vector<double> Vtl_Engine::get_accel_var_ecef_m_s2()
{
std::vector<double> temp = {42,42,42};
temp[0] = kf_P_x(6,6);
temp[1] = kf_P_x(7,7);
temp[2] = kf_P_x(8,8);
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_P_x(6, 6);
temp[1] = kf_P_x(7, 7);
temp[2] = kf_P_x(8, 8);
return temp;
}
double Vtl_Engine::get_latitude()
{
return -1.0;
}
double Vtl_Engine::get_longitude()
{
return -1.0;
}
@ -722,7 +741,7 @@ double Vtl_Engine::get_height()
double Vtl_Engine::get_user_clock_offset_s()
{
double temp=0;
double temp = 0;
temp = kf_x[9];
return temp;

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

@ -17,10 +17,10 @@
#ifndef GNSS_SDR_VTL_ENGINE_H
#define GNSS_SDR_VTL_ENGINE_H
#include "MATH_CONSTANTS.h"
#include "trackingcmd.h"
#include "vtl_conf.h"
#include "vtl_data.h"
#include "MATH_CONSTANTS.h"
#include <armadillo>
#include <cstdint>
#include <string>
@ -102,10 +102,10 @@ private:
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_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_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);
bool model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);
double EmpujeLkTable(double t_disparo);
};

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

@ -49,16 +49,16 @@
#include <matio.h> // for Mat_VarCreate
#include <pmt/pmt_sugar.h> // for mp
#include <volk_gnsssdr/volk_gnsssdr.h>
#include "iostream"
#include <algorithm> // for fill_n
#include <array>
#include <cmath> // for fmod, round, floor
#include <exception> // for exception
#include <fstream>
#include <iostream> // for cout, cerr
#include <map>
#include <numeric>
#include <vector>
#include "iostream"
#include <fstream>
#if HAS_GENERIC_LAMBDA
#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";
if (cmd->channel_id == this->d_channel)
{
arma::vec x_tmp;
arma::mat F_tmp;
@ -657,23 +656,29 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
double old_doppler_rate = d_x_old_old(3);
double old_code_phase_chips = d_x_old_old(0);
if(cmd->enable_carrier_nco_cmd){
if(cmd->enable_code_nco_cmd){
if(abs(d_x_old_old(2) - tmp_x(2))>50){
std::cout <<"channel: "<< this->d_channel
if (cmd->enable_carrier_nco_cmd)
{
if (cmd->enable_code_nco_cmd)
{
if (abs(d_x_old_old(2) - tmp_x(2)) > 50)
{
std::cout << "channel: " << this->d_channel
<< " tracking_cmd TOO FAR: "
<< abs(d_x_old_old(2) - tmp_x(2))<< "Hz"
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " \n";
}else{
std::cout <<"channel: "<< this->d_channel
}
else
{
std::cout << "channel: " << this->d_channel
<< " tracking_cmd NEAR: "
<< abs(d_x_old_old(2) - tmp_x(2))<< "Hz"
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " \n";
}
d_x_old_old(2) = tmp_x(2); //replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
}else{
}
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
@ -708,7 +713,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
else
{
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();
}
}
@ -1252,7 +1257,7 @@ void kf_tracking::run_Kf()
// new code phase estimation
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
d_carrier_phase_kf_rad = d_x_new_new(1);