From 2d4c5b4d7bc4830fe21f2b73ea76ad925c5321d7 Mon Sep 17 00:00:00 2001
From: "M.A. Gomez" <gomezlma@inta.es>
Date: Tue, 14 Mar 2023 15:29:18 +0100
Subject: [PATCH] fix: vtl_engine bug

---
 src/algorithms/PVT/libs/vtl_engine.cc | 131 ++++++++++++--------------
 src/algorithms/PVT/libs/vtl_engine.h  |  11 ++-
 2 files changed, 64 insertions(+), 78 deletions(-)

diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc
index 46aeb28b2..66d20189d 100644
--- a/src/algorithms/PVT/libs/vtl_engine.cc
+++ b/src/algorithms/PVT/libs/vtl_engine.cc
@@ -25,8 +25,11 @@ Vtl_Engine::Vtl_Engine()
     counter = 0;
     refSampleCounter = 0;
     n_of_states = 11;
+    delta_t_cmd = 0;
+
     kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0;  //TODO: use a real value.;
     kf_x = arma::zeros(n_of_states, 1);
+ 
 }
 
 Vtl_Engine::~Vtl_Engine()
@@ -38,14 +41,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
     //TODO: Implement main VTL loop here
     using arma::as_scalar;
 
-    if (refSampleCounter = 0)
+    if (refSampleCounter == 0)
         {
             refSampleCounter = new_data.sample_counter;
         }
     double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0;
     refSampleCounter = new_data.sample_counter;
-    static double delta_t_cmd = 0;
+
     bool flag_cmd = false;
+    bool flag_time_cmd = false;
     delta_t_cmd = delta_t_cmd + delta_t_vtl;  // update timer for vtl trk command
     if (delta_t_cmd >= 0.3)
         {
@@ -63,7 +67,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
     kf_Q = arma::eye(n_of_states, n_of_states);
 
     kf_F = arma::eye(n_of_states, n_of_states);
-    bool test = kf_F_fill(kf_F, kf_dt);
+    kf_F_fill(kf_F, kf_dt);
 
     //kf_H = arma::zeros(3 * new_data.sat_number, n_of_states);
     kf_y = arma::zeros(3 * new_data.sat_number, 1);
@@ -74,6 +78,10 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
     ;
     // ################## Kalman Tracking ######################################
     counter++;  //uint64_t
+
+    if (counter>2500){
+       flag_time_cmd = true; 
+    }
     //new_data.kf_state.print("new_data kf initial");
     uint32_t closure_point = 3;
 
@@ -102,7 +110,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
             double acc_x = 0;
             double acc_y = 0;
             double acc_z = 0;
-            test = model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter);
+            // model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter);
             kf_x(6) = acc_x;
             kf_x(7) = acc_y;
             kf_x(8) = acc_z;
@@ -161,7 +169,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
     a_y = arma::zeros(new_data.sat_number, 1);
     a_z = arma::zeros(new_data.sat_number, 1);
     // cout<<"llegado aqui tambien"<<endl;
-    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);
+    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
         {
             new_data.sat_LOS(i, 0) = a_x(i);
@@ -170,14 +178,14 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
         }
 
     kf_H = arma::zeros(3 * 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_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt);
 
     for (int32_t i = 0; i < new_data.sat_number; i++)  //neccesary quantities
         {
             // rhoDot2_pri(i)=(rhoDot_pri(i)-rhoDot_pri_old(i))/kf_dt;
         }
     // Kalman estimation (measurement update)
-    test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri * 0, new_data.pr_m, new_data.doppler_hz, kf_x);
+    kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri * 0, 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
     // Kalman filter update step
@@ -193,7 +201,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
     kf_x = kf_x - kf_xerr;  // updated state estimation (a priori + error)
                             //    kf_x.print("state of kalman: ");
     //     // ################## Geometric Transformation ######################################
-    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);
+    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
         {
@@ -202,7 +210,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
             //rhoDot2_pri(chan,t)=-acc_effect(chan,t);
         }
 
-    test = kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt);
+    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):
@@ -224,24 +232,23 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
             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)) / Lambda_GPS_L1;
             trk_cmd.code_phase_chips = 0;  //kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3;
 
-            if (flag_cmd)
-                {
-                    trk_cmd.enable_carrier_nco_cmd = true;
-                }
+            if (flag_time_cmd)
+            {
+                if (flag_cmd)
+                    {
+                        trk_cmd.enable_carrier_nco_cmd = true;
+                    }
+                else
+                    {
+                        trk_cmd.enable_carrier_nco_cmd = false;  // do NOT apply corrections! loop convergence issue
+                    }
+            }
             else
-                {
-                    trk_cmd.enable_carrier_nco_cmd = false;  // do NOT apply corrections! loop convergence issue
-                }
+            {
+                trk_cmd.enable_carrier_nco_cmd = false;  // do NOT apply corrections! loop convergence issue
+            }
+            
 
-            if (counter < 1500)
-                {
-                    // std::cout<<"yet to soon"<<std::endl;
-                    trk_cmd.enable_code_nco_cmd = false;  // do NOT apply corrections! initial convergence issue
-                }
-            else
-                {
-                    trk_cmd.enable_code_nco_cmd = true;
-                }
             trk_cmd.sample_counter = new_data.sample_counter;
             trk_cmd.channel_id = channel;
             trk_cmd_outs.push_back(trk_cmd);
@@ -253,40 +260,26 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
             //                   << " \n";
             //     }
         }
-    // 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
-    //     {
+    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
+        {
 
-    //         if(new_data.sat_number>5){
-    //             // dump_vtl_file << "pr_m"
-    //             //             << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2)
-    //             //             << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)<<endl;
-    //             // dump_vtl_file << "prDot_m"
-    //             //             << "," << kf_yerr(new_data.sat_number)<< "," << kf_yerr(new_data.sat_number+1)<< "," << kf_yerr(new_data.sat_number+2)
-    //             //             << "," << kf_yerr(new_data.sat_number+3) << "," << kf_yerr(new_data.sat_number+4)<<  "," << kf_yerr(new_data.sat_number+5)<< endl;
-    //             // // dump_vtl_file << "K_column"
-    //             // //             << "," << kf_K.row(5)(6) << "," << kf_K.row(5)(7) << "," <<kf_K.row(5)(8)<< "," << kf_K.row(5)(9)
-    //             // //             << "," << kf_K.row(5)(10) << "," << kf_K.row(5)(11) << endl;
-    //             // // dump_vtl_file << "K_column2"
-    //             // //             << "," << kf_K.row(5)(0) << "," << kf_K.row(5)(1) << "," <<kf_K.row(5)(2)<< "," << kf_K.row(5)(3)
-    //             // //             << "," << kf_K.row(5)(4) << "," << kf_K.row(5)(5) << endl;
-    //         }
-    //         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)<< "," << 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) << "," << 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();
-    //     }
+            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)<< "," << 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) << "," << 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;
 }
 
@@ -306,7 +299,7 @@ void Vtl_Engine::configure(Vtl_Conf 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)
+void 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
         {
@@ -333,11 +326,9 @@ bool Vtl_Engine::kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arm
             kf_H(i + 2 * sat_number, 8) = az(i);
             kf_H(i + 2 * sat_number, 10) = kf_dt;
         }
-
-    return -1;
 }
 
-bool Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt)
+void 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;
@@ -351,11 +342,9 @@ bool Vtl_Engine::kf_F_fill(arma::mat &kf_F, double 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)
+void 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
         {
@@ -375,10 +364,9 @@ bool Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colv
             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_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x)
+void Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x)
 {
     for (int32_t i = 0; i < sat_number; i++)  // Measurement vector
         {
@@ -386,10 +374,9 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r
             kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1 + kf_x(10)) - rhoDot_pri(i);
             kf_yerr(i + 2 * sat_number) = -rhoDot2_pri(i);
         }
-    return -1;
 }
 
-bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter)
+void Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter)
 {
     arma::colvec u_vec;
     arma::colvec acc_vec;
@@ -411,7 +398,7 @@ bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma
             if (u > 6)
                 {
                     t_disparo = t_disparo + dt;
-                    std::cout << "u : " << u << endl;
+                    // std::cout << "u : " << u << endl;
                     double diam_cohete = 120.0e-3;  // 120 mm
                     double mass_rocket = 50.0;      //50Kg
 
@@ -438,7 +425,7 @@ bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma
 
                     // acc_vec.print("acc_vec");
                     // % return
-                    cout << "modelo 3Dof actuando,t:" << t_disparo << endl;
+                    // cout << "modelo 3Dof actuando,t:" << t_disparo << endl;
                     acc_x = acc_vec(0);
                     acc_y = acc_vec(1);
                     acc_z = acc_vec(2);
@@ -459,8 +446,6 @@ bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma
             acc_y = 0;
             acc_z = 0;
         }
-
-    return -1;
 }
 
 double Vtl_Engine::EmpujeLkTable(double t_disparo)
diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h
index 5317114a1..a7b081f3f 100644
--- a/src/algorithms/PVT/libs/vtl_engine.h
+++ b/src/algorithms/PVT/libs/vtl_engine.h
@@ -100,12 +100,13 @@ private:
     uint32_t counter;
     int n_of_states;
     uint64_t refSampleCounter;
+    double delta_t_cmd = 0;
 
-    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_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
-    bool model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);
+    void 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
+    void kf_F_fill(arma::mat &kf_F, double kf_dt);                                                                                                                                      // System Matrix constructor
+    void 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
+    void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
+    void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);
     double EmpujeLkTable(double t_disparo);
 };