ADD: first KF close loop

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -1151,10 +1151,14 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                             new_vtl_data.rx_dts(0) = rx_position_and_time[3];
                             new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6;  // [ppm] to [s]
 
-                            //Call the VTL engine loop: miguel: Should we wait until valid PVT solution?
                             //new_vtl_data.debug_print();
                             vtl_engine.vtl_loop(new_vtl_data);
-
+                            pvt_sol.rr[0]=new_vtl_data.kf_state(0);//rx_p
+                            pvt_sol.rr[1]=new_vtl_data.kf_state(1);//rx_p
+                            pvt_sol.rr[2]=new_vtl_data.kf_state(2);//rx_p
+                            pvt_sol.rr[3]=new_vtl_data.kf_state(3);//rx_v
+                            pvt_sol.rr[4]=new_vtl_data.kf_state(4);//rx_v
+                            pvt_sol.rr[5]=new_vtl_data.kf_state(5);//rx_v
                             //new_vtl_data.debug_print();
                         }
                     // compute Ground speed and COG

src/algorithms/PVT/libs/vtl_data.cc

@@ -45,6 +45,7 @@ void Vtl_Data::init_storage(int n_sats)
     rx_var = arma::vec(1);
     rx_pvt_var = arma::vec(8);
     kf_state = arma::vec(8);
+    kf_P = arma::mat(8,8);
     epoch_tow_s = 0;
     sample_counter = 0;
 }
@@ -60,7 +61,7 @@ void Vtl_Data::debug_print()
     //sat_LOS.print("VTL SAT LOS");
     // kf_state.print("EKF STATE");
     
-    pr_m.print("Satellite Code pseudoranges [m]");
+    //pr_m.print("Satellite Code pseudoranges [m]");
-    doppler_hz.print("satellite Carrier Dopplers [Hz]");
+    //doppler_hz.print("satellite Carrier Dopplers [Hz]");
     // carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
 }

src/algorithms/PVT/libs/vtl_data.h

@@ -55,6 +55,7 @@ public:
     arma::mat rx_dts;          // Receiver clock bias and drift [s,s/s]
     arma::colvec rx_var;       // Receiver position and clock error variance [m^2]
     arma::colvec kf_state;     // KF STATE
+    arma::mat kf_P;     // KF STATE
     // time handling
     double epoch_tow_s;       // current observation RX time [s]
     uint64_t sample_counter;  // current sample counter associated with RX time [samples from start]

src/algorithms/PVT/libs/vtl_engine.cc

@@ -31,6 +31,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
 {
     //TODO: Implement main VTL loop here
     using arma::as_scalar;
+
     // ################## Kalman filter initialization ######################################
     // covariances (static)
     kf_P_x  = arma::eye(8, 8); //TODO: use a real value.
@@ -57,6 +58,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
     kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix
     
     // ################## Kalman Tracking ######################################
+    static uint32_t  counter=0; //counter  
+    counter=counter+1; //uint64_t 
+    cout << "counter" << counter;
+
+    if(counter<500){ //
         // receiver solution from rtklib_solver
         kf_x(0)=new_data.rx_p(0);
         kf_x(1)=new_data.rx_p(1);
@@ -66,6 +72,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
         kf_x(5)=new_data.rx_v(2);
         kf_x(6)=new_data.rx_dts(0); 
         kf_x(7)=new_data.rx_dts(1);
+    }
+    else{
+        kf_x=new_data.kf_state;
+        kf_P_x=new_data.kf_P;
+    }
 
     for (int32_t i = 0; i < 8; i++) // State error Covariance Matrix Q (PVT)
     {
@@ -76,7 +87,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
     // Kalman state prediction (time update)
     // kf_x.print(" KF RTKlib STATE");
     new_data.kf_state=kf_x; 
-    //kf_x = kf_F * kf_x;                        // state prediction
+    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
@@ -160,16 +171,20 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
     kf_xerr = kf_K * (kf_yerr);                                 // Error state estimation
     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(6) =kf_x(6) /SPEED_OF_LIGHT_M_S;
+    // kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S;
-    kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S;
+    // kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S;
-    new_data.pr_res.print(" pr RESIDUALS");
+
-    //new_data.kf_state.print(" KF RTKlib STATE");
+    kf_x(6)=cdeltat_u/SPEED_OF_LIGHT_M_S;
-    //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state;
+    kf_x(7)=cdeltatDot_u/SPEED_OF_LIGHT_M_S; 
-    //cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100;
+
+    //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;
 
 //     // ################## Geometric Transformation ######################################