mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-02-10 08:00:10 +00:00
Code Issues Releases Wiki Activity

ADD: vtl file output

Browse Source
This commit is contained in:
M.A.Gomez 2022-11-24 17:04:18 +00:00
parent 25362551e6
commit f716b04002
2 changed files with 68 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1053,6 +1053,9 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
dops(index_aux, azel.data(), 0.0, d_dop.data());
}
this->set_valid_position(true);
//this->set_averaging_flag(true);
//this->set_averaging_depth(100);
//this->perform_pos_averaging();
std::array<double, 4> rx_position_and_time{};
rx_position_and_time[0] = pvt_sol.rr[0]; // [m]
rx_position_and_time[1] = pvt_sol.rr[1]; // [m]
@ -1119,10 +1122,8 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
new_vtl_data.sat_health_flag(n) = svh.at(n);
new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0];
// TODO: first version of VTL works only with ONE frequency band (band #0 is L1)
//new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges
//To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp);
//corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts)
//cout<<"dtr "<<rx_position_and_time[3]*SPEED_OF_LIGHT_M_S<<"m";
new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]+SPEED_OF_LIGHT_M_S * dts[n * 2];
new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0];
new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0];
@ -1154,23 +1155,23 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
//new_vtl_data.debug_print();
double PVT[6]={0};
vtl_engine.vtl_loop(new_vtl_data, PVT);
pvt_sol.rr[0]=PVT[0];
pvt_sol.rr[1]=PVT[1];
pvt_sol.rr[2]=PVT[2];
pvt_sol.rr[3]=PVT[3];
pvt_sol.rr[4]=PVT[4];
pvt_sol.rr[5]=PVT[5];
// pvt_sol.rr[0]=PVT[0];
// pvt_sol.rr[1]=PVT[1];
// pvt_sol.rr[2]=PVT[2];
// pvt_sol.rr[3]=PVT[3];
// pvt_sol.rr[4]=PVT[4];
// pvt_sol.rr[5]=PVT[5];
}
else{
//MAGL: the code should not enter here once the vtl has started
// .. but it does!
//and not only that but pvt_sol.rr seems to have NOT reasonable values
pvt_sol.rr[0]=rx_position_and_time[0]; // [m]
pvt_sol.rr[1]=rx_position_and_time[1]; // [m]
pvt_sol.rr[2]=rx_position_and_time[2]; // [m]
pvt_sol.rr[3]=4.2e6;
pvt_sol.rr[4]=4.2e6;
pvt_sol.rr[5]=4.2e6;
// pvt_sol.rr[0]=rx_position_and_time[0]; // [m]
// pvt_sol.rr[1]=rx_position_and_time[1]; // [m]
// pvt_sol.rr[2]=rx_position_and_time[2]; // [m]
// pvt_sol.rr[3]=4.2e6;
// pvt_sol.rr[4]=4.2e6;
// pvt_sol.rr[5]=4.2e6;
}
// compute Ground speed and COG
double ground_speed_ms = 0.0;

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

@ -16,6 +16,7 @@
#include "vtl_engine.h"
#include "iostream"
#include <fstream>
using namespace std;
@ -37,7 +38,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
kf_P_x = arma::eye(8, 8); //TODO: use a real value.
kf_x = arma::zeros(8, 1);
kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number);
double kf_dt=1e-1;
double kf_dt=0.1;
kf_Q = arma::zeros(8, 8);
kf_F = arma::zeros(8, 8);
@ -59,8 +60,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
// ################## Kalman Tracking ######################################
static uint32_t counter=0; //counter
static double prueba[6]={0}; //counter
counter=counter+1; //uint64_t
cout << "counter" << counter;
cout << "counter" << counter<<endl;
if(counter<3000){ //
// receiver solution from rtklib_solver
@ -74,18 +76,37 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
kf_x(7)=new_data.rx_dts(1);
}
else{
kf_x=new_data.kf_state;
kf_x(0)=prueba[0];
kf_x(1)=prueba[1];
kf_x(2)=prueba[2];
kf_x(3)=prueba[3];
kf_x(4)=prueba[4];
kf_x(5)=prueba[5];
kf_x(6)=new_data.rx_dts(0);
kf_x(7)=new_data.rx_dts(1);
kf_P_x=new_data.kf_P;
}
for (int32_t i = 0; i < 8; i++) // State error Covariance Matrix Q (PVT)
// State error Covariance Matrix Q (PVT)
for (int32_t i = 0; i < 8; i++)
{
// It is diagonal 8x8 matrix
kf_Q(i, i) = new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate.
kf_Q(i, i) = 50.0;//new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate.
}
// Measurement error Covariance Matrix R assembling
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) = 5.0; //TODO: fill with real values.
kf_R(i+new_data.sat_number, i+new_data.sat_number) = 10.0;
}
// Kalman state prediction (time update)
// kf_x.print(" KF RTKlib STATE");
kf_x.print(" KF RTKlib STATE");
// cout<<"state_fed"<<endl;
// for(int i=0;i<6;i++){
// cout<<prueba[i]<<endl;
// }
new_data.kf_state=kf_x;
kf_x = kf_F * kf_x; // state prediction
kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction
@ -145,7 +166,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
kf_yerr(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(i);
}
kf_yerr.print("KF measurement vector difference");
kf_yerr.print("KF innovation");
// DOUBLES DIFFERENCES
// kf_yerr = arma::zeros(2*new_data.sat_number, 1);
@ -157,29 +178,42 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
// kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-(new_data.doppler_hz(i)-new_data.doppler_hz(i-1));
// }
// kf_yerr.print("DOUBLES DIFFERENCES");
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling
{
// It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error)
kf_R(i, i) = 20.0; //TODO: fill with real values.
kf_R(i+new_data.sat_number, i+new_data.sat_number) = 10.0;
}
// Kalman filter update step
kf_S = kf_H * kf_P_x* kf_H.t() + kf_R; // innovation covariance matrix (S)
kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain
kf_xerr = kf_K * (kf_yerr); // Error state estimation
//kf_xerr.print(" kf_xerr");
kf_x = kf_x + kf_xerr; // updated state estimation (a priori + error)
kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix
new_data.kf_state=kf_x; //updated state estimation
new_data.kf_P=kf_P_x; //update state estimation error covariance
// States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S)
kf_x.print(" KF STATE");
// TODO: compare how KF state diverges from RTKLIB solution!
fstream dump_vtl_file;
dump_vtl_file.open("dump_vtl_file.csv", ios::out|ios::app);
if (!dump_vtl_file) {
cout << "File not created!";
}
else {
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)<<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)<<endl;
dump_vtl_file.close();
}
PVT_out[0]=kf_x(0);
PVT_out[1]=kf_x(1);
PVT_out[2]=kf_x(2);
PVT_out[3]=kf_x(3);
PVT_out[4]=kf_x(4);
PVT_out[5]=kf_x(5);
// States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S)
prueba[0]=kf_x(0);
prueba[1]=kf_x(1);
prueba[2]=kf_x(2);
prueba[3]=kf_x(3);
prueba[4]=kf_x(4);
prueba[5]=kf_x(5);
// kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S;
// kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S;
@ -188,8 +222,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
//new_data.pr_res.print(" pr RESIDUALS");
//!new_data.kf_state.print(" KF RTKlib STATE");
//!cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state;
//!cout << " KF posteriori STATE diference %1" << (kf_x-new_data.kf_state)/new_data.kf_state;
cout << " KF posteriori STATE diference %100" << (kf_x-new_data.kf_state)/new_data.kf_state*100;
// // ################## Geometric Transformation ######################################