Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into doppler_rate

src/algorithms/PVT/adapters/rtklib_pvt.cc

@@ -504,7 +504,7 @@ bool RtklibPvt::save_assistance_to_XML()
     LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename_;
     std::map<int, Gps_Ephemeris> eph_map = pvt_->get_GPS_L1_ephemeris_map();
 
-    if (eph_map.size() > 0)
+    if (eph_map.empty() == false)
         {
             try
                 {

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc

@@ -412,7 +412,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
     // save GPS L2CM ephemeris to XML file
     std::string file_name = "eph_GPS_CNAV.xml";
 
-    if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0)
+    if (d_ls_pvt->gps_cnav_ephemeris_map.empty() == false)
         {
             try
                 {
@@ -435,7 +435,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
     // save GPS L1 CA ephemeris to XML file
     file_name = "eph_GPS_L1CA.xml";
 
-    if (d_ls_pvt->gps_ephemeris_map.size() > 0)
+    if (d_ls_pvt->gps_ephemeris_map.empty() == false)
         {
             try
                 {
@@ -458,7 +458,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
     // save Galileo E1 ephemeris to XML file
     file_name = "eph_Galileo_E1.xml";
 
-    if (d_ls_pvt->galileo_ephemeris_map.size() > 0)
+    if (d_ls_pvt->galileo_ephemeris_map.empty() == false)
         {
             try
                 {
@@ -481,7 +481,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
     // save GLONASS GNAV ephemeris to XML file
     file_name = "eph_GLONASS_GNAV.xml";
 
-    if (d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0)
+    if (d_ls_pvt->glonass_gnav_ephemeris_map.empty() == false)
         {
             try
                 {
@@ -573,28 +573,28 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
                                 }
                             try
                                 {
-                                    if (d_ls_pvt->gps_ephemeris_map.size() > 0)
+                                    if (d_ls_pvt->gps_ephemeris_map.empty() == false)
                                         {
                                             if (tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.end())
                                                 {
                                                     d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]);  // keep track of locking time
                                                 }
                                         }
-                                    if (d_ls_pvt->galileo_ephemeris_map.size() > 0)
+                                    if (d_ls_pvt->galileo_ephemeris_map.empty() == false)
                                         {
                                             if (tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.end())
                                                 {
                                                     d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]);  // keep track of locking time
                                                 }
                                         }
-                                    if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0)
+                                    if (d_ls_pvt->gps_cnav_ephemeris_map.empty() == false)
                                         {
                                             if (tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.end())
                                                 {
                                                     d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]);  // keep track of locking time
                                                 }
                                         }
-                                    if (d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0)
+                                    if (d_ls_pvt->glonass_gnav_ephemeris_map.empty() == false)
                                         {
                                             if (tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end())
                                                 {
@@ -616,7 +616,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
                 }
 
             // ############ 2 COMPUTE THE PVT ################################
-            if (gnss_observables_map.size() > 0)
+            if (gnss_observables_map.empty() == false)
                 {
                     double current_RX_time = gnss_observables_map.begin()->second.RX_time;
                     uint32_t current_RX_time_ms = static_cast<uint32_t>(current_RX_time * 1000.0);

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -496,7 +496,8 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
                             if (rtk_.ssat[i].vsat[0] == 1) used_sats++;
                         }
 
-                    double azel[used_sats * 2];
+                    std::vector<double> azel;
+                    azel.reserve(used_sats * 2);
                     unsigned int index_aux = 0;
                     for (unsigned int i = 0; i < MAXSAT; i++)
                         {
@@ -507,7 +508,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
                                     index_aux++;
                                 }
                         }
-                    if (index_aux > 0) dops(index_aux, azel, 0.0, dop_);
+                    if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
 
                     this->set_valid_position(true);
                     arma::vec rx_position_and_time(4);

src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc

@@ -280,7 +280,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
 
     d_gnss_synchro->Acq_delay_samples = 0.0;
     d_gnss_synchro->Acq_doppler_hz = 0.0;
-    d_gnss_synchro->Acq_samplestamp_samples = 0;
+    d_gnss_synchro->Acq_doppler_step = 0U;
+    d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
     d_mag = 0.0;
     d_input_power = 0.0;
     const double GALILEO_TWO_PI = 6.283185307179600;
@@ -328,7 +329,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_state(int state)
         {
             d_gnss_synchro->Acq_delay_samples = 0.0;
             d_gnss_synchro->Acq_doppler_hz = 0.0;
-            d_gnss_synchro->Acq_samplestamp_samples = 0;
+            d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+            d_gnss_synchro->Acq_doppler_step = 0U;
             d_well_count = 0;
             d_mag = 0.0;
             d_input_power = 0.0;
@@ -376,7 +378,8 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
                         //restart acquisition variables
                         d_gnss_synchro->Acq_delay_samples = 0.0;
                         d_gnss_synchro->Acq_doppler_hz = 0.0;
-                        d_gnss_synchro->Acq_samplestamp_samples = 0;
+                        d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+                        d_gnss_synchro->Acq_doppler_step = 0U;
                         d_well_count = 0;
                         d_mag = 0.0;
                         d_input_power = 0.0;
@@ -633,7 +636,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
                                         d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
                                         d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
                                         d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
-
+                                        d_gnss_synchro->Acq_doppler_step = d_doppler_step;
                                         // 5- Compute the test statistics and compare to the threshold
                                         d_test_statistics = d_mag / d_input_power;
                                     }

src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc

@@ -151,10 +151,10 @@ void galileo_pcps_8ms_acquisition_cc::init()
     d_gnss_synchro->Flag_valid_symbol_output = false;
     d_gnss_synchro->Flag_valid_pseudorange = false;
     d_gnss_synchro->Flag_valid_word = false;
-
+    d_gnss_synchro->Acq_doppler_step = 0U;
     d_gnss_synchro->Acq_delay_samples = 0.0;
     d_gnss_synchro->Acq_doppler_hz = 0.0;
-    d_gnss_synchro->Acq_samplestamp_samples = 0;
+    d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
     d_mag = 0.0;
     d_input_power = 0.0;
     const double GALILEO_TWO_PI = 6.283185307179600;
@@ -188,7 +188,8 @@ void galileo_pcps_8ms_acquisition_cc::set_state(int state)
         {
             d_gnss_synchro->Acq_delay_samples = 0.0;
             d_gnss_synchro->Acq_doppler_hz = 0.0;
-            d_gnss_synchro->Acq_samplestamp_samples = 0;
+            d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+            d_gnss_synchro->Acq_doppler_step = 0U;
             d_well_count = 0;
             d_mag = 0.0;
             d_input_power = 0.0;
@@ -219,7 +220,8 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
                         //restart acquisition variables
                         d_gnss_synchro->Acq_delay_samples = 0.0;
                         d_gnss_synchro->Acq_doppler_hz = 0.0;
-                        d_gnss_synchro->Acq_samplestamp_samples = 0;
+                        d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+                        d_gnss_synchro->Acq_doppler_step = 0U;
                         d_well_count = 0;
                         d_mag = 0.0;
                         d_input_power = 0.0;
@@ -328,6 +330,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
                                 d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
                                 d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
                                 d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
+                                d_gnss_synchro->Acq_doppler_step = d_doppler_step;
                             }
 
                         // Record results to file if required

src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc

@@ -261,7 +261,7 @@ void pcps_acquisition::init()
     d_gnss_synchro->Flag_valid_symbol_output = false;
     d_gnss_synchro->Flag_valid_pseudorange = false;
     d_gnss_synchro->Flag_valid_word = false;
-
+    d_gnss_synchro->Acq_doppler_step = 0U;
     d_gnss_synchro->Acq_delay_samples = 0.0;
     d_gnss_synchro->Acq_doppler_hz = 0.0;
     d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
@@ -334,6 +334,7 @@ void pcps_acquisition::set_state(int32_t state)
             d_gnss_synchro->Acq_delay_samples = 0.0;
             d_gnss_synchro->Acq_doppler_hz = 0.0;
             d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+            d_gnss_synchro->Acq_doppler_step = 0U;
             d_mag = 0.0;
             d_input_power = 0.0;
             d_test_statistics = 0.0;
@@ -725,6 +726,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
             d_gnss_synchro->Acq_delay_samples = static_cast<double>(std::fmod(static_cast<float>(indext), acq_parameters.samples_per_code));
             d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
             d_gnss_synchro->Acq_samplestamp_samples = samp_count;
+            d_gnss_synchro->Acq_doppler_step = acq_parameters.doppler_step2;
         }
 
     lk.lock();
@@ -865,6 +867,7 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)),
                 d_gnss_synchro->Acq_delay_samples = 0.0;
                 d_gnss_synchro->Acq_doppler_hz = 0.0;
                 d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+                d_gnss_synchro->Acq_doppler_step = 0U;
                 d_mag = 0.0;
                 d_input_power = 0.0;
                 d_test_statistics = 0.0;

src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc

@@ -180,10 +180,10 @@ void pcps_acquisition_fine_doppler_cc::init()
     d_gnss_synchro->Flag_valid_symbol_output = false;
     d_gnss_synchro->Flag_valid_pseudorange = false;
     d_gnss_synchro->Flag_valid_word = false;
-
+    d_gnss_synchro->Acq_doppler_step = 0U;
     d_gnss_synchro->Acq_delay_samples = 0.0;
     d_gnss_synchro->Acq_doppler_hz = 0.0;
-    d_gnss_synchro->Acq_samplestamp_samples = 0;
+    d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
     d_state = 0;
 }
 
@@ -295,6 +295,7 @@ double pcps_acquisition_fine_doppler_cc::compute_CAF()
     d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
     d_gnss_synchro->Acq_doppler_hz = static_cast<double>(index_doppler * d_doppler_step - d_config_doppler_max);
     d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
+    d_gnss_synchro->Acq_doppler_step = d_doppler_step;
 
     return d_test_statistics;
 }
@@ -461,7 +462,8 @@ void pcps_acquisition_fine_doppler_cc::set_state(int state)
         {
             d_gnss_synchro->Acq_delay_samples = 0.0;
             d_gnss_synchro->Acq_doppler_hz = 0.0;
-            d_gnss_synchro->Acq_samplestamp_samples = 0;
+            d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+            d_gnss_synchro->Acq_doppler_step = 0U;
             d_well_count = 0;
             d_test_statistics = 0.0;
             d_active = true;

src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc

@@ -150,10 +150,10 @@ void pcps_assisted_acquisition_cc::init()
     d_gnss_synchro->Flag_valid_symbol_output = false;
     d_gnss_synchro->Flag_valid_pseudorange = false;
     d_gnss_synchro->Flag_valid_word = false;
-
+    d_gnss_synchro->Acq_doppler_step = 0U;
     d_gnss_synchro->Acq_delay_samples = 0.0;
     d_gnss_synchro->Acq_doppler_hz = 0.0;
-    d_gnss_synchro->Acq_samplestamp_samples = 0;
+    d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
     d_input_power = 0.0;
     d_state = 0;
 
@@ -279,6 +279,7 @@ double pcps_assisted_acquisition_cc::search_maximum()
     d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
     d_gnss_synchro->Acq_doppler_hz = static_cast<double>(index_doppler * d_doppler_step + d_doppler_min);
     d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
+    d_gnss_synchro->Acq_doppler_step = d_doppler_step;
 
     // Record results to file if required
     if (d_dump)

src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc

@@ -165,10 +165,10 @@ void pcps_cccwsr_acquisition_cc::init()
     d_gnss_synchro->Flag_valid_symbol_output = false;
     d_gnss_synchro->Flag_valid_pseudorange = false;
     d_gnss_synchro->Flag_valid_word = false;
-
+    d_gnss_synchro->Acq_doppler_step = 0U;
     d_gnss_synchro->Acq_delay_samples = 0.0;
     d_gnss_synchro->Acq_doppler_hz = 0.0;
-    d_gnss_synchro->Acq_samplestamp_samples = 0;
+    d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
     d_mag = 0.0;
     d_input_power = 0.0;
 
@@ -203,7 +203,8 @@ void pcps_cccwsr_acquisition_cc::set_state(int state)
         {
             d_gnss_synchro->Acq_delay_samples = 0.0;
             d_gnss_synchro->Acq_doppler_hz = 0.0;
-            d_gnss_synchro->Acq_samplestamp_samples = 0;
+            d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+            d_gnss_synchro->Acq_doppler_step = 0U;
             d_well_count = 0;
             d_mag = 0.0;
             d_input_power = 0.0;
@@ -234,7 +235,8 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
                         //restart acquisition variables
                         d_gnss_synchro->Acq_delay_samples = 0.0;
                         d_gnss_synchro->Acq_doppler_hz = 0.0;
-                        d_gnss_synchro->Acq_samplestamp_samples = 0;
+                        d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+                        d_gnss_synchro->Acq_doppler_step = 0U;
                         d_well_count = 0;
                         d_mag = 0.0;
                         d_input_power = 0.0;
@@ -354,6 +356,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
                                 d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
                                 d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
                                 d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
+                                d_gnss_synchro->Acq_doppler_step = d_doppler_step;
                             }
 
                         // Record results to file if required

src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc

@@ -290,10 +290,10 @@ void pcps_opencl_acquisition_cc::init()
     d_gnss_synchro->Flag_valid_symbol_output = false;
     d_gnss_synchro->Flag_valid_pseudorange = false;
     d_gnss_synchro->Flag_valid_word = false;
-
+    d_gnss_synchro->Acq_doppler_step = 0U;
     d_gnss_synchro->Acq_delay_samples = 0.0;
     d_gnss_synchro->Acq_doppler_hz = 0.0;
-    d_gnss_synchro->Acq_samplestamp_samples = 0;
+    d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
     d_mag = 0.0;
     d_input_power = 0.0;
 
@@ -450,6 +450,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
                             d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
                             d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
                             d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
+                            d_gnss_synchro->Acq_doppler_step = d_doppler_step;
 
                             // 5- Compute the test statistics and compare to the threshold
                             //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
@@ -613,6 +614,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
                             d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
                             d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
                             d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
+                            d_gnss_synchro->Acq_doppler_step = d_doppler_step;
 
                             // 5- Compute the test statistics and compare to the threshold
                             //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
@@ -676,7 +678,8 @@ void pcps_opencl_acquisition_cc::set_state(int state)
         {
             d_gnss_synchro->Acq_delay_samples = 0.0;
             d_gnss_synchro->Acq_doppler_hz = 0.0;
-            d_gnss_synchro->Acq_samplestamp_samples = 0;
+            d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+            d_gnss_synchro->Acq_doppler_step = 0U;
             d_well_count = 0;
             d_mag = 0.0;
             d_input_power = 0.0;
@@ -708,7 +711,8 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
                         //restart acquisition variables
                         d_gnss_synchro->Acq_delay_samples = 0.0;
                         d_gnss_synchro->Acq_doppler_hz = 0.0;
-                        d_gnss_synchro->Acq_samplestamp_samples = 0;
+                        d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+                        d_gnss_synchro->Acq_doppler_step = 0U;
                         d_well_count = 0;
                         d_mag = 0.0;
                         d_input_power = 0.0;

src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc

@@ -199,7 +199,8 @@ void pcps_quicksync_acquisition_cc::init()
     //DLOG(INFO) << "START init";
     d_gnss_synchro->Acq_delay_samples = 0.0;
     d_gnss_synchro->Acq_doppler_hz = 0.0;
-    d_gnss_synchro->Acq_samplestamp_samples = 0;
+    d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+    d_gnss_synchro->Acq_doppler_step = 0U;
     d_mag = 0.0;
     d_input_power = 0.0;
 
@@ -236,7 +237,8 @@ void pcps_quicksync_acquisition_cc::set_state(int state)
         {
             d_gnss_synchro->Acq_delay_samples = 0.0;
             d_gnss_synchro->Acq_doppler_hz = 0.0;
-            d_gnss_synchro->Acq_samplestamp_samples = 0;
+            d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+            d_gnss_synchro->Acq_doppler_step = 0U;
             d_well_count = 0;
             d_mag = 0.0;
             d_input_power = 0.0;
@@ -279,7 +281,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
                         //restart acquisition variables
                         d_gnss_synchro->Acq_delay_samples = 0.0;
                         d_gnss_synchro->Acq_doppler_hz = 0.0;
-                        d_gnss_synchro->Acq_samplestamp_samples = 0;
+                        d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+                        d_gnss_synchro->Acq_doppler_step = 0U;
                         d_well_count = 0;
                         d_mag = 0.0;
                         d_input_power = 0.0;
@@ -456,6 +459,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
                                         d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]);
                                         d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
                                         d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
+                                        d_gnss_synchro->Acq_doppler_step = d_doppler_step;
 
                                         /* 5- Compute the test statistics and compare to the threshold d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/
                                         d_test_statistics = d_mag / d_input_power;

src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc

@@ -166,10 +166,10 @@ void pcps_tong_acquisition_cc::init()
     d_gnss_synchro->Flag_valid_symbol_output = false;
     d_gnss_synchro->Flag_valid_pseudorange = false;
     d_gnss_synchro->Flag_valid_word = false;
-
+    d_gnss_synchro->Acq_doppler_step = 0U;
     d_gnss_synchro->Acq_delay_samples = 0.0;
     d_gnss_synchro->Acq_doppler_hz = 0.0;
-    d_gnss_synchro->Acq_samplestamp_samples = 0;
+    d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
     d_mag = 0.0;
     d_input_power = 0.0;
 
@@ -211,7 +211,8 @@ void pcps_tong_acquisition_cc::set_state(int state)
         {
             d_gnss_synchro->Acq_delay_samples = 0.0;
             d_gnss_synchro->Acq_doppler_hz = 0.0;
-            d_gnss_synchro->Acq_samplestamp_samples = 0;
+            d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+            d_gnss_synchro->Acq_doppler_step = 0U;
             d_dwell_count = 0;
             d_tong_count = d_tong_init_val;
             d_mag = 0.0;
@@ -250,7 +251,8 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
                         //restart acquisition variables
                         d_gnss_synchro->Acq_delay_samples = 0.0;
                         d_gnss_synchro->Acq_doppler_hz = 0.0;
-                        d_gnss_synchro->Acq_samplestamp_samples = 0;
+                        d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
+                        d_gnss_synchro->Acq_doppler_step = 0U;
                         d_dwell_count = 0;
                         d_tong_count = d_tong_init_val;
                         d_mag = 0.0;
@@ -345,6 +347,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
                                 d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
                                 d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
                                 d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
+                                d_gnss_synchro->Acq_doppler_step = d_doppler_step;
                             }
 
                         // Record results to file if required

src/algorithms/libs/galileo_e1_signal_processing.cc

@@ -33,6 +33,7 @@
 #include "galileo_e1_signal_processing.h"
 #include "Galileo_E1.h"
 #include "gnss_signal_processing.h"
+#include <volk_gnsssdr/volk_gnsssdr.h>
 #include <string>
 
 
@@ -42,7 +43,7 @@ void galileo_e1_code_gen_int(int* _dest, char _Signal[3], int32_t _prn)
     int32_t prn = _prn - 1;
     int32_t index = 0;
 
-    /* A simple error check */
+    // A simple error check
     if ((_prn < 1) || (_prn > 50))
         {
             return;
@@ -107,7 +108,7 @@ void galileo_e1_sinboc_61_gen_int(int* _dest, int* _prn, uint32_t _length_out)
 void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], uint32_t _prn)
 {
     std::string _galileo_signal = _Signal;
-    uint32_t _codeLength = static_cast<uint32_t>(Galileo_E1_B_CODE_LENGTH_CHIPS);
+    const uint32_t _codeLength = static_cast<const uint32_t>(Galileo_E1_B_CODE_LENGTH_CHIPS);
     int32_t primary_code_E1_chips[4092];                            // _codeLength not accepted by Clang
     galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn);  //generate Galileo E1 code, 1 sample per chip
     for (uint32_t i = 0; i < _codeLength; i++)
@@ -157,15 +158,16 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
     // This function is based on the GNU software GPS for MATLAB in Kay Borre's book
     std::string _galileo_signal = _Signal;
     uint32_t _samplesPerCode;
-    const int32_t _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ;  //Hz
-    uint32_t _codeLength = Galileo_E1_B_CODE_LENGTH_CHIPS;
-    int32_t primary_code_E1_chips[static_cast<int32_t>(Galileo_E1_B_CODE_LENGTH_CHIPS)];
+    const int32_t _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ;  // Hz
+    uint32_t _codeLength = static_cast<uint32_t>(Galileo_E1_B_CODE_LENGTH_CHIPS);
+    int32_t* primary_code_E1_chips = static_cast<int32_t*>(volk_gnsssdr_malloc(static_cast<uint32_t>(Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
+
     _samplesPerCode = static_cast<uint32_t>(static_cast<double>(_fs) / (static_cast<double>(_codeFreqBasis) / static_cast<double>(_codeLength)));
     const int32_t _samplesPerChip = (_cboc == true) ? 12 : 2;
 
     const uint32_t delay = ((static_cast<int32_t>(Galileo_E1_B_CODE_LENGTH_CHIPS) - _chip_shift) % static_cast<int32_t>(Galileo_E1_B_CODE_LENGTH_CHIPS)) * _samplesPerCode / Galileo_E1_B_CODE_LENGTH_CHIPS;
 
-    galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn);  //generate Galileo E1 code, 1 sample per chip
+    galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn);  // generate Galileo E1 code, 1 sample per chip
 
     float* _signal_E1;
 
@@ -174,24 +176,26 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
 
     if (_cboc == true)
         {
-            galileo_e1_gen_float(_signal_E1, primary_code_E1_chips, _Signal);  //generate cboc 12 samples per chip
+            galileo_e1_gen_float(_signal_E1, primary_code_E1_chips, _Signal);  // generate cboc 12 samples per chip
         }
     else
         {
-            int32_t _signal_E1_int[_codeLength];
-            galileo_e1_sinboc_11_gen_int(_signal_E1_int, primary_code_E1_chips, _codeLength);  //generate sinboc(1,1) 2 samples per chip
+            int32_t* _signal_E1_int = static_cast<int32_t*>(volk_gnsssdr_malloc(_codeLength * sizeof(int32_t), volk_gnsssdr_get_alignment()));
+            galileo_e1_sinboc_11_gen_int(_signal_E1_int, primary_code_E1_chips, _codeLength);  // generate sinboc(1,1) 2 samples per chip
 
             for (uint32_t ii = 0; ii < _codeLength; ++ii)
                 {
                     _signal_E1[ii] = static_cast<float>(_signal_E1_int[ii]);
                 }
+            volk_gnsssdr_free(_signal_E1_int);
         }
 
     if (_fs != _samplesPerChip * _codeFreqBasis)
         {
             float* _resampled_signal = new float[_samplesPerCode];
+
             resampler(_signal_E1, _resampled_signal, _samplesPerChip * _codeFreqBasis, _fs,
-                _codeLength, _samplesPerCode);  //resamples code to fs
+                _codeLength, _samplesPerCode);  // resamples code to fs
 
             delete[] _signal_E1;
             _signal_E1 = _resampled_signal;
@@ -221,6 +225,7 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
         }
 
     delete[] _signal_E1;
+    volk_gnsssdr_free(primary_code_E1_chips);
 }
 
 
@@ -229,7 +234,7 @@ void galileo_e1_code_gen_complex_sampled(std::complex<float>* _dest, char _Signa
     bool _secondary_flag)
 {
     std::string _galileo_signal = _Signal;
-    const int32_t _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ;  //Hz
+    const int32_t _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ;  // Hz
     uint32_t _samplesPerCode = static_cast<uint32_t>(static_cast<double>(_fs) /
                                                      (static_cast<double>(_codeFreqBasis) / static_cast<double>(Galileo_E1_B_CODE_LENGTH_CHIPS)));
 
@@ -238,7 +243,7 @@ void galileo_e1_code_gen_complex_sampled(std::complex<float>* _dest, char _Signa
             _samplesPerCode *= static_cast<int32_t>(Galileo_E1_C_SECONDARY_CODE_LENGTH);
         }
 
-    float real_code[_samplesPerCode];
+    float* real_code = static_cast<float*>(volk_gnsssdr_malloc(_samplesPerCode * sizeof(float), volk_gnsssdr_get_alignment()));
 
     galileo_e1_code_gen_float_sampled(real_code, _Signal, _cboc, _prn, _fs, _chip_shift, _secondary_flag);
 
@@ -246,6 +251,7 @@ void galileo_e1_code_gen_complex_sampled(std::complex<float>* _dest, char _Signa
         {
             _dest[ii] = std::complex<float>(real_code[ii], 0.0f);
         }
+    volk_gnsssdr_free(real_code);
 }
 
 

src/algorithms/libs/gnss_signal_processing.cc

@@ -158,7 +158,7 @@ void hex_to_binary_converter(int32_t* _dest, char _from)
 }
 
 
-void resampler(float* _from, float* _dest, float _fs_in,
+void resampler(const float* _from, float* _dest, float _fs_in,
     float _fs_out, uint32_t _length_in, uint32_t _length_out)
 {
     uint32_t _codeValueIndex;
@@ -182,7 +182,7 @@ void resampler(float* _from, float* _dest, float _fs_in,
 }
 
 
-void resampler(std::complex<float>* _from, std::complex<float>* _dest, float _fs_in,
+void resampler(const std::complex<float>* _from, std::complex<float>* _dest, float _fs_in,
     float _fs_out, uint32_t _length_in, uint32_t _length_out)
 {
     uint32_t _codeValueIndex;

src/algorithms/libs/gnss_signal_processing.h

@@ -65,14 +65,14 @@ void hex_to_binary_converter(int32_t* _dest, char _from);
  * \brief This function resamples a sequence of float values.
  *
  */
-void resampler(float* _from, float* _dest,
+void resampler(const float* _from, float* _dest,
     float _fs_in, float _fs_out, uint32_t _length_in,
     uint32_t _length_out);
 /*!
  * \brief This function resamples a sequence of complex values.
  *
  */
-void resampler(std::complex<float>* _from, std::complex<float>* _dest,
+void resampler(const std::complex<float>* _from, std::complex<float>* _dest,
     float _fs_in, float _fs_out, uint32_t _length_in,
     uint32_t _length_out);
 

src/algorithms/libs/gps_sdr_signal_processing.cc

@@ -116,7 +116,7 @@ void gps_l1_ca_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift)
 
 void gps_l1_ca_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift)
 {
-    uint32_t _code_length = 1023;
+    const uint32_t _code_length = 1023;
     int32_t ca_code_int[_code_length];
 
     gps_l1_ca_code_gen_int(ca_code_int, _prn, _chip_shift);
@@ -130,7 +130,7 @@ void gps_l1_ca_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift)
 
 void gps_l1_ca_code_gen_complex(std::complex<float>* _dest, int32_t _prn, uint32_t _chip_shift)
 {
-    uint32_t _code_length = 1023;
+    const uint32_t _code_length = 1023;
     int32_t ca_code_int[_code_length];
 
     gps_l1_ca_code_gen_int(ca_code_int, _prn, _chip_shift);

src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc

@@ -163,8 +163,8 @@ galileo_e1b_telemetry_decoder_cc::~galileo_e1b_telemetry_decoder_cc()
 
 void galileo_e1b_telemetry_decoder_cc::decode_word(double *page_part_symbols, int32_t frame_length)
 {
-    double page_part_symbols_deint[frame_length];
     // 1. De-interleave
+    double *page_part_symbols_deint = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
     deinterleaver(GALILEO_INAV_INTERLEAVER_ROWS, GALILEO_INAV_INTERLEAVER_COLS, page_part_symbols, page_part_symbols_deint);
 
     // 2. Viterbi decoder
@@ -178,8 +178,9 @@ void galileo_e1b_telemetry_decoder_cc::decode_word(double *page_part_symbols, in
                 }
         }
 
-    int32_t page_part_bits[frame_length / 2];
+    int32_t *page_part_bits = static_cast<int32_t *>(volk_gnsssdr_malloc((frame_length / 2) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
     viterbi_decoder(page_part_symbols_deint, page_part_bits);
+    volk_gnsssdr_free(page_part_symbols_deint);
 
     // 3. Call the Galileo page decoder
     std::string page_String;
@@ -217,6 +218,7 @@ void galileo_e1b_telemetry_decoder_cc::decode_word(double *page_part_symbols, in
             d_nav.split_page(page_String.c_str(), flag_even_word_arrived);
             flag_even_word_arrived = 1;
         }
+    volk_gnsssdr_free(page_part_bits);
 
     // 4. Push the new navigation data to the queues
     if (d_nav.have_new_ephemeris() == true)
@@ -312,7 +314,7 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
     consume_each(1);
 
     d_flag_preamble = false;
-    uint32_t required_symbols = GALILEO_INAV_PAGE_SYMBOLS + d_symbols_per_preamble;
+    uint32_t required_symbols = static_cast<uint32_t>(GALILEO_INAV_PAGE_SYMBOLS) + static_cast<uint32_t>(d_symbols_per_preamble);
 
     if (d_symbol_history.size() > required_symbols)
         {
@@ -370,7 +372,7 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
                     // NEW Galileo page part is received
                     // 0. fetch the symbols into an array
                     int32_t frame_length = GALILEO_INAV_PAGE_PART_SYMBOLS - d_symbols_per_preamble;
-                    double page_part_symbols[frame_length];
+                    double *page_part_symbols = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
 
                     for (int32_t i = 0; i < frame_length; i++)
                         {
@@ -412,6 +414,7 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
                                     d_nav.flag_TOW_set = false;
                                 }
                         }
+                    volk_gnsssdr_free(page_part_symbols);
                 }
         }
 
@@ -424,7 +427,7 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
                 {
                     // TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
                     d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.TOW_5 * 1000.0);
-                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS;
+                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS);
                     d_nav.flag_TOW_5 = false;
                 }
 
@@ -432,20 +435,20 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
                 {
                     // TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
                     d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.TOW_6 * 1000.0);
-                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS;
+                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS);
                     d_nav.flag_TOW_6 = false;
                 }
             else
                 {
                     // this page has no timing information
-                    d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS;  // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD;
+                    d_TOW_at_current_symbol_ms += static_cast<uint32_t>(GALILEO_E1_CODE_PERIOD_MS);  // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD;
                 }
         }
     else  // if there is not a new preamble, we define the TOW of the current symbol
         {
             if (d_nav.flag_TOW_set == true)
                 {
-                    d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS;
+                    d_TOW_at_current_symbol_ms += static_cast<uint32_t>(GALILEO_E1_CODE_PERIOD_MS);
                 }
         }
 

src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc

@@ -79,8 +79,8 @@ void galileo_e5a_telemetry_decoder_cc::deinterleaver(int32_t rows, int32_t cols,
 
 void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols, int32_t frame_length)
 {
-    double page_symbols_deint[frame_length];
     // 1. De-interleave
+    double *page_symbols_deint = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
     deinterleaver(GALILEO_FNAV_INTERLEAVER_ROWS, GALILEO_FNAV_INTERLEAVER_COLS, page_symbols, page_symbols_deint);
 
     // 2. Viterbi decoder
@@ -93,8 +93,9 @@ void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols, int32_t
                     page_symbols_deint[i] = -page_symbols_deint[i];
                 }
         }
-    int32_t page_bits[frame_length / 2];
+    int32_t *page_bits = static_cast<int32_t *>(volk_gnsssdr_malloc((frame_length / 2) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
     viterbi_decoder(page_symbols_deint, page_bits);
+    volk_gnsssdr_free(page_symbols_deint);
 
     // 3. Call the Galileo page decoder
     std::string page_String;
@@ -109,6 +110,7 @@ void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols, int32_t
                     page_String.push_back('0');
                 }
         }
+    volk_gnsssdr_free(page_bits);
 
     // DECODE COMPLETE WORD (even + odd) and TEST CRC
     d_nav.split_page(page_String);
@@ -436,37 +438,37 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
             if (d_nav.flag_TOW_1 == true)
                 {
                     d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.FNAV_TOW_1 * 1000.0);
-                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
+                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
                     d_nav.flag_TOW_1 = false;
                 }
             else if (d_nav.flag_TOW_2 == true)
                 {
                     d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.FNAV_TOW_2 * 1000.0);
-                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
+                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
                     d_nav.flag_TOW_2 = false;
                 }
             else if (d_nav.flag_TOW_3 == true)
                 {
                     d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.FNAV_TOW_3 * 1000.0);
-                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
+                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
                     d_nav.flag_TOW_3 = false;
                 }
             else if (d_nav.flag_TOW_4 == true)
                 {
                     d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.FNAV_TOW_4 * 1000.0);
-                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
+                    d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
                     d_nav.flag_TOW_4 = false;
                 }
             else
                 {
-                    d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS;
+                    d_TOW_at_current_symbol_ms += static_cast<uint32_t>(GALILEO_E5a_CODE_PERIOD_MS);
                 }
         }
     else  // if there is not a new preamble, we define the TOW of the current symbol
         {
             if (d_nav.flag_TOW_set == true)
                 {
-                    d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS;
+                    d_TOW_at_current_symbol_ms += static_cast<uint32_t>(GALILEO_E5a_CODE_PERIOD_MS);
                 }
         }
 

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc

@@ -82,7 +82,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
                     n++;
                 }
         }
-    d_stat = 0;
+    d_stat = 0U;
     d_flag_frame_sync = false;
     d_prev_GPS_frame_4bytes = 0;
     d_TOW_at_Preamble_ms = 0;
@@ -93,8 +93,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
     flag_PLL_180_deg_phase_locked = false;
     d_preamble_time_samples = 0ULL;
     d_TOW_at_current_symbol_ms = 0;
-    d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS);  // Change fixed buffer size
-    d_symbol_history.clear();                                 // Clear all the elements in the buffer
+    d_symbol_history.set_capacity(GPS_CA_PREAMBLE_LENGTH_SYMBOLS);
     d_crc_error_synchronization_counter = 0;
     d_current_subframe_symbol = 0;
 }
@@ -103,6 +102,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
 gps_l1_ca_telemetry_decoder_cc::~gps_l1_ca_telemetry_decoder_cc()
 {
     volk_gnsssdr_free(d_preambles_symbols);
+    d_symbol_history.clear();
     if (d_dump_file.is_open() == true)
         {
             try
@@ -321,7 +321,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
     // record the oldest subframe symbol before inserting a new symbol into the circular buffer
     if (d_current_subframe_symbol < GPS_SUBFRAME_MS and d_symbol_history.size() > 0)
         {
-            d_subframe_symbols[d_current_subframe_symbol] = d_symbol_history.at(0).Prompt_I;
+            d_subframe_symbols[d_current_subframe_symbol] = d_symbol_history[0].Prompt_I;
             d_current_subframe_symbol++;
         }
 
@@ -337,9 +337,9 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
             // std::cout << "-------\n";
             for (uint32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++)
                 {
-                    if (d_symbol_history.at(i).Flag_valid_symbol_output == true)
+                    if (d_symbol_history[i].Flag_valid_symbol_output == true)
                         {
-                            if (d_symbol_history.at(i).Prompt_I < 0)  // symbols clipping
+                            if (d_symbol_history[i].Prompt_I < 0)  // symbols clipping
                                 {
                                     corr_value -= d_preambles_symbols[i];
                                 }
@@ -358,18 +358,18 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
             if (d_stat == 0)
                 {
                     // record the preamble sample stamp
-                    d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter;  // record the preamble sample stamp
-                    DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history.at(0).Tracking_sample_counter=" << d_symbol_history.at(0).Tracking_sample_counter;
+                    d_preamble_time_samples = d_symbol_history[0].Tracking_sample_counter;  // record the preamble sample stamp
+                    DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history[0].Tracking_sample_counter=" << d_symbol_history[0].Tracking_sample_counter;
                     d_stat = 1;  // enter into frame pre-detection status
                 }
             else if (d_stat == 1)  // check 6 seconds of preamble separation
                 {
-                    preamble_diff_ms = std::round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
+                    preamble_diff_ms = std::round(((static_cast<double>(d_symbol_history[0].Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history[0].fs)) * 1000.0);
                     if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) % GPS_SUBFRAME_MS == 0)
                         {
                             DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite;
                             d_flag_preamble = true;
-                            d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter;  // record the PRN start sample index associated to the preamble
+                            d_preamble_time_samples = d_symbol_history[0].Tracking_sample_counter;  // record the PRN start sample index associated to the preamble
                             if (!d_flag_frame_sync)
                                 {
                                     d_flag_frame_sync = true;
@@ -383,7 +383,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
                                             flag_PLL_180_deg_phase_locked = false;
                                         }
                                     DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at "
-                                               << static_cast<double>(d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs) << " [s]";
+                                               << static_cast<double>(d_preamble_time_samples) / static_cast<double>(d_symbol_history[0].fs) << " [s]";
                                 }
 
                             // try to decode the subframe:
@@ -407,7 +407,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
         {
             if (d_stat == 1)
                 {
-                    preamble_diff_ms = round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
+                    preamble_diff_ms = round(((static_cast<double>(d_symbol_history[0].Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history[0].fs)) * 1000.0);
                     if (preamble_diff_ms > GPS_SUBFRAME_MS)
                         {
                             DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms;

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc

@@ -64,8 +64,8 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc(
     DLOG(INFO) << "GPS L5 TELEMETRY PROCESSING: satellite " << d_satellite;
     d_channel = 0;
     d_flag_valid_word = false;
-    d_TOW_at_current_symbol_ms = 0;
-    d_TOW_at_Preamble_ms = 0;
+    d_TOW_at_current_symbol_ms = 0U;
+    d_TOW_at_Preamble_ms = 0U;
     // initialize the CNAV frame decoder (libswiftcnav)
     cnav_msg_decoder_init(&d_cnav_decoder);
     for (int32_t aux = 0; aux < GPS_L5i_NH_CODE_LENGTH; aux++)

src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc

@@ -189,7 +189,7 @@ sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::symbol_aligner_and_dec
 {
     // convolutional code properties
     d_KK = 7;
-    int32_t nn = 2;
+    const int32_t nn = 2;
     int32_t g_encoder[nn];
     g_encoder[0] = 121;
     g_encoder[1] = 91;

src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c

@@ -30,7 +30,6 @@
  */
 
 
-
 #include "edc.h"
 #include "bits.h"
 #include "cnav_msg.h"
@@ -48,26 +47,26 @@
  * Block Viterbi decoding parameters.
  */
 /** Viterbi decoder reversed polynomial A */
-#define GPS_L2C_V27_POLY_A       (0x4F) /* 0b01001111 - reversed 0171*/
+#define GPS_L2C_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/
 
 /** Viterbi decoder reversed polynomial B */
-#define GPS_L2C_V27_POLY_B       (0x6D) /* 0b01101101 - reversed 0133 */
+#define GPS_L2C_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */
 /*
  * GPS L2C message constants.
  */
 
 /** GPS L2C preamble */
-#define GPS_CNAV_PREAMBLE1          (0b10001011u)
+const u32 GPS_CNAV_PREAMBLE1 = 0x8Bu; /* (0b10001011u) */
 /** Inverted GPS L2C preamble */
-#define GPS_CNAV_PREAMBLE2          (0b01110100u)
+const u32 GPS_CNAV_PREAMBLE2 = 0x74u; /* (0b01110100u) */
 /** GPS L2C preamble length in bits */
-#define GPS_CNAV_PREAMBLE_LENGTH    (8)
+#define GPS_CNAV_PREAMBLE_LENGTH (8)
 /** GPS L2C CNAV message length in bits */
-#define GPS_CNAV_MSG_LENGTH         (300)
+#define GPS_CNAV_MSG_LENGTH (300)
 /** GPS LC2 CNAV CRC length in bits */
-#define GPS_CNAV_MSG_CRC_LENGTH     (24)
+#define GPS_CNAV_MSG_CRC_LENGTH (24)
 /** GPS L2C CNAV message payload length in bits */
-#define GPS_CNAV_MSG_DATA_LENGTH    (GPS_CNAV_MSG_LENGTH - GPS_CNAV_MSG_CRC_LENGTH)
+#define GPS_CNAV_MSG_DATA_LENGTH (GPS_CNAV_MSG_LENGTH - GPS_CNAV_MSG_CRC_LENGTH)
 /** GPS L2C CNAV message lock detector threshold */
 #define GPS_CNAV_LOCK_MAX_CRC_FAILS (10)
 
@@ -85,7 +84,7 @@
 static u32 _cnav_compute_crc(cnav_v27_part_t *part)
 {
     u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
-            part->invert);
+        part->invert);
 
     return crc;
 }
@@ -104,7 +103,7 @@ static u32 _cnav_compute_crc(cnav_v27_part_t *part)
 static u32 _cnav_extract_crc(const cnav_v27_part_t *part)
 {
     u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
-            GPS_CNAV_MSG_CRC_LENGTH);
+        GPS_CNAV_MSG_CRC_LENGTH);
     if (part->invert)
         {
             crc ^= 0xFFFFFF;
@@ -152,7 +151,7 @@ static void _cnav_rescan_preamble(cnav_v27_part_t *part)
     if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH)
         {
             bitshl(part->decoded, sizeof(part->decoded),
-                    part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1);
+                part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1);
             part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1;
         }
 }
@@ -200,11 +199,12 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch)
      * - N - Number of bits to put into decoded buffer
      * - M - Number of bits in the tail to ignore.
      */
-    unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS +
-            CHAR_BIT - 1) / CHAR_BIT];
+    unsigned char tmp_bits[(GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS +
+                               CHAR_BIT - 1) /
+                           CHAR_BIT];
 
     v27_chainback_likely(&part->dec, tmp_bits,
-            GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
+        GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
 
     /* Read decoded bits and add them to the decoded buffer */
     bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS);
@@ -238,10 +238,9 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch)
                 }
             if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded)
                 {
-
                     /* We have collected 300 bits starting from message preamble. Now try
                      * to compute CRC-24Q */
-                    u32 crc  = _cnav_compute_crc(part);
+                    u32 crc = _cnav_compute_crc(part);
                     u32 crc2 = _cnav_extract_crc(part);
 
                     if (part->message_lock)
@@ -260,8 +259,8 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch)
                                     if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS)
                                         {
                                             /* CRC has failed too many times - drop the lock. */
-                                            part->n_crc_fail    = 0;
-                                            part->message_lock  = false;
+                                            part->n_crc_fail = 0;
+                                            part->message_lock = false;
                                             part->preamble_seen = false;
                                             /* Try to find a new preamble, reuse data from buffer. */
                                             retry = true;
@@ -272,8 +271,8 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch)
                         {
                             /* CRC match - message can be decoded */
                             part->message_lock = true;
-                            part->crc_ok       = true;
-                            part->n_crc_fail   = 0;
+                            part->crc_ok = true;
+                            part->n_crc_fail = 0;
                         }
                     else
                         {
@@ -346,13 +345,13 @@ static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay)
                             _cnav_msg_invert(part);
                         }
 
-                    msg->prn    = getbitu(part->decoded, 8, 6);
+                    msg->prn = getbitu(part->decoded, 8, 6);
                     msg->msg_id = getbitu(part->decoded, 14, 6);
-                    msg->tow    = getbitu(part->decoded, 20, 17);
-                    msg->alert  = getbitu(part->decoded, 37, 1) ? true : false;
+                    msg->tow = getbitu(part->decoded, 20, 17);
+                    msg->alert = getbitu(part->decoded, 37, 1) ? true : false;
 
                     /* copy RAW message for GNSS-SDR */
-                    memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
+                    memcpy(msg->raw_msg, part->decoded, GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
 
                     *delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) * 2 + part->n_symbols;
 
@@ -388,15 +387,15 @@ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
 {
     memset(dec, 0, sizeof(*dec));
     v27_init(&dec->part1.dec,
-            dec->part1.decisions,
-            GPS_L2_V27_HISTORY_LENGTH_BITS,
-            cnav_msg_decoder_get_poly(),
-            0);
+        dec->part1.decisions,
+        GPS_L2_V27_HISTORY_LENGTH_BITS,
+        cnav_msg_decoder_get_poly(),
+        0);
     v27_init(&dec->part2.dec,
-            dec->part2.decisions,
-            GPS_L2_V27_HISTORY_LENGTH_BITS,
-            cnav_msg_decoder_get_poly(),
-            0);
+        dec->part2.decisions,
+        GPS_L2_V27_HISTORY_LENGTH_BITS,
+        cnav_msg_decoder_get_poly(),
+        0);
     dec->part1.init = true;
     dec->part2.init = true;
     _cnav_add_symbol(&dec->part2, 0x80);
@@ -426,9 +425,9 @@ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
  * \retval false More data is required.
  */
 bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
-        u8 symbol,
-        cnav_msg_t *msg,
-        u32 *pdelay)
+    u8 symbol,
+    cnav_msg_t *msg,
+    u32 *pdelay)
 {
     _cnav_add_symbol(&dec->part1, symbol);
     _cnav_add_symbol(&dec->part2, symbol);
@@ -470,7 +469,7 @@ const v27_poly_t *cnav_msg_decoder_get_poly(void)
     if (!initialized)
         {
             /* Coefficients for polynomial object */
-            const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B };
+            const signed char coeffs[2] = {GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B};
 
             /* Racing condition handling: the data can be potential initialized more
              * than once in case multiple threads request concurrent access. However,

src/algorithms/tracking/adapters/CMakeLists.txt

@@ -35,6 +35,7 @@ set(TRACKING_ADAPTER_SOURCES
      gps_l2_m_dll_pll_tracking.cc
      glonass_l1_ca_dll_pll_tracking.cc
      glonass_l1_ca_dll_pll_c_aid_tracking.cc
+     gps_l1_ca_kf_tracking.cc
      gps_l5_dll_pll_tracking.cc
      glonass_l2_ca_dll_pll_tracking.cc
      glonass_l2_ca_dll_pll_c_aid_tracking.cc
@@ -49,6 +50,7 @@ include_directories(
      ${CMAKE_SOURCE_DIR}/src/algorithms/tracking/gnuradio_blocks
      ${CMAKE_SOURCE_DIR}/src/algorithms/tracking/libs
      ${CMAKE_SOURCE_DIR}/src/algorithms/libs
+     ${ARMADILLO_INCLUDE_DIRS}
      ${GLOG_INCLUDE_DIRS}
      ${GFlags_INCLUDE_DIRS}
      ${GNURADIO_RUNTIME_INCLUDE_DIRS}

src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc

@@ -0,0 +1,174 @@
+/*!
+ * \file gps_l1_ca_kf_tracking.cc
+ * \brief Implementation of an adapter of a DLL + Kalman carrier
+ * tracking loop block for GPS L1 C/A signals
+ * \author Javier Arribas, 2018. jarribas(at)cttc.es
+ * \author Jordi Vila-Valls 2018. jvila(at)cttc.es
+ * \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
+ *
+ * Reference:
+ * J. Vila-Valls, P. Closas, M. Navarro and C. Fernández-Prades,
+ * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
+ * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
+ * Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2018  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+
+#include "gps_l1_ca_kf_tracking.h"
+#include "gnss_sdr_flags.h"
+#include "GPS_L1_CA.h"
+#include "configuration_interface.h"
+#include <glog/logging.h>
+
+
+using google::LogMessage;
+
+GpsL1CaKfTracking::GpsL1CaKfTracking(
+    ConfigurationInterface* configuration, std::string role,
+    unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
+{
+    DLOG(INFO) << "role " << role;
+    //################# CONFIGURATION PARAMETERS ########################
+    int order;
+    int fs_in;
+    int vector_length;
+    int f_if;
+    bool dump;
+    std::string dump_filename;
+    std::string item_type;
+    std::string default_item_type = "gr_complex";
+    float dll_bw_hz;
+    float early_late_space_chips;
+    bool bce_run;
+    unsigned int bce_ptrans;
+    unsigned int bce_strans;
+    int bce_nu;
+    int bce_kappa;
+
+    item_type = configuration->property(role + ".item_type", default_item_type);
+    order = configuration->property(role + ".order", 2);
+    int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
+    fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
+    f_if = configuration->property(role + ".if", 0);
+    dump = configuration->property(role + ".dump", false);
+    dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
+    if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
+    early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
+    std::string default_dump_filename = "./track_ch";
+    dump_filename = configuration->property(role + ".dump_filename", default_dump_filename);
+    vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
+
+    bce_run = configuration->property(role + ".bce_run", false);
+    bce_ptrans = configuration->property(role + ".p_transient", 0);
+    bce_strans = configuration->property(role + ".s_transient", 0);
+    bce_nu = configuration->property(role + ".bce_nu", 0);
+    bce_kappa = configuration->property(role + ".bce_kappa", 0);
+
+    //################# MAKE TRACKING GNURadio object ###################
+    if (item_type.compare("gr_complex") == 0)
+        {
+            item_size_ = sizeof(gr_complex);
+            tracking_ = gps_l1_ca_kf_make_tracking_cc(
+                order,
+                f_if,
+                fs_in,
+                vector_length,
+                dump,
+                dump_filename,
+                dll_bw_hz,
+                early_late_space_chips,
+                bce_run,
+                bce_ptrans,
+                bce_strans,
+                bce_nu,
+                bce_kappa);
+        }
+    else
+        {
+            item_size_ = sizeof(gr_complex);
+            LOG(WARNING) << item_type << " unknown tracking item type.";
+        }
+    channel_ = 0;
+    DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
+}
+
+
+GpsL1CaKfTracking::~GpsL1CaKfTracking()
+{
+}
+
+
+void GpsL1CaKfTracking::start_tracking()
+{
+    tracking_->start_tracking();
+}
+
+
+/*
+ * Set tracking channel unique ID
+ */
+void GpsL1CaKfTracking::set_channel(unsigned int channel)
+{
+    channel_ = channel;
+    tracking_->set_channel(channel);
+}
+
+
+void GpsL1CaKfTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
+{
+    tracking_->set_gnss_synchro(p_gnss_synchro);
+}
+
+
+void GpsL1CaKfTracking::connect(gr::top_block_sptr top_block)
+{
+    if (top_block)
+        { /* top_block is not null */
+        };
+    //nothing to connect, now the tracking uses gr_sync_decimator
+}
+
+
+void GpsL1CaKfTracking::disconnect(gr::top_block_sptr top_block)
+{
+    if (top_block)
+        { /* top_block is not null */
+        };
+    //nothing to disconnect, now the tracking uses gr_sync_decimator
+}
+
+
+gr::basic_block_sptr GpsL1CaKfTracking::get_left_block()
+{
+    return tracking_;
+}
+
+
+gr::basic_block_sptr GpsL1CaKfTracking::get_right_block()
+{
+    return tracking_;
+}

src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h

@@ -0,0 +1,105 @@
+/*!
+ * \file GPS_L1_CA_KF_Tracking.h
+ * \brief  Interface of an adapter of a DLL + Kalman carrier
+ * tracking loop block for GPS L1 C/A signals
+ * \author Javier Arribas, 2018. jarribas(at)cttc.es
+ * \author Jordi Vila-Valls 2018. jvila(at)cttc.es
+ * \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
+ *
+ * Reference:
+ * J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades,
+ * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
+ * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
+ * Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2018  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_
+#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_
+
+#include "gps_l1_ca_kf_tracking_cc.h"
+#include "tracking_interface.h"
+#include <string>
+
+class ConfigurationInterface;
+
+/*!
+ * \brief This class implements a code DLL + carrier PLL tracking loop
+ */
+class GpsL1CaKfTracking : public TrackingInterface
+{
+public:
+    GpsL1CaKfTracking(ConfigurationInterface* configuration,
+        std::string role,
+        unsigned int in_streams,
+        unsigned int out_streams);
+
+    virtual ~GpsL1CaKfTracking();
+
+    inline std::string role() override
+    {
+        return role_;
+    }
+
+    //! Returns "GPS_L1_CA_KF_Tracking"
+    inline std::string implementation() override
+    {
+        return "GPS_L1_CA_KF_Tracking";
+    }
+
+    inline size_t item_size() override
+    {
+        return item_size_;
+    }
+
+    void connect(gr::top_block_sptr top_block) override;
+    void disconnect(gr::top_block_sptr top_block) override;
+    gr::basic_block_sptr get_left_block() override;
+    gr::basic_block_sptr get_right_block() override;
+
+    /*!
+     * \brief Set tracking channel unique ID
+     */
+    void set_channel(unsigned int channel) override;
+
+    /*!
+     * \brief Set acquisition/tracking common Gnss_Synchro object pointer
+     * to efficiently exchange synchronization data between acquisition and tracking blocks
+     */
+    void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override;
+
+    void start_tracking() override;
+
+private:
+    gps_l1_ca_kf_tracking_cc_sptr tracking_;
+    size_t item_size_;
+    unsigned int channel_;
+    std::string role_;
+    unsigned int in_streams_;
+    unsigned int out_streams_;
+};
+
+#endif  // GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_

src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt

@@ -34,6 +34,7 @@ set(TRACKING_GR_BLOCKS_SOURCES
      glonass_l1_ca_dll_pll_tracking_cc.cc
      glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc
      glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc
+     gps_l1_ca_kf_tracking_cc.cc  
      glonass_l2_ca_dll_pll_tracking_cc.cc
      glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc
      glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc
@@ -48,6 +49,7 @@ include_directories(
      ${CMAKE_SOURCE_DIR}/src/core/receiver
      ${CMAKE_SOURCE_DIR}/src/algorithms/tracking/libs
      ${CMAKE_SOURCE_DIR}/src/algorithms/libs
+     ${ARMADILLO_INCLUDE_DIRS}
      ${GLOG_INCLUDE_DIRS}
      ${GFlags_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc

@@ -0,0 +1,956 @@
+/*!
+ * \file gps_l1_ca_kf_tracking_cc.cc
+ * \brief Implementation of a processing block of a DLL + Kalman carrier
+ * tracking loop for GPS L1 C/A signals
+ * \author Javier Arribas, 2018. jarribas(at)cttc.es
+ * \author Jordi Vila-Valls 2018. jvila(at)cttc.es
+ * \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
+ *
+ * Reference:
+ * J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades,
+ * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
+ * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
+ * Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2018  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "gps_l1_ca_kf_tracking_cc.h"
+#include "gps_sdr_signal_processing.h"
+#include "tracking_discriminators.h"
+#include "lock_detectors.h"
+#include "gnss_sdr_flags.h"
+#include "GPS_L1_CA.h"
+#include "control_message_factory.h"
+#include <boost/lexical_cast.hpp>
+#include <gnuradio/io_signature.h>
+#include <glog/logging.h>
+#include <volk_gnsssdr/volk_gnsssdr.h>
+#include <matio.h>
+#include <cmath>
+#include <iostream>
+#include <memory>
+#include <sstream>
+
+
+using google::LogMessage;
+
+gps_l1_ca_kf_tracking_cc_sptr
+gps_l1_ca_kf_make_tracking_cc(
+    uint32_t order,
+    int64_t if_freq,
+    int64_t fs_in,
+    uint32_t vector_length,
+    bool dump,
+    std::string dump_filename,
+    float dll_bw_hz,
+    float early_late_space_chips,
+    bool bce_run,
+    uint32_t bce_ptrans,
+    uint32_t bce_strans,
+    int32_t bce_nu,
+    int32_t bce_kappa)
+{
+    return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(order, if_freq,
+        fs_in, vector_length, dump, dump_filename, dll_bw_hz, early_late_space_chips,
+        bce_run, bce_ptrans, bce_strans, bce_nu, bce_kappa));
+}
+
+
+void Gps_L1_Ca_Kf_Tracking_cc::forecast(int noutput_items,
+    gr_vector_int &ninput_items_required)
+{
+    if (noutput_items != 0)
+        {
+            ninput_items_required[0] = static_cast<int>(d_vector_length) * 2;  // set the required available samples in each call
+        }
+}
+
+
+Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
+    uint32_t order,
+    int64_t if_freq,
+    int64_t fs_in,
+    uint32_t vector_length,
+    bool dump,
+    std::string dump_filename,
+    float dll_bw_hz,
+    float early_late_space_chips,
+    bool bce_run,
+    uint32_t bce_ptrans,
+    uint32_t bce_strans,
+    int32_t bce_nu,
+    int32_t bce_kappa) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
+                             gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
+{
+    // Telemetry bit synchronization message port input
+    this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
+    this->message_port_register_out(pmt::mp("events"));
+
+    // initialize internal vars
+    d_order = order;
+    d_dump = dump;
+    d_if_freq = if_freq;
+    d_fs_in = fs_in;
+    d_vector_length = vector_length;
+    d_dump_filename = dump_filename;
+
+    d_current_prn_length_samples = static_cast<int>(d_vector_length);
+
+    // Initialize tracking  ==========================================
+    d_code_loop_filter.set_DLL_BW(dll_bw_hz);
+
+    // --- DLL variables --------------------------------------------------------
+    d_early_late_spc_chips = early_late_space_chips;  // Define early-late offset (in chips)
+
+    // Initialization of local code replica
+    // Get space for a vector with the C/A code replica sampled 1x/chip
+    d_ca_code = static_cast<float *>(volk_gnsssdr_malloc(static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment()));
+
+    // correlator outputs (scalar)
+    d_n_correlator_taps = 3;  // Early, Prompt, and Late
+    d_correlator_outs = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
+    for (int32_t n = 0; n < d_n_correlator_taps; n++)
+        {
+            d_correlator_outs[n] = gr_complex(0, 0);
+        }
+    d_local_code_shift_chips = static_cast<float *>(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment()));
+    // Set TAPs delay values [chips]
+    d_local_code_shift_chips[0] = -d_early_late_spc_chips;
+    d_local_code_shift_chips[1] = 0.0;
+    d_local_code_shift_chips[2] = d_early_late_spc_chips;
+
+    multicorrelator_cpu.init(2 * d_current_prn_length_samples, d_n_correlator_taps);
+
+    // --- Perform initializations ------------------------------
+    // define initial code frequency basis of NCO
+    d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ;
+    // define residual code phase (in chips)
+    d_rem_code_phase_samples = 0.0;
+    // define residual carrier phase
+    d_rem_carr_phase_rad = 0.0;
+    // define residual carrier phase covariance
+    d_carr_phase_sigma2 = 0.0;
+
+    // sample synchronization
+    d_sample_counter = 0;
+    d_acq_sample_stamp = 0;
+
+    d_enable_tracking = false;
+    d_pull_in = false;
+
+    // CN0 estimation and lock detector buffers
+    d_cn0_estimation_counter = 0;
+    d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
+    d_carrier_lock_test = 1;
+    d_CN0_SNV_dB_Hz = 0;
+    d_carrier_lock_fail_counter = 0;
+    d_carrier_lock_threshold = FLAGS_carrier_lock_th;
+
+    systemName["G"] = std::string("GPS");
+    systemName["S"] = std::string("SBAS");
+
+    d_acquisition_gnss_synchro = 0;
+    d_channel = 0;
+    d_acq_code_phase_samples = 0.0;
+    d_acq_carrier_doppler_hz = 0.0;
+    d_carrier_doppler_hz = 0.0;
+    d_carrier_dopplerrate_hz2 = 0.0;
+    d_acc_carrier_phase_rad = 0.0;
+    d_code_phase_samples = 0.0;
+    d_rem_code_phase_chips = 0.0;
+    d_code_phase_step_chips = 0.0;
+    d_carrier_phase_step_rad = 0.0;
+    code_error_chips = 0.0;
+    code_error_filt_chips = 0.0;
+
+    set_relative_rate(1.0 / static_cast<double>(d_vector_length));
+
+    // Kalman filter initialization (receiver initialization)
+
+    double CN_dB_Hz = 30;
+    double CN_lin = pow(10, CN_dB_Hz / 10.0);
+
+    double sigma2_phase_detector_cycles2;
+    sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD));
+
+    // covariances (static)
+    double sigma2_carrier_phase = GPS_TWO_PI / 4;
+    double sigma2_doppler = 450;
+    double sigma2_doppler_rate = pow(4.0 * GPS_TWO_PI, 2) / 12.0;
+
+    kf_P_x_ini = arma::zeros(2, 2);
+    kf_P_x_ini(0, 0) = sigma2_carrier_phase;
+    kf_P_x_ini(1, 1) = sigma2_doppler;
+
+    kf_R = arma::zeros(1, 1);
+    kf_R(0, 0) = sigma2_phase_detector_cycles2;
+
+    kf_Q = arma::zeros(2, 2);
+    kf_Q(0, 0) = pow(GPS_L1_CA_CODE_PERIOD, 4);
+    kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD;
+
+    kf_F = arma::zeros(2, 2);
+    kf_F(0, 0) = 1.0;
+    kf_F(0, 1) = GPS_TWO_PI * GPS_L1_CA_CODE_PERIOD;
+    kf_F(1, 0) = 0.0;
+    kf_F(1, 1) = 1.0;
+
+    kf_H = arma::zeros(1, 2);
+    kf_H(0, 0) = 1.0;
+
+    kf_x = arma::zeros(2, 1);
+    kf_y = arma::zeros(1, 1);
+    kf_P_y = arma::zeros(1, 1);
+
+    // order three
+    if (d_order == 3)
+        {
+            kf_P_x_ini = arma::resize(kf_P_x_ini, 3, 3);
+            kf_P_x_ini(2, 2) = sigma2_doppler_rate;
+
+            kf_Q = arma::zeros(3, 3);
+            kf_Q(0, 0) = pow(GPS_L1_CA_CODE_PERIOD, 4);
+            kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD;
+            kf_Q(2, 2) = GPS_L1_CA_CODE_PERIOD;
+
+            kf_F = arma::resize(kf_F, 3, 3);
+            kf_F(0, 2) = 0.5 * GPS_TWO_PI * pow(GPS_L1_CA_CODE_PERIOD, 2);
+            kf_F(1, 2) = GPS_L1_CA_CODE_PERIOD;
+            kf_F(2, 0) = 0.0;
+            kf_F(2, 1) = 0.0;
+            kf_F(2, 2) = 1.0;
+
+            kf_H = arma::resize(kf_H, 1, 3);
+            kf_H(0, 2) = 0.0;
+
+            kf_x = arma::resize(kf_x, 3, 1);
+            kf_x(2, 0) = 0.0;
+        }
+
+    // Bayesian covariance estimator initialization
+    kf_iter = 0;
+    bayes_run = bce_run;
+    bayes_ptrans = bce_ptrans;
+    bayes_strans = bce_strans;
+
+    bayes_kappa = bce_kappa;
+    bayes_nu = bce_nu;
+    kf_R_est = kf_R;
+
+    bayes_estimator.init(arma::zeros(1, 1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R) * (bayes_nu + 2));
+}
+
+void Gps_L1_Ca_Kf_Tracking_cc::start_tracking()
+{
+    /*
+     *  correct the code phase according to the delay between acq and trk
+     */
+    d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
+    d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
+    d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
+    d_acq_carrier_doppler_step_hz = static_cast<double>(d_acquisition_gnss_synchro->Acq_doppler_step);
+
+    // Correct Kalman filter covariance according to acq doppler step size (3 sigma)
+    if (d_acquisition_gnss_synchro->Acq_doppler_step > 0)
+        {
+            kf_P_x_ini(1, 1) = pow(d_acq_carrier_doppler_step_hz / 3.0, 2);
+            bayes_estimator.init(arma::zeros(1, 1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R) * (bayes_nu + 2));
+        }
+
+    int64_t acq_trk_diff_samples;
+    double acq_trk_diff_seconds;
+    acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp);  //-d_vector_length;
+    DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples;
+    acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
+    // Doppler effect Fd = (C / (C + Vr)) * F
+    double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
+    // new chip and prn sequence periods based on acq Doppler
+    double T_chip_mod_seconds;
+    double T_prn_mod_seconds;
+    double T_prn_mod_samples;
+    d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
+    d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
+    T_chip_mod_seconds = 1 / d_code_freq_chips;
+    T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
+    T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
+
+    d_current_prn_length_samples = round(T_prn_mod_samples);
+
+    double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
+    double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
+    double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
+    double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
+    double corrected_acq_phase_samples, delay_correction_samples;
+    corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
+    if (corrected_acq_phase_samples < 0)
+        {
+            corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
+        }
+    delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples;
+
+    d_acq_code_phase_samples = corrected_acq_phase_samples;
+
+    d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
+    d_carrier_dopplerrate_hz2 = 0;
+    d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
+
+    // DLL filter initialization
+    d_code_loop_filter.initialize();  // initialize the code filter
+
+    // generate local reference ALWAYS starting at chip 1 (1 sample per chip)
+    gps_l1_ca_code_gen_float(d_ca_code, d_acquisition_gnss_synchro->PRN, 0);
+
+    multicorrelator_cpu.set_local_code_and_taps(static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
+    for (int32_t n = 0; n < d_n_correlator_taps; n++)
+        {
+            d_correlator_outs[n] = gr_complex(0, 0);
+        }
+
+    d_carrier_lock_fail_counter = 0;
+    d_rem_code_phase_samples = 0;
+    d_rem_carr_phase_rad = 0.0;
+    d_rem_code_phase_chips = 0.0;
+    d_acc_carrier_phase_rad = 0.0;
+    d_carr_phase_sigma2 = 0.0;
+
+    d_code_phase_samples = d_acq_code_phase_samples;
+
+    std::string sys_ = &d_acquisition_gnss_synchro->System;
+    sys = sys_.substr(0, 1);
+
+    // DEBUG OUTPUT
+    std::cout << "Tracking of GPS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
+    LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel;
+
+    // enable tracking
+    d_pull_in = true;
+    d_enable_tracking = true;
+
+    LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
+              << " Code Phase correction [samples]=" << delay_correction_samples
+              << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
+}
+
+
+Gps_L1_Ca_Kf_Tracking_cc::~Gps_L1_Ca_Kf_Tracking_cc()
+{
+    if (d_dump_file.is_open())
+        {
+            try
+                {
+                    d_dump_file.close();
+                }
+            catch (const std::exception &ex)
+                {
+                    LOG(WARNING) << "Exception in destructor " << ex.what();
+                }
+        }
+    if (d_dump)
+        {
+            if (d_channel == 0)
+                {
+                    std::cout << "Writing .mat files ...";
+                }
+            Gps_L1_Ca_Kf_Tracking_cc::save_matfile();
+            if (d_channel == 0)
+                {
+                    std::cout << " done." << std::endl;
+                }
+        }
+    try
+        {
+            volk_gnsssdr_free(d_local_code_shift_chips);
+            volk_gnsssdr_free(d_correlator_outs);
+            volk_gnsssdr_free(d_ca_code);
+            delete[] d_Prompt_buffer;
+            multicorrelator_cpu.free();
+        }
+    catch (const std::exception &ex)
+        {
+            LOG(WARNING) << "Exception in destructor " << ex.what();
+        }
+}
+
+
+int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
+{
+    // READ DUMP FILE
+    std::ifstream::pos_type size;
+    int32_t number_of_double_vars = 1;
+    int32_t number_of_float_vars = 19;
+    int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
+                               sizeof(float) * number_of_float_vars + sizeof(uint32_t);
+    std::ifstream dump_file;
+    dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
+    try
+        {
+            dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
+        }
+    catch (const std::ifstream::failure &e)
+        {
+            std::cerr << "Problem opening dump file:" << e.what() << std::endl;
+            return 1;
+        }
+    // count number of epochs and rewind
+    int64_t num_epoch = 0;
+    if (dump_file.is_open())
+        {
+            size = dump_file.tellg();
+            num_epoch = static_cast<int64_t>(size) / static_cast<int64_t>(epoch_size_bytes);
+            dump_file.seekg(0, std::ios::beg);
+        }
+    else
+        {
+            return 1;
+        }
+    float *abs_VE = new float[num_epoch];
+    float *abs_E = new float[num_epoch];
+    float *abs_P = new float[num_epoch];
+    float *abs_L = new float[num_epoch];
+    float *abs_VL = new float[num_epoch];
+    float *Prompt_I = new float[num_epoch];
+    float *Prompt_Q = new float[num_epoch];
+    uint64_t *PRN_start_sample_count = new uint64_t[num_epoch];
+    float *acc_carrier_phase_rad = new float[num_epoch];
+    float *carrier_doppler_hz = new float[num_epoch];
+    float *carrier_dopplerrate_hz2 = new float[num_epoch];
+    float *code_freq_chips = new float[num_epoch];
+    float *carr_error_hz = new float[num_epoch];
+    float *carr_noise_sigma2 = new float[num_epoch];
+    float *carr_error_filt_hz = new float[num_epoch];
+    float *code_error_chips = new float[num_epoch];
+    float *code_error_filt_chips = new float[num_epoch];
+    float *CN0_SNV_dB_Hz = new float[num_epoch];
+    float *carrier_lock_test = new float[num_epoch];
+    float *aux1 = new float[num_epoch];
+    double *aux2 = new double[num_epoch];
+    uint32_t *PRN = new uint32_t[num_epoch];
+
+    try
+        {
+            if (dump_file.is_open())
+                {
+                    for (int64_t i = 0; i < num_epoch; i++)
+                        {
+                            dump_file.read(reinterpret_cast<char *>(&abs_VE[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&abs_E[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&abs_P[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&abs_L[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&abs_VL[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&Prompt_I[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&Prompt_Q[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&PRN_start_sample_count[i]), sizeof(uint64_t));
+                            dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&carrier_dopplerrate_hz2[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&code_freq_chips[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&carr_error_hz[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&carr_noise_sigma2[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&code_error_chips[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&code_error_filt_chips[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&CN0_SNV_dB_Hz[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&carrier_lock_test[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&aux1[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&aux2[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&PRN[i]), sizeof(uint32_t));
+                        }
+                }
+            dump_file.close();
+        }
+    catch (const std::ifstream::failure &e)
+        {
+            std::cerr << "Problem reading dump file:" << e.what() << std::endl;
+            delete[] abs_VE;
+            delete[] abs_E;
+            delete[] abs_P;
+            delete[] abs_L;
+            delete[] abs_VL;
+            delete[] Prompt_I;
+            delete[] Prompt_Q;
+            delete[] PRN_start_sample_count;
+            delete[] acc_carrier_phase_rad;
+            delete[] carrier_doppler_hz;
+            delete[] carrier_dopplerrate_hz2;
+            delete[] code_freq_chips;
+            delete[] carr_error_hz;
+            delete[] carr_noise_sigma2;
+            delete[] carr_error_filt_hz;
+            delete[] code_error_chips;
+            delete[] code_error_filt_chips;
+            delete[] CN0_SNV_dB_Hz;
+            delete[] carrier_lock_test;
+            delete[] aux1;
+            delete[] aux2;
+            delete[] PRN;
+            return 1;
+        }
+
+    // WRITE MAT FILE
+    mat_t *matfp;
+    matvar_t *matvar;
+    std::string filename = d_dump_filename;
+    filename.erase(filename.length() - 4, 4);
+    filename.append(".mat");
+    matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
+    if (reinterpret_cast<long *>(matfp) != NULL)
+        {
+            size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
+            matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carrier_dopplerrate_hz2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_dopplerrate_hz2, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carr_noise_sigma2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_noise_sigma2, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB);  // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+        }
+    Mat_Close(matfp);
+    delete[] abs_VE;
+    delete[] abs_E;
+    delete[] abs_P;
+    delete[] abs_L;
+    delete[] abs_VL;
+    delete[] Prompt_I;
+    delete[] Prompt_Q;
+    delete[] PRN_start_sample_count;
+    delete[] acc_carrier_phase_rad;
+    delete[] carrier_doppler_hz;
+    delete[] carrier_dopplerrate_hz2;
+    delete[] code_freq_chips;
+    delete[] carr_error_hz;
+    delete[] carr_noise_sigma2;
+    delete[] carr_error_filt_hz;
+    delete[] code_error_chips;
+    delete[] code_error_filt_chips;
+    delete[] CN0_SNV_dB_Hz;
+    delete[] carrier_lock_test;
+    delete[] aux1;
+    delete[] aux2;
+    delete[] PRN;
+    return 0;
+}
+
+
+void Gps_L1_Ca_Kf_Tracking_cc::set_channel(uint32_t channel)
+{
+    gr::thread::scoped_lock l(d_setlock);
+    d_channel = channel;
+    LOG(INFO) << "Tracking Channel set to " << d_channel;
+    // ############# ENABLE DATA FILE LOG #################
+    if (d_dump)
+        {
+            if (!d_dump_file.is_open())
+                {
+                    try
+                        {
+                            d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
+                            d_dump_filename.append(".dat");
+                            d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
+                            d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
+                            LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
+                        }
+                    catch (const std::ifstream::failure &e)
+                        {
+                            LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
+                        }
+                }
+        }
+}
+
+
+void Gps_L1_Ca_Kf_Tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro)
+{
+    d_acquisition_gnss_synchro = p_gnss_synchro;
+}
+
+
+int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
+    gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
+{
+    // process vars
+    d_carr_phase_error_rad = 0.0;
+    double code_error_chips = 0.0;
+    double code_error_filt_chips = 0.0;
+
+    // Block input data and block output stream pointers
+    const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]);
+    Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]);
+
+    // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
+    Gnss_Synchro current_synchro_data = Gnss_Synchro();
+
+    if (d_enable_tracking == true)
+        {
+            // Fill the acquisition data
+            current_synchro_data = *d_acquisition_gnss_synchro;
+            // Receiver signal alignment
+            if (d_pull_in == true)
+                {
+                    // Signal alignment (skip samples until the incoming signal is aligned with local replica)
+                    uint64_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
+                    double acq_trk_shif_correction_samples = static_cast<double>(d_current_prn_length_samples) - std::fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_current_prn_length_samples));
+                    int32_t samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
+                    if (samples_offset < 0)
+                        {
+                            samples_offset = 0;
+                        }
+                    d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_acq_code_phase_samples;
+
+                    d_sample_counter += samples_offset;  // count for the processed samples
+                    d_pull_in = false;
+
+                    current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
+                    current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+                    current_synchro_data.fs = d_fs_in;
+                    current_synchro_data.correlation_length_ms = 1;
+                    *out[0] = current_synchro_data;
+                    // Kalman filter initialization reset
+                    kf_P_x = kf_P_x_ini;
+                    // Update Kalman states based on acquisition information
+                    kf_x(0) = d_carrier_phase_step_rad * samples_offset;
+                    kf_x(1) = d_carrier_doppler_hz;
+                    if (kf_x.n_elem > 2)
+                        {
+                            kf_x(2) = d_carrier_dopplerrate_hz2;
+                        }
+
+                    // Covariance estimation initialization reset
+                    kf_iter = 0;
+                    bayes_estimator.init(arma::zeros(1, 1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R) * (bayes_nu + 2));
+
+                    consume_each(samples_offset);  // shift input to perform alignment with local replica
+                    return 1;
+                }
+
+            // ################# CARRIER WIPEOFF AND CORRELATORS ##############################
+            // Perform carrier wipe-off and compute Early, Prompt and Late correlation
+            multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in);
+            multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad,
+                d_carrier_phase_step_rad,
+                d_rem_code_phase_chips,
+                d_code_phase_step_chips,
+                d_current_prn_length_samples);
+
+            // ################## Kalman Carrier Tracking ######################################
+
+            // Kalman state prediction (time update)
+            kf_x_pre = kf_F * kf_x;                        //state prediction
+            kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q;  //state error covariance prediction
+
+            // Update discriminator [rads/Ti]
+            d_carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]);  // prompt output
+
+            // Kalman estimation (measurement update)
+            double sigma2_phase_detector_cycles2;
+            double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0);
+            sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD));
+
+            kf_y(0) = d_carr_phase_error_rad;  // measurement vector
+            kf_R(0, 0) = sigma2_phase_detector_cycles2;
+
+            if (bayes_run && (kf_iter >= bayes_ptrans))
+                {
+                    bayes_estimator.update_sequential(kf_y);
+                }
+            if (bayes_run && (kf_iter >= (bayes_ptrans + bayes_strans)))
+                {
+                    // TODO: Resolve segmentation fault
+                    kf_P_y = bayes_estimator.get_Psi_est();
+                    kf_R_est = kf_P_y - kf_H * kf_P_x_pre * kf_H.t();
+                }
+            else
+                {
+                    kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R;  // innovation covariance matrix
+                    kf_R_est = kf_R;
+                }
+
+            // Kalman filter update step
+            kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y);                 // Kalman gain
+            kf_x = kf_x_pre + kf_K * kf_y;                                      // updated state estimation
+            kf_P_x = (arma::eye(size(kf_P_x_pre)) - kf_K * kf_H) * kf_P_x_pre;  // update state estimation error covariance matrix
+
+            // Store Kalman filter results
+            d_rem_carr_phase_rad = kf_x(0);  // set a new carrier Phase estimation to the NCO
+            d_carrier_doppler_hz = kf_x(1);  // set a new carrier Doppler estimation to the NCO
+            if (kf_x.n_elem > 2)
+                {
+                    d_carrier_dopplerrate_hz2 = kf_x(2);
+                }
+            else
+                {
+                    d_carrier_dopplerrate_hz2 = 0;
+                }
+            d_carr_phase_sigma2 = kf_R_est(0, 0);
+
+            // ################## DLL ##########################################################
+            // New code Doppler frequency estimation based on carrier frequency estimation
+            d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
+            // DLL discriminator
+            code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]);  // [chips/Ti] early and late
+            // Code discriminator filter
+            code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips);  // [chips/second]
+            double T_chip_seconds = 1.0 / static_cast<double>(d_code_freq_chips);
+            double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
+            double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds);  // [seconds]
+
+            // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
+            // keep alignment parameters for the next input buffer
+            // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
+            double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
+            double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
+            d_current_prn_length_samples = static_cast<int>(round(K_blk_samples));  // round to a discrete number of samples
+
+            //################### NCO COMMANDS #################################################
+            // carrier phase step (NCO phase increment per sample) [rads/sample]
+            d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
+            // carrier phase accumulator
+            d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
+
+            //################### DLL COMMANDS #################################################
+            // code phase step (Code resampler phase increment per sample) [chips/sample]
+            d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
+            // remnant code phase [chips]
+            d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_current_prn_length_samples);  // rounding error < 1 sample
+            d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast<double>(d_fs_in));
+
+            // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
+            if (d_cn0_estimation_counter < FLAGS_cn0_samples)
+                {
+                    // fill buffer with prompt correlator output values
+                    d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1];  //prompt
+                    d_cn0_estimation_counter++;
+                }
+            else
+                {
+                    d_cn0_estimation_counter = 0;
+                    // Code lock indicator
+                    d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD);
+                    // Carrier lock indicator
+                    d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples);
+                    // Loss of lock detection
+                    if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
+                        {
+                            //if (d_channel == 1)
+                            //std::cout << "Carrier Lock Test Fail in channel " << d_channel << ": " << d_carrier_lock_test << " < " << d_carrier_lock_threshold << "," << nfail++ << std::endl;
+                            d_carrier_lock_fail_counter++;
+                            //nfail++;
+                        }
+                    else
+                        {
+                            if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
+                        }
+                    if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
+                        {
+                            std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
+                            LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
+                            this->message_port_pub(pmt::mp("events"), pmt::from_long(3));  // 3 -> loss of lock
+                            d_carrier_lock_fail_counter = 0;
+                            d_enable_tracking = false;
+                        }
+                }
+            // ########### Output the tracking data to navigation and PVT ##########
+            current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());
+            current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
+            current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples);
+            current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
+            current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
+            current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+            current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
+            current_synchro_data.Flag_valid_symbol_output = true;
+            current_synchro_data.correlation_length_ms = 1;
+
+            kf_iter++;
+        }
+    else
+        {
+            for (int32_t n = 0; n < d_n_correlator_taps; n++)
+                {
+                    d_correlator_outs[n] = gr_complex(0, 0);
+                }
+
+            current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples);
+            current_synchro_data.System = {'G'};
+            current_synchro_data.correlation_length_ms = 1;
+        }
+
+    // assign the GNU Radio block output data
+    current_synchro_data.fs = d_fs_in;
+    *out[0] = current_synchro_data;
+
+    if (d_dump)
+        {
+            // MULTIPLEXED FILE RECORDING - Record results to file
+            float prompt_I;
+            float prompt_Q;
+            float tmp_E, tmp_P, tmp_L;
+            float tmp_VE = 0.0;
+            float tmp_VL = 0.0;
+            float tmp_float;
+            double tmp_double;
+            prompt_I = d_correlator_outs[1].real();
+            prompt_Q = d_correlator_outs[1].imag();
+            tmp_E = std::abs<float>(d_correlator_outs[0]);
+            tmp_P = std::abs<float>(d_correlator_outs[1]);
+            tmp_L = std::abs<float>(d_correlator_outs[2]);
+            try
+                {
+                    // EPR
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_VE), sizeof(float));
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_E), sizeof(float));
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_P), sizeof(float));
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_L), sizeof(float));
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_VL), sizeof(float));
+                    // PROMPT I and Q (to analyze navigation symbols)
+                    d_dump_file.write(reinterpret_cast<char *>(&prompt_I), sizeof(float));
+                    d_dump_file.write(reinterpret_cast<char *>(&prompt_Q), sizeof(float));
+                    // PRN start sample stamp
+                    d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(uint64_t));
+                    // accumulated carrier phase
+                    tmp_float = d_acc_carrier_phase_rad;
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    // carrier and code frequency
+                    tmp_float = d_carrier_doppler_hz;
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    tmp_float = d_carrier_dopplerrate_hz2;
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    tmp_float = d_code_freq_chips;
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    // Kalman commands
+                    tmp_float = static_cast<float>(d_carr_phase_error_rad * GPS_TWO_PI);
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    tmp_float = static_cast<float>(d_carr_phase_sigma2);
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    tmp_float = static_cast<float>(d_rem_carr_phase_rad * GPS_TWO_PI);
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    // DLL commands
+                    tmp_float = code_error_chips;
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    tmp_float = code_error_filt_chips;
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    // CN0 and carrier lock test
+                    tmp_float = d_CN0_SNV_dB_Hz;
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    tmp_float = d_carrier_lock_test;
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    // AUX vars (for debug purposes)
+                    tmp_float = d_rem_code_phase_samples;
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    tmp_double = static_cast<double>(d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples));
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
+                    // PRN
+                    uint32_t prn_ = d_acquisition_gnss_synchro->PRN;
+                    d_dump_file.write(reinterpret_cast<char *>(&prn_), sizeof(uint32_t));
+                }
+            catch (const std::ifstream::failure &e)
+                {
+                    LOG(WARNING) << "Exception writing trk dump file " << e.what();
+                }
+        }
+
+    consume_each(d_current_prn_length_samples);        // this is necessary in gr::block derivates
+    d_sample_counter += d_current_prn_length_samples;  // count for the processed samples
+    return 1;                                          // output tracking result ALWAYS even in the case of d_enable_tracking==false
+}

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h

@@ -0,0 +1,221 @@
+/*!
+ * \file gps_l1_ca_kf_tracking_cc.cc
+ * \brief Interface of a processing block of a DLL + Kalman carrier
+ * tracking loop for GPS L1 C/A signals
+ * \author Javier Arribas, 2018. jarribas(at)cttc.es
+ * \author Jordi Vila-Valls 2018. jvila(at)cttc.es
+ * \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
+ *
+ * Reference:
+ * J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades,
+ * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
+ * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
+ * Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2018  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H
+#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H
+
+#include "gnss_synchro.h"
+#include "tracking_2nd_DLL_filter.h"
+#include "tracking_2nd_PLL_filter.h"
+#include "cpu_multicorrelator_real_codes.h"
+#include "bayesian_estimation.h"
+#include <armadillo>
+#include <gnuradio/block.h>
+#include <fstream>
+#include <map>
+#include <string>
+
+class Gps_L1_Ca_Kf_Tracking_cc;
+
+typedef boost::shared_ptr<Gps_L1_Ca_Kf_Tracking_cc>
+    gps_l1_ca_kf_tracking_cc_sptr;
+
+gps_l1_ca_kf_tracking_cc_sptr
+gps_l1_ca_kf_make_tracking_cc(uint32_t order,
+    int64_t if_freq,
+    int64_t fs_in, uint32_t vector_length,
+    bool dump,
+    std::string dump_filename,
+    float pll_bw_hz,
+    float early_late_space_chips,
+    bool bce_run,
+    uint32_t bce_ptrans,
+    uint32_t bce_strans,
+    int32_t bce_nu,
+    int32_t bce_kappa);
+
+
+/*!
+ * \brief This class implements a DLL + PLL tracking loop block
+ */
+class Gps_L1_Ca_Kf_Tracking_cc : public gr::block
+{
+public:
+    ~Gps_L1_Ca_Kf_Tracking_cc();
+
+    void set_channel(uint32_t channel);
+    void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
+    void start_tracking();
+
+    int general_work(int noutput_items, gr_vector_int& ninput_items,
+        gr_vector_const_void_star& input_items, gr_vector_void_star& output_items);
+
+    void forecast(int noutput_items, gr_vector_int& ninput_items_required);
+
+private:
+    friend gps_l1_ca_kf_tracking_cc_sptr
+    gps_l1_ca_kf_make_tracking_cc(uint32_t order,
+        int64_t if_freq,
+        int64_t fs_in, uint32_t vector_length,
+        bool dump,
+        std::string dump_filename,
+        float dll_bw_hz,
+        float early_late_space_chips,
+        bool bce_run,
+        uint32_t bce_ptrans,
+        uint32_t bce_strans,
+        int32_t bce_nu,
+        int32_t bce_kappa);
+
+    Gps_L1_Ca_Kf_Tracking_cc(uint32_t order,
+        int64_t if_freq,
+        int64_t fs_in, uint32_t vector_length,
+        bool dump,
+        std::string dump_filename,
+        float dll_bw_hz,
+        float early_late_space_chips,
+        bool bce_run,
+        uint32_t bce_ptrans,
+        uint32_t bce_strans,
+        int32_t bce_nu,
+        int32_t bce_kappa);
+
+    // tracking configuration vars
+    uint32_t d_order;
+    uint32_t d_vector_length;
+    bool d_dump;
+
+    Gnss_Synchro* d_acquisition_gnss_synchro;
+    uint32_t d_channel;
+
+    int64_t d_if_freq;
+    int64_t d_fs_in;
+
+    double d_early_late_spc_chips;
+
+    // remaining code phase and carrier phase between tracking loops
+    double d_rem_code_phase_samples;
+    double d_rem_code_phase_chips;
+    double d_rem_carr_phase_rad;
+
+    // Kalman filter variables
+    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_pre;  // Predicted state error covariance matrix
+    arma::mat kf_P_y;      // innovation covariance matrix
+
+    arma::mat kf_F;  // state transition matrix
+    arma::mat kf_H;  // system matrix
+    arma::mat kf_R;  // measurement error covariance matrix
+    arma::mat kf_Q;  // system error covariance matrix
+
+    arma::colvec kf_x;      // state vector
+    arma::colvec kf_x_pre;  // predicted state vector
+    arma::colvec kf_y;      // measurement vector
+    arma::mat kf_K;         // Kalman gain matrix
+
+    // Bayesian estimator
+    Bayesian_estimator bayes_estimator;
+    arma::mat kf_R_est;  // measurement error covariance
+    uint32_t bayes_ptrans;
+    uint32_t bayes_strans;
+    int32_t bayes_nu;
+    int32_t bayes_kappa;
+
+    bool bayes_run;
+    uint32_t kf_iter;
+
+    // PLL and DLL filter library
+    Tracking_2nd_DLL_filter d_code_loop_filter;
+    // Tracking_2nd_PLL_filter d_carrier_loop_filter;
+
+    // acquisition
+    double d_acq_carrier_doppler_step_hz;
+    double d_acq_code_phase_samples;
+    double d_acq_carrier_doppler_hz;
+    // correlator
+    int32_t d_n_correlator_taps;
+    float* d_ca_code;
+    float* d_local_code_shift_chips;
+    gr_complex* d_correlator_outs;
+    cpu_multicorrelator_real_codes multicorrelator_cpu;
+
+    // tracking vars
+    double d_code_freq_chips;
+    double d_code_phase_step_chips;
+    double d_carrier_doppler_hz;
+    double d_carrier_dopplerrate_hz2;
+    double d_carrier_phase_step_rad;
+    double d_acc_carrier_phase_rad;
+    double d_carr_phase_error_rad;
+    double d_carr_phase_sigma2;
+    double d_code_phase_samples;
+    double code_error_chips;
+    double code_error_filt_chips;
+
+    // PRN period in samples
+    int32_t d_current_prn_length_samples;
+
+    // processing samples counters
+    uint64_t d_sample_counter;
+    uint64_t d_acq_sample_stamp;
+
+    // CN0 estimation and lock detector
+    int32_t d_cn0_estimation_counter;
+    gr_complex* d_Prompt_buffer;
+    double d_carrier_lock_test;
+    double d_CN0_SNV_dB_Hz;
+    double d_carrier_lock_threshold;
+    int32_t d_carrier_lock_fail_counter;
+
+    // control vars
+    bool d_enable_tracking;
+    bool d_pull_in;
+
+    // file dump
+    std::string d_dump_filename;
+    std::ofstream d_dump_file;
+
+    std::map<std::string, std::string> systemName;
+    std::string sys;
+
+    int32_t save_matfile();
+};
+
+#endif  // GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H

src/algorithms/tracking/libs/CMakeLists.txt

@@ -44,6 +44,7 @@ set(TRACKING_LIB_SOURCES
      tracking_FLL_PLL_filter.cc
      tracking_loop_filter.cc
      dll_pll_conf.cc
+     bayesian_estimation.cc
 )
 
 if(ENABLE_FPGA)
@@ -56,6 +57,7 @@ include_directories(
      ${CMAKE_SOURCE_DIR}/src/core/interfaces
      ${CMAKE_SOURCE_DIR}/src/core/receiver
      ${CMAKE_SOURCE_DIR}/src/algorithms/libs
+     ${ARMADILLO_INCLUDE_DIRS}
      ${VOLK_INCLUDE_DIRS}
      ${GLOG_INCLUDE_DIRS}
      ${GFlags_INCLUDE_DIRS}

src/algorithms/tracking/libs/bayesian_estimation.cc

@@ -0,0 +1,187 @@
+/*!
+ * \file bayesian_estimation.cc
+ * \brief Interface of a library with Bayesian noise statistic estimation
+ *
+ * Bayesian_estimator is a Bayesian estimator which attempts to estimate
+ * the properties of a stochastic process based on a sequence of
+ * discrete samples of the sequence.
+ *
+ * [1] TODO: Refs
+ *
+ * \authors <ul>
+ *          <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu
+ *          <li> Jordi Vila-Valls 2018. jvila(at)cttc.es
+ *          </ul>
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2018  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "bayesian_estimation.h"
+
+
+Bayesian_estimator::Bayesian_estimator()
+{
+    int ny = 1;
+    mu_prior = arma::zeros(ny, 1);
+    kappa_prior = 0;
+    nu_prior = 0;
+    Psi_prior = arma::eye(ny, ny) * (nu_prior + ny + 1);
+
+    mu_est = mu_prior;
+    Psi_est = Psi_prior;
+}
+
+Bayesian_estimator::Bayesian_estimator(int ny)
+{
+    mu_prior = arma::zeros(ny, 1);
+    kappa_prior = 0;
+    nu_prior = 0;
+    Psi_prior = arma::eye(ny, ny) * (nu_prior + ny + 1);
+
+    mu_est = mu_prior;
+    Psi_est = Psi_prior;
+}
+
+Bayesian_estimator::Bayesian_estimator(const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0)
+{
+    mu_prior = mu_prior_0;
+    kappa_prior = kappa_prior_0;
+    nu_prior = nu_prior_0;
+    Psi_prior = Psi_prior_0;
+
+    mu_est = mu_prior;
+    Psi_est = Psi_prior;
+}
+
+Bayesian_estimator::~Bayesian_estimator()
+{
+}
+
+void Bayesian_estimator::init(const arma::mat& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0)
+{
+    mu_prior = mu_prior_0;
+    kappa_prior = kappa_prior_0;
+    nu_prior = nu_prior_0;
+    Psi_prior = Psi_prior_0;
+
+    mu_est = mu_prior;
+    Psi_est = Psi_prior;
+}
+
+/*
+ * Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in
+ * the class structure, and update the priors according to the computed posteriors
+ */
+void Bayesian_estimator::update_sequential(const arma::vec& data)
+{
+    int K = data.n_cols;
+    int ny = data.n_rows;
+
+    if (mu_prior.is_empty())
+        {
+            mu_prior = arma::zeros(ny, 1);
+        }
+
+    if (Psi_prior.is_empty())
+        {
+            Psi_prior = arma::zeros(ny, ny);
+        }
+
+    arma::vec y_mean = arma::mean(data, 1);
+    arma::mat Psi_N = arma::zeros(ny, ny);
+
+    for (int kk = 0; kk < K; kk++)
+        {
+            Psi_N = Psi_N + (data.col(kk) - y_mean) * ((data.col(kk) - y_mean).t());
+        }
+
+    arma::vec mu_posterior = (kappa_prior * mu_prior + K * y_mean) / (kappa_prior + K);
+    int kappa_posterior = kappa_prior + K;
+    int nu_posterior = nu_prior + K;
+    arma::mat Psi_posterior = Psi_prior + Psi_N + (kappa_prior * K) / (kappa_prior + K) * (y_mean - mu_prior) * ((y_mean - mu_prior).t());
+
+    mu_est = mu_posterior;
+    if ((nu_posterior - ny - 1) > 0)
+        {
+            Psi_est = Psi_posterior / (nu_posterior - ny - 1);
+        }
+    else
+        {
+            Psi_est = Psi_posterior / (nu_posterior + ny + 1);
+        }
+
+    mu_prior = mu_posterior;
+    kappa_prior = kappa_posterior;
+    nu_prior = nu_posterior;
+    Psi_prior = Psi_posterior;
+}
+
+
+/*
+ * Perform Bayesian noise estimation using a new set of normal-inverse-Wishart priors
+ * and update the priors according to the computed posteriors
+ */
+void Bayesian_estimator::update_sequential(const arma::vec& data, const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0)
+{
+    int K = data.n_cols;
+    int ny = data.n_rows;
+
+    arma::vec y_mean = arma::mean(data, 1);
+    arma::mat Psi_N = arma::zeros(ny, ny);
+
+    for (int kk = 0; kk < K; kk++)
+        {
+            Psi_N = Psi_N + (data.col(kk) - y_mean) * ((data.col(kk) - y_mean).t());
+        }
+
+    arma::vec mu_posterior = (kappa_prior_0 * mu_prior_0 + K * y_mean) / (kappa_prior_0 + K);
+    int kappa_posterior = kappa_prior_0 + K;
+    int nu_posterior = nu_prior_0 + K;
+    arma::mat Psi_posterior = Psi_prior_0 + Psi_N + (kappa_prior_0 * K) / (kappa_prior_0 + K) * (y_mean - mu_prior_0) * ((y_mean - mu_prior_0).t());
+
+    mu_est = mu_posterior;
+    if ((nu_posterior - ny - 1) > 0)
+        {
+            Psi_est = Psi_posterior / (nu_posterior - ny - 1);
+        }
+    else
+        {
+            Psi_est = Psi_posterior / (nu_posterior + ny + 1);
+        }
+
+    mu_prior = mu_posterior;
+    kappa_prior = kappa_posterior;
+    nu_prior = nu_posterior;
+    Psi_prior = Psi_posterior;
+}
+
+arma::mat Bayesian_estimator::get_mu_est() const
+{
+    return mu_est;
+}
+
+arma::mat Bayesian_estimator::get_Psi_est() const
+{
+    return Psi_est;
+}

src/algorithms/tracking/libs/bayesian_estimation.h

@@ -0,0 +1,85 @@
+/*!
+ * \file bayesian_estimation.h
+ * \brief Interface of a library with Bayesian noise statistic estimation
+ *
+ * Bayesian_estimator is a Bayesian estimator which attempts to estimate
+ * the properties of a stochastic process based on a sequence of
+ * discrete samples of the sequence.
+ *
+ * [1] TODO: Refs
+ *
+ * \authors <ul>
+ *          <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu
+ *          <li> Jordi Vila-Valls 2018. jvila(at)cttc.es
+ *          </ul>
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2018  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_BAYESIAN_ESTIMATION_H_
+#define GNSS_SDR_BAYESIAN_ESTIMATION_H_
+
+#include <gnuradio/gr_complex.h>
+#include <armadillo>
+
+/*! \brief Bayesian_estimator is an estimator of noise characteristics (i.e. mean, covariance)
+ *
+ * Bayesian_estimator is an estimator which performs estimation of noise characteristics from
+ * a sequence of identically and independently distributed (IID) samples of a stationary
+ * stochastic process by way of Bayesian inference using conjugate priors. The posterior
+ * distribution is assumed to be Gaussian with mean \mathbf{\mu} and covariance \hat{\mathbf{C}},
+ * which has a conjugate prior given by a normal-inverse-Wishart distribution with paramemters
+ * \mathbf{\mu}_{0}, \kappa_{0}, \nu_{0}, and \mathbf{\Psi}.
+ *
+ * [1] TODO: Ref1
+ *
+ */
+
+class Bayesian_estimator
+{
+public:
+    Bayesian_estimator();
+    Bayesian_estimator(int ny);
+    Bayesian_estimator(const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0);
+    ~Bayesian_estimator();
+
+    void init(const arma::mat& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0);
+
+    void update_sequential(const arma::vec& data);
+    void update_sequential(const arma::vec& data, const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0);
+
+    arma::mat get_mu_est() const;
+    arma::mat get_Psi_est() const;
+
+private:
+    arma::vec mu_est;
+    arma::mat Psi_est;
+
+    arma::vec mu_prior;
+    int kappa_prior;
+    int nu_prior;
+    arma::mat Psi_prior;
+};
+
+#endif