From c46a14dd5b56d09a9a4d01ae50277167108cb5c3 Mon Sep 17 00:00:00 2001
From: "M.A.Gomez" <gomezlma@inta.es>
Date: Sun, 11 Dec 2022 00:33:45 +0000
Subject: [PATCH] ADD: vtl_kf obsv output

---
 src/algorithms/PVT/libs/vtl_engine.cc | 77 +++++++++++++--------------
 src/algorithms/PVT/libs/vtl_engine.h  |  3 ++
 2 files changed, 40 insertions(+), 40 deletions(-)

diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc
index b82025950..9d4b0a36d 100755
--- a/src/algorithms/PVT/libs/vtl_engine.cc
+++ b/src/algorithms/PVT/libs/vtl_engine.cc
@@ -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
diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h
index b7b79ceae..767abe5c4 100755
--- a/src/algorithms/PVT/libs/vtl_engine.h
+++ b/src/algorithms/PVT/libs/vtl_engine.h
@@ -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;