From f716b040027a569b11e7aec33859d45fd45205cf Mon Sep 17 00:00:00 2001
From: "M.A.Gomez" <gomezlma@inta.es>
Date: Thu, 24 Nov 2022 17:04:18 +0000
Subject: [PATCH] ADD: vtl file output

---
 src/algorithms/PVT/libs/rtklib_solver.cc | 29 +++++-----
 src/algorithms/PVT/libs/vtl_engine.cc    | 73 +++++++++++++++++-------
 2 files changed, 68 insertions(+), 34 deletions(-)

diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc
index 7e438035d..f9030d641 100755
--- a/src/algorithms/PVT/libs/rtklib_solver.cc
+++ b/src/algorithms/PVT/libs/rtklib_solver.cc
@@ -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;
diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc
index 7deb893d9..817f0cbe0 100755
--- a/src/algorithms/PVT/libs/vtl_engine.cc
+++ b/src/algorithms/PVT/libs/vtl_engine.cc
@@ -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 ######################################