mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-04 01:03:04 +00:00
Code Issues Releases Wiki Activity

ADD: vtl_kf obsv output

Browse Source
This commit is contained in:
M.A.Gomez
2022-12-11 00:33:45 +00:00
parent 3659c29182
commit c46a14dd5b
2 changed files with 40 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -35,7 +35,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
// ################## Kalman filter initialization ######################################
// covariances (static)
kf_P_x = arma::eye(8, 8); //TODO: use a real value.
kf_P_x = arma::eye(8, 8)*1.0; //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=0.1;
@@ -58,7 +58,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
counter = counter+1; //uint64_t
cout << "counter" << counter<<endl;
//new_data.kf_state.print("new_data kf initial");
if(counter<100){ //
if(counter<3000){ //
// receiver solution from rtklib_solver
kf_x(0) = new_data.rx_p(0);
kf_x(1) = new_data.rx_p(1);
@@ -69,6 +69,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
kf_x(6) = new_data.rx_dts(0)*SPEED_OF_LIGHT_M_S;
kf_x(7) = new_data.rx_dts(1)*SPEED_OF_LIGHT_M_S;
kf_x = kf_F * kf_x; // state prediction
} else {
// receiver solution from previous KF step
kf_x(0) = new_data.kf_state[0];
@@ -91,13 +93,13 @@ 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) = 40.0; //TODO: fill with real values.
kf_R(i+new_data.sat_number, i+new_data.sat_number) = 10.0;
kf_R(i, i) = 10.0; //TODO: fill with real values.
kf_R(i+new_data.sat_number, i+new_data.sat_number) = 30.0;
}
// Kalman state prediction (time update)
new_data.kf_state=kf_x;
kf_x = kf_F * kf_x; // state prediction
//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
//from error state variables to variables
// From state variables definition
@@ -114,6 +116,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
d = arma::zeros(new_data.sat_number, 1);
rho_pri = arma::zeros(new_data.sat_number, 1);
rhoDot_pri = arma::zeros(new_data.sat_number, 1);
rho_pri_filt= arma::zeros(new_data.sat_number, 1);
rhoDot_pri_filt = arma::zeros(new_data.sat_number, 1);
doppler_hz_filt = arma::zeros(new_data.sat_number, 1);
a_x = 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);
@@ -151,22 +158,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
{
//kf_y(i) = new_data.pr_m(i); // i-Satellite
//kf_y(i+new_data.sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; // i-Satellite
kf_yerr(i)=rho_pri(i)-new_data.pr_m(i);//-0.000157*SPEED_OF_LIGHT_M_S;
kf_yerr(i)=rho_pri(i)-new_data.pr_m(i);
kf_yerr(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1)-rhoDot_pri(i);
}
// DOUBLES DIFFERENCES
// kf_yerr = arma::zeros(2*new_data.sat_number, 1);
// for (int32_t i = 1; i < new_data.sat_number; i++) // Measurement vector
// {
// kf_y(i)=new_data.pr_m(i)-new_data.pr_m(i-1);
// kf_yerr(i)=kf_y(i)-(rho_pri(i)+rho_pri(i-1));
// kf_y(i+new_data.sat_number)=(rhoDot_pri(i)-rhoDot_pri(i-1))/Lambda_GPS_L1;
// 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");
// 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
@@ -175,23 +171,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
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_P=kf_P_x;
new_data.kf_state=kf_x; //updated state estimation
//new_data.kf_state.print("computed kf_state");
// 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 {
// 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)<<endl;
// dump_vtl_file << "kf_state"<< ","<<new_data.kf_state(0)<< ","<<new_data.kf_state(1)<< ","<<new_data.kf_state(2)<< ","<<new_data.kf_state(3)<< ","<<new_data.kf_state(4)<< ","<<new_data.kf_state(5)<< ","<<new_data.kf_state(6)<< ","<<new_data.kf_state(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();
// }
//new_data.pr_res.print(" pr RESIDUALS");
//!new_data.kf_state.print(" KF RTKlib STATE");
// cout << " KF posteriori STATE diference %100" << (kf_x-new_data.kf_state)/new_data.kf_state*100;
// // ################## Geometric Transformation ######################################
// // x_u=kf_x(0);
@@ -212,13 +192,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
d(i)=sqrt(d(i));
//compute pseudorange estimation
rho_pri(i)=d(i)+kf_x(6)*SPEED_OF_LIGHT_M_S;
rho_pri(i)=d(i)+kf_x(6);
//compute LOS sat-receiver vector components
a_x(i)=-(new_data.sat_p(i, 0)-kf_x(0))/d(i);
a_y(i)=-(new_data.sat_p(i, 1)-kf_x(1))/d(i);
a_z(i)=-(new_data.sat_p(i, 2)-kf_x(2))/d(i);
//compute pseudorange rate estimation
rhoDot_pri(i)=(new_data.sat_v(i, 0)-kf_x(3))*a_x(i)+(new_data.sat_v(i, 1)-kf_x(4))*a_y(i)+(new_data.sat_v(i, 2)-kf_x(5))*a_z(i)+kf_x(7)*SPEED_OF_LIGHT_M_S;
rhoDot_pri(i)=(new_data.sat_v(i, 0)-kf_x(3))*a_x(i)+(new_data.sat_v(i, 1)-kf_x(4))*a_y(i)+(new_data.sat_v(i, 2)-kf_x(5))*a_z(i)+kf_x(7);
}
kf_H = arma::zeros(2*new_data.sat_number,8);
@@ -237,12 +217,29 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector
{
rho_pri(i)=new_data.pr_m(i)-kf_yerr(i); // now filtered
rhoDot_pri(i)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-kf_yerr(i+new_data.sat_number); // now filtered
rho_pri_filt(i)=new_data.pr_m(i)+kf_yerr(i); // now filtered
rhoDot_pri_filt(i)=(new_data.doppler_hz(i)*Lambda_GPS_L1+kf_x(7))-kf_yerr(i+new_data.sat_number); // now filtered
// TO DO: convert rhoDot_pri to doppler shift!
// Doppler shift defined as pseudorange rate measurement divided by the negative of carrier wavelength.
rhoDot_pri(i)=-rhoDot_pri(i)/Lambda_GPS_L1;
doppler_hz_filt(i)=(rhoDot_pri_filt(i)-kf_x(7))/Lambda_GPS_L1;
}
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 {
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)<<endl;
dump_vtl_file << "kf_state"<< ","<<new_data.kf_state(0)<< ","<<new_data.kf_state(1)<< ","<<new_data.kf_state(2)<< ","<<new_data.kf_state(3)<< ","<<new_data.kf_state(4)<< ","<<new_data.kf_state(5)<< ","<<new_data.kf_state(6)<< ","<<new_data.kf_state(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 << "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();
}
//TODO: Fill the tracking commands outputs
// Notice: keep the same satellite order as in the Vtl_Data matrices
// sample code

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

@@ -66,6 +66,9 @@ private:
arma::colvec d;
arma::colvec rho_pri;
arma::colvec rhoDot_pri;
arma::colvec rho_pri_filt;
arma::colvec rhoDot_pri_filt;
arma::colvec doppler_hz_filt;
arma::colvec a_x;
arma::colvec a_y;
arma::colvec a_z;