From 8056b54045dceb949246e87e95d33b50a6ae8ee7 Mon Sep 17 00:00:00 2001
From: "M.A. Gomez" <gomezlma@inta.es>
Date: Sun, 12 Feb 2023 12:58:55 +0100
Subject: [PATCH] REFACTOR: vtl_engine

---
 src/algorithms/PVT/libs/vtl_engine.cc | 331 +++++++++++++-------------
 src/algorithms/PVT/libs/vtl_engine.h  |  21 +-
 2 files changed, 166 insertions(+), 186 deletions(-)

diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc
index ed05850c2..6875765e4 100755
--- a/src/algorithms/PVT/libs/vtl_engine.cc
+++ b/src/algorithms/PVT/libs/vtl_engine.cc
@@ -33,31 +33,28 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
     //TODO: Implement main VTL loop here
     using arma::as_scalar;
     
-    
+    static uint64_t refSampleCounter = new_data.sample_counter;
+    double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; 
+    refSampleCounter = new_data.sample_counter;
     // ################## Kalman filter initialization ######################################
+    //State variables
+    int n_of_states=11;
+    static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0;  //TODO: use a real value.; 
+    static arma::mat kf_x = arma::zeros(n_of_states, 1);
+    arma::mat kf_dx = arma::zeros(n_of_states, 1);
     // covariances (static)
-    kf_P_x = arma::eye(12, 12) * 1.0;  //TODO: use a real value.
-    kf_x = arma::zeros(12, 1);
+      
     kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number);
-    double kf_dt = 0.05;
-    kf_Q = arma::eye(12, 12);
+    double kf_dt = delta_t_vtl; //0.05;
+    kf_Q = arma::eye(n_of_states, n_of_states);
 
-    kf_F = arma::eye(12, 12);
-    kf_F(0, 3) = kf_dt;
-    kf_F(1, 4) = kf_dt;
-    kf_F(2, 5) = kf_dt;
+    kf_F = arma::eye(n_of_states, n_of_states);
+    bool test = kf_F_fill(kf_F,kf_dt);
 
-    kf_F(3, 6) = kf_dt;
-    kf_F(4, 7) = kf_dt;
-    kf_F(5, 8) = kf_dt;
-    
-    kf_F(9, 10) = kf_dt; 
-    kf_F(10, 11) = kf_dt; 
-
-    kf_H = arma::zeros(2 * new_data.sat_number, 12);
+    //kf_H = arma::zeros(2 * new_data.sat_number, n_of_states);
     kf_y = arma::zeros(2 * new_data.sat_number, 1);
     kf_yerr = arma::zeros(2 * new_data.sat_number, 1);
-    kf_xerr = arma::zeros(12, 1);
+    kf_xerr = arma::zeros(n_of_states, 1);
     kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number);  // kf_P_y innovation covariance matrix
 
     // ################## Kalman Tracking ######################################
@@ -68,36 +65,39 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
     
     if (counter < closure_point)
         {  
-            // receiver solution from rtklib_solver
+            // // receiver solution from rtklib_solver
+            kf_dx=kf_x;
             kf_x(0) = new_data.rx_p(0);
             kf_x(1) = new_data.rx_p(1);
             kf_x(2) = new_data.rx_p(2);
             kf_x(3) = new_data.rx_v(0);
             kf_x(4) = new_data.rx_v(1);
             kf_x(5) = new_data.rx_v(2);
-
+            kf_x(6) = 0;
+            kf_x(7) = 0;
+            kf_x(8) = 0;
             kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S;
             kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S;
-            kf_x(11) = 1.0;
 
-            kf_x = kf_F * kf_x;  // state prediction
+            kf_dx=kf_x-kf_dx;
+            kf_dx = kf_F * kf_dx;  // state prediction
         }
     else
         {
             // receiver solution from previous KF step
-            kf_x(0) = new_data.kf_state[0];
-            kf_x(1) = new_data.kf_state[1];
-            kf_x(2) = new_data.kf_state[2];
-            kf_x(3) = new_data.kf_state[3];
-            kf_x(4) = new_data.kf_state[4];
-            kf_x(5) = new_data.kf_state[5];
-            kf_x(6) = new_data.kf_state[6];
-            kf_x(7) = new_data.kf_state[7];
-            kf_x(8) = new_data.kf_state[8];
-            kf_x(9) = new_data.kf_state[9];
-            kf_x(10) = new_data.kf_state[10];
-            kf_x(11) = new_data.kf_state[11];
-            kf_P_x = new_data.kf_P;
+            // kf_x(0) = x_u;
+            // kf_x(1) = y_u;
+            // kf_x(2) = z_u;
+            // kf_x(3) = xDot_u;
+            // kf_x(4) = yDot_u;
+            // kf_x(5) = zDot_u;
+            // kf_x(6) = xDot2_u;
+            // kf_x(7) = yDot2_u;
+            // kf_x(8) = zDot2_u;
+            // kf_x(9) = cdeltat_u;
+            // kf_x(10) = cdeltatDot_u;
+
+            //kf_P_x = new_data.kf_P;
         }
    
     // State error Covariance Matrix Q (PVT)
@@ -111,41 +111,31 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
     kf_Q(6, 6) = .1;
     kf_Q(7, 7) = .1;
     kf_Q(8, 8) = .1;
-    kf_Q(9, 9) = 100.0;
-    kf_Q(10, 10) = 40.0;
-    kf_Q(11, 11) = 1.0;
+    kf_Q(9, 9) = 4.0;
+    kf_Q(10, 10) = 100.0;
  
     // 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) = 50.0*50.0/new_data.sat_CN0_dB_hz(i);  //TODO: fill with real values.
-            kf_R(i + new_data.sat_number, i + new_data.sat_number) = 5.0*50.0/new_data.sat_CN0_dB_hz(i);
+            kf_R(i, i) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i);  //TODO: fill with real values.
+            kf_R(i + new_data.sat_number, i + new_data.sat_number) = 1.0;//*50.0/new_data.sat_CN0_dB_hz(i);
             
-            if(50.0*50.0/new_data.sat_CN0_dB_hz(i)>70||5.0*50.0/new_data.sat_CN0_dB_hz(i)>7){
-                kf_R(i, i) = 10e4;
-                kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4;
-            }
+            // if(50.0*50.0/new_data.sat_CN0_dB_hz(i)>70||5.0*50.0/new_data.sat_CN0_dB_hz(i)>7){
+            //     kf_R(i, i) = 10e4;
+            //     kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4;
+            // }
         }
-
+    // arma::mat test = kf_P_x.diag();
+    // test.print("P diagonal");
+    //  test = kf_Q.diag();
+    // test.print("Q diagonal");
     // Kalman state prediction (time update)
     //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
     // TODO: cast to type properly
-    x_u = kf_x(0);
-    y_u = kf_x(1);
-    z_u = kf_x(2);
-    xDot_u = kf_x(3);
-    yDot_u = kf_x(4);
-    zDot_u = kf_x(5);
-    xDot2_u = kf_x(6);
-    yDot2_u = kf_x(7);
-    zDot2_u = kf_x(8);
-    cdeltat_u = kf_x(9);
-    cdeltatDot_u = kf_x(10);
 
     d = arma::zeros(new_data.sat_number, 1);
     rho_pri = arma::zeros(new_data.sat_number, 1);
@@ -158,119 +148,41 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
     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);
+
+    test = obsv_calc(rho_pri,rhoDot_pri,a_x, a_y, a_z,new_data.sat_number,new_data.sat_p,new_data.sat_v,kf_x);
     for (int32_t i = 0; i < new_data.sat_number; i++)  //neccesary quantities
         {
-            //d(i) is the distance sat(i) to receiver
-            d(i) = (new_data.sat_p(i, 0) - x_u) * (new_data.sat_p(i, 0) - x_u);
-            d(i) = d(i) + (new_data.sat_p(i, 1) - y_u) * (new_data.sat_p(i, 1) - y_u);
-            d(i) = d(i) + (new_data.sat_p(i, 2) - z_u) * (new_data.sat_p(i, 2) - z_u);
-            d(i) = sqrt(d(i));
-
-            //compute pseudorange estimation
-            rho_pri(i) = d(i) + cdeltat_u;
-            //compute LOS sat-receiver vector components
-            a_x(i) = -(new_data.sat_p(i, 0) - x_u) / d(i);
-            a_y(i) = -(new_data.sat_p(i, 1) - y_u) / d(i);
-            a_z(i) = -(new_data.sat_p(i, 2) - z_u) / d(i);
             new_data.sat_LOS(i, 0) = a_x(i);
             new_data.sat_LOS(i, 1) = a_y(i);
             new_data.sat_LOS(i, 2) = a_z(i);
-            //compute pseudorange rate estimation
-            rhoDot_pri(i) = (new_data.sat_v(i, 0) - xDot_u) * a_x(i) + (new_data.sat_v(i, 1) - yDot_u) * a_y(i) + (new_data.sat_v(i, 2) - zDot_u) * a_z(i);
-            rhoDot_pri(i) = rhoDot_pri(i) + a_x(i)*xDot2_u*kf_dt+a_y(i)*yDot2_u*kf_dt+a_z(i)*zDot2_u*kf_dt;;
         }
             // cout<<new_data.sat_number<<"sat_number";
 
-    kf_H = arma::zeros(2 * new_data.sat_number, 12);
-
-    for (int32_t i = 0; i < new_data.sat_number; i++)  // Measurement matrix H assembling
-        {
-            // It has 12 columns (12 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
-            kf_H(i, 0) = a_x(i);
-            kf_H(i, 1) = a_y(i);
-            kf_H(i, 2) = a_z(i);
-            kf_H(i, 9) = 1.0;
-            kf_H(i + new_data.sat_number, 3) = a_x(i);
-            kf_H(i + new_data.sat_number, 4) = a_y(i);
-            kf_H(i + new_data.sat_number, 5) = a_z(i);
-
-            kf_H(i + new_data.sat_number, 6) = a_x(i)*kf_dt;
-            kf_H(i + new_data.sat_number, 7) = a_y(i)*kf_dt;
-            kf_H(i + new_data.sat_number, 8) = a_z(i)*kf_dt;
-            kf_H(i + new_data.sat_number, 10) = 1.0;
-        }
+    kf_H = arma::zeros(2 * new_data.sat_number, n_of_states);
+    test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt);
+    // kf_H.print("kf_H diag:");
     // Kalman estimation (measurement update)
-    for (int32_t i = 0; i < new_data.sat_number; i++)  // Measurement vector
-        {
-            kf_yerr(i) = rho_pri(i) - new_data.pr_m(i);
-            if (counter < closure_point)
-            { 
-                kf_yerr(i + new_data.sat_number) = (new_data.doppler_hz(i) * Lambda_GPS_L1) - rhoDot_pri(i);
-            }else{
-                kf_yerr(i + new_data.sat_number) = (new_data.doppler_hz(i) * Lambda_GPS_L1+cdeltatDot_u) - rhoDot_pri(i);
-    
-            }
-        }
+    test = kf_NLmeasurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x);
 
+    kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q;  // state error covariance prediction
+    // kf_P_x.print("a priori P: ");
     // 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_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_P = kf_P_x;
+    kf_x = kf_x;// - kf_xerr;                                      // updated state estimation (a priori + error)
+    
+    if(abs(delta_t_vtl)>.5){
+        kf_xerr.print("kf_xERR: ");
+        cout<<"kf_dt: "<<delta_t_vtl<<endl;
+        kf_x.print("kf_x:");
+    }
+    // kf_x.print("state CORRECTED:");
 
     //     // ################## Geometric Transformation ######################################
-
-    //     // x_u=kf_x(0);
-    //     // y_u=kf_x(1);
-    //     // z_u=kf_x(2);
-    //     // xDot_u=kf_x(3);
-    //     // yDot_u=kf_x(4);
-    //     // zDot_u=kf_x(5);
-    //     // cdeltat_u=kf_x(6)*SPEED_OF_LIGHT_M_S;
-    //     // cdeltatDot_u=kf_x(7)*SPEED_OF_LIGHT_M_S;
-
-    for (int32_t i = 0; i < new_data.sat_number; i++)  //neccesary quantities
-        {
-            //d(i) is the distance sat(i) to receiver
-            d(i) = (new_data.sat_p(i, 0) - kf_x(0)) * (new_data.sat_p(i, 0) - kf_x(0));
-            d(i) = d(i) + (new_data.sat_p(i, 1) - kf_x(1)) * (new_data.sat_p(i, 1) - kf_x(1));
-            d(i) = d(i) + (new_data.sat_p(i, 2) - kf_x(2)) * (new_data.sat_p(i, 2) - kf_x(2));
-            d(i) = sqrt(d(i));
-
-            //compute pseudorange estimation
-            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);
-            rhoDot_pri(i) = rhoDot_pri(i) + a_x(i)*xDot2_u*kf_dt+a_y(i)*yDot2_u*kf_dt+a_z(i)*zDot2_u*kf_dt;
-
-        }
-
-    kf_H = arma::zeros(2 * new_data.sat_number, 12);
-
-    for (int32_t i = 0; i < new_data.sat_number; i++)  // Measurement matrix H assembling
-        {
-            // It has 12 columns (12 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
-            kf_H(i, 0) = a_x(i);
-            kf_H(i, 1) = a_y(i);
-            kf_H(i, 2) = a_z(i);
-            kf_H(i, 9) = 1.0;
-            kf_H(i + new_data.sat_number, 3) = a_x(i);
-            kf_H(i + new_data.sat_number, 4) = a_y(i);
-            kf_H(i + new_data.sat_number, 5) = a_z(i);
-
-            kf_H(i + new_data.sat_number, 6) = a_x(i)*kf_dt;
-            kf_H(i + new_data.sat_number, 7) = a_y(i)*kf_dt;
-            kf_H(i + new_data.sat_number, 8) = a_z(i)*kf_dt;
-            kf_H(i + new_data.sat_number, 10) = 1.0;
-
-        }
-
+    test = obsv_calc(rho_pri,rhoDot_pri,a_x, a_y, a_z,new_data.sat_number,new_data.sat_p,new_data.sat_v,kf_x);
+    test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt);
     //  Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x
     kf_yerr = kf_H * kf_xerr;
     //  Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
@@ -280,21 +192,14 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
     for (int32_t channel = 0; channel < new_data.sat_number; channel++)  // Measurement vector
         {
             rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel);                                                             // now filtered
-            rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1 + kf_x(7)) - kf_yerr(channel + new_data.sat_number);  // now filtered
-
-            if(counter < closure_point)
-            {
-            doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1;
-            }else{
-            doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1;  
-            }
-
+            rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1) - kf_yerr(channel + new_data.sat_number);  // now filtered
+            doppler_hz_filt(channel) = (rhoDot_pri_filt(channel)) / Lambda_GPS_L1; 
             //TODO: Fill the tracking commands outputs
             // Notice: keep the same satellite order as in the Vtl_Data matrices
             // sample code
             trk_cmd.carrier_phase_rads = 0;                                                // difficult of calculation
             trk_cmd.carrier_freq_hz = doppler_hz_filt(channel);  //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction
-            trk_cmd.carrier_freq_rate_hz_s = 0;
+            trk_cmd.carrier_freq_rate_hz_s = (a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8));
             trk_cmd.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3;
             trk_cmd.enable_carrier_nco_cmd = true;
             trk_cmd.enable_code_nco_cmd = true;
@@ -309,8 +214,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
             //     }
         }
 
-    new_data.kf_state = kf_x;  //updated state estimation
-
     fstream dump_vtl_file;
     dump_vtl_file.open("dump_vtl_file.csv", ios::out | ios::app);
     dump_vtl_file.precision(15);
@@ -320,14 +223,29 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
         }
     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;
+
+            if(new_data.sat_number>5){      
+                // dump_vtl_file << "pr_m"
+                //             << "," << new_data.pr_m(0) << "," << new_data.pr_m(1)  << "," << new_data.pr_m(2)  
+                //             << "," << new_data.pr_m(3)  << "," << new_data.pr_m(4)<<"," << new_data.pr_m(5)   << endl;
+                // dump_vtl_file << "prDot_m"
+                //             << "," << new_data.doppler_hz(0)<< "," << new_data.doppler_hz(1)<< "," << new_data.doppler_hz(2) 
+                //             << "," << new_data.doppler_hz(3) << "," << new_data.doppler_hz(4)<< "," << new_data.doppler_hz(5)<< endl;
+                // dump_vtl_file << "sat_position"
+                //             << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << "," << kf_xerr(5) 
+                //             << "," << kf_xerr(6) << "," << kf_xerr(7) << endl;
+                // dump_vtl_file << "sat_velocity"
+                //             << "," << 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)<< new_data.kf_state(8) << endl;
+                          << "," << kf_x(0) << "," << kf_x(1) << "," << kf_x(2) << "," << kf_x(3) << "," << kf_x(4) << "," << kf_x(5) << "," << kf_x(6) << "," << kf_x(7)<< "," << kf_x(8) <<"," << kf_x(9) <<"," << kf_x(10)<< endl;
+            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)<< "," << kf_xerr(8) <<"," << kf_xerr(9) <<"," << kf_xerr(10)<< 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;
+                          << "," << 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) << "," << delta_t_vtl  << 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();
         }
     return true;
@@ -348,3 +266,74 @@ void Vtl_Engine::configure(Vtl_Conf config_)
     config = config_;
     //TODO: initialize internal variables
 }
+
+bool Vtl_Engine::kf_H_fill(arma::mat &kf_H,int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt)
+{
+	    for (int32_t i = 0; i < sat_number; i++)  // Measurement matrix H assembling
+        {
+            // It has n_of_states columns (n_of_states states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
+            kf_H(i, 0) = ax(i);
+            kf_H(i, 1) = ay(i);
+            kf_H(i, 2) = az(i);
+            kf_H(i, 9) = 1.0;
+            kf_H(i + sat_number, 3) = ax(i);
+            kf_H(i + sat_number, 4) = ay(i);
+            kf_H(i + sat_number, 5) = az(i);
+
+            kf_H(i + sat_number, 6) = ax(i)*kf_dt;
+            kf_H(i + sat_number, 7) = ay(i)*kf_dt;
+            kf_H(i + sat_number, 8) = az(i)*kf_dt;
+            kf_H(i + sat_number, 10) = 1.0;
+
+        }
+
+    return -1;
+}
+
+bool Vtl_Engine::kf_F_fill(arma::mat &kf_F,double kf_dt)
+{
+    kf_F(0, 3) = kf_dt; kf_F(0, 6) = kf_dt*kf_dt/2;
+    kf_F(1, 4) = kf_dt; kf_F(1, 7) = kf_dt*kf_dt/2;
+    kf_F(2, 5) = kf_dt; kf_F(2, 8) = kf_dt*kf_dt/2;
+
+    kf_F(3, 6) = kf_dt;
+    kf_F(4, 7) = kf_dt;
+    kf_F(5, 8) = kf_dt;
+    
+    kf_F(9, 10) = kf_dt; 
+
+    return -1;
+}
+
+bool Vtl_Engine::obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x)
+{
+    for (int32_t i = 0; i < sat_number; i++)  //neccesary quantities
+    {
+        //d(i) is the distance sat(i) to receiver
+        d(i) = (sat_p(i, 0) - kf_x(0)) * (sat_p(i, 0) - kf_x(0));
+        d(i) = d(i) + (sat_p(i, 1) - kf_x(1)) * (sat_p(i, 1) - kf_x(1));
+        d(i) = d(i) + (sat_p(i, 2) - kf_x(2)) * (sat_p(i, 2) - kf_x(2));
+        d(i) = sqrt(d(i));
+
+        //compute pseudorange estimation
+        rho_pri(i) = d(i) + kf_x(9);
+        //compute LOS sat-receiver vector componentsx
+        ax(i) = -(sat_p(i, 0) - kf_x(0)) / d(i);
+        ay(i) = -(sat_p(i, 1) - kf_x(1)) / d(i);
+        az(i) = -(sat_p(i, 2) - kf_x(2)) / d(i);
+        //compute pseudorange rate estimation
+        rhoDot_pri(i) = (sat_v(i, 0) - kf_x(3)) * a_x(i) + (sat_v(i, 1) - kf_x(4)) * a_y(i) + (sat_v(i, 2) - kf_x(5)) * a_z(i);
+        //rhoDot_pri(i) = rhoDot_pri(i) + a_x(i)*xDot2_u*kf_dt+a_y(i)*yDot2_u*kf_dt+a_z(i)*zDot2_u*kf_dt;
+    }
+	return -1;
+}
+
+bool Vtl_Engine::kf_NLmeasurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x)
+{
+    for (int32_t i = 0; i < sat_number; i++)  // Measurement vector
+    {
+        kf_yerr(i) = rho_pri(i) - pr_m(i);
+        kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1+kf_x(10)) - rhoDot_pri(i);
+    }
+    return -1;
+}
\ No newline at end of file
diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h
index 86e1c8a0d..127144a68 100755
--- a/src/algorithms/PVT/libs/vtl_engine.h
+++ b/src/algorithms/PVT/libs/vtl_engine.h
@@ -52,19 +52,6 @@ private:
     Vtl_Conf config;
     //TODO: Internal VTL persistent variables here
    
-    //State variables
-    double x_u;
-    double y_u;
-    double z_u;
-    double xDot_u;
-    double yDot_u;
-    double zDot_u;
-    double xDot2_u;
-    double yDot2_u;
-    double zDot2_u;
-    double cdeltat_u;
-    double cdeltatDot_u;
-    
     // Transformation variables
     arma::colvec d;
     arma::colvec rho_pri;
@@ -78,7 +65,7 @@ private:
     
     // Kalman filter matrices
     arma::mat kf_P_x_ini;  // initial state error covariance matrix
-    arma::mat kf_P_x;      // state error covariance matrix
+    // arma::mat kf_P_x;      // state error covariance matrix
     arma::mat kf_P_x_pre;  // Predicted state error covariance matrix
     arma::mat kf_S;      // innovation covariance matrix
 
@@ -96,8 +83,12 @@ private:
 
     // Gaussian estimator
     arma::mat kf_R_est;  // measurement error covariance
-};
 
+    bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor
+    bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor
+    bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation
+    bool kf_NLmeasurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
+};
 
 /** \} */
 /** \} */