Add moving average parameter in observables

src/algorithms/PVT/adapters/hybrid_pvt.cc

@@ -177,4 +177,3 @@ gr::basic_block_sptr HybridPvt::get_right_block()
 {
     return pvt_; // this is a sink, nothing downstream
 }
-

src/algorithms/PVT/libs/hybrid_ls_pvt.cc

@@ -82,6 +82,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
     std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
     std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
     std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
+    std::map<int,Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
+
     int valid_pseudoranges = gnss_pseudoranges_map.size();
 
     arma::mat W = arma::eye(valid_pseudoranges, valid_pseudoranges); // channels weights matrix
@@ -317,4 +319,3 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
         }
     return b_valid_position;
 }
-

src/algorithms/PVT/libs/hybrid_ls_pvt.h

@@ -39,11 +39,12 @@
 #include "ls_pvt.h"
 #include "galileo_navigation_message.h"
 #include "gps_navigation_message.h"
+#include "gps_cnav_navigation_message.h"
 #include "gnss_synchro.h"
-#include "galileo_ephemeris.h"
+//#include "galileo_ephemeris.h"
-#include "galileo_utc_model.h"
+//#include "galileo_utc_model.h"
-#include "gps_ephemeris.h"
+//#include "gps_ephemeris.h"
-#include "gps_utc_model.h"
+//#include "gps_utc_model.h"
 
 
 /*!
@@ -64,7 +65,8 @@ public:
     Gps_Navigation_Message* d_GPS_ephemeris;
 
     std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
-    std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new Galileo_Ephemeris
+    std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
+    std::map<int,Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map; 
     Galileo_Utc_Model galileo_utc_model;
     Galileo_Iono galileo_iono;
     Galileo_Almanac galileo_almanac;

src/algorithms/observables/adapters/galileo_e1_observables.cc

@@ -31,8 +31,9 @@
 
 
 #include "galileo_e1_observables.h"
-#include "configuration_interface.h"
 #include <glog/logging.h>
+#include "configuration_interface.h"
+#include "Galileo_E1.h"
 
 using google::LogMessage;
 
@@ -48,7 +49,8 @@ GalileoE1Observables::GalileoE1Observables(ConfigurationInterface* configuration
     DLOG(INFO) << "role " << role;
     dump_ = configuration->property(role + ".dump", false);
     dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
-    observables_ = galileo_e1_make_observables_cc(in_streams_, dump_, dump_filename_);
+    unsigned int history_deep = configuration->property(role + ".averaging_depth", GALILEO_E1_HISTORY_DEEP);
+    observables_ = galileo_e1_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep);
     DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")";
 }
 
@@ -91,4 +93,3 @@ gr::basic_block_sptr GalileoE1Observables::get_right_block()
 {
     return observables_;
 }
-

src/algorithms/observables/adapters/gps_l1_ca_observables.cc

@@ -33,6 +33,7 @@
 #include "gps_l1_ca_observables.h"
 #include "configuration_interface.h"
 #include <glog/logging.h>
+#include "GPS_L1_CA.h"
 
 using google::LogMessage;
 
@@ -48,7 +49,8 @@ GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration,
     DLOG(INFO) << "role " << role;
     dump_ = configuration->property(role + ".dump", false);
     dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
-    observables_ = gps_l1_ca_make_observables_cc(in_streams_, dump_, dump_filename_);
+    unsigned int history_deep = configuration->property(role + ".averaging_depth", GPS_L1_CA_HISTORY_DEEP);
+    observables_ = gps_l1_ca_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep);
     DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")";
 }
 
@@ -91,4 +93,3 @@ gr::basic_block_sptr GpsL1CaObservables::get_right_block()
 {
     return observables_;
 }
-

src/algorithms/observables/adapters/hybrid_observables.cc

@@ -33,6 +33,8 @@
 #include "hybrid_observables.h"
 #include "configuration_interface.h"
 #include <glog/logging.h>
+#include "GPS_L1_CA.h"
+#include "Galileo_E1.h"
 
 using google::LogMessage;
 
@@ -48,7 +50,17 @@ HybridObservables::HybridObservables(ConfigurationInterface* configuration,
     DLOG(INFO) << "role " << role;
     dump_ = configuration->property(role + ".dump", false);
     dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
-    observables_ = hybrid_make_observables_cc(in_streams_, dump_, dump_filename_);
+    unsigned int default_depth = 0;
+    if (GPS_L1_CA_HISTORY_DEEP == GALILEO_E1_HISTORY_DEEP)
+    {
+      default_depth = GPS_L1_CA_HISTORY_DEEP;
+    }
+    else
+    {
+      default_depth = 100;
+    }
+    unsigned int history_deep = configuration->property(role + ".averaging_depth", default_depth);
+    observables_ = hybrid_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep);
     DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")";
 }
 
@@ -91,4 +103,3 @@ gr::basic_block_sptr HybridObservables::get_right_block()
 {
     return observables_;
 }
-

src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc

@@ -50,13 +50,13 @@ using google::LogMessage;
 
 
 galileo_e1_observables_cc_sptr
-galileo_e1_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename)
+galileo_e1_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history)
 {
-    return galileo_e1_observables_cc_sptr(new galileo_e1_observables_cc(nchannels, dump, dump_filename));
+    return galileo_e1_observables_cc_sptr(new galileo_e1_observables_cc(nchannels, dump, dump_filename, deep_history));
 }
 
 
-galileo_e1_observables_cc::galileo_e1_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename) :
+galileo_e1_observables_cc::galileo_e1_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) :
      gr::block("galileo_e1_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
      gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)))
 {
@@ -64,6 +64,7 @@ galileo_e1_observables_cc::galileo_e1_observables_cc(unsigned int nchannels, boo
     d_dump = dump;
     d_nchannels = nchannels;
     d_dump_filename = dump_filename;
+    history_deep = deep_history;
 
     for (unsigned int i = 0; i < d_nchannels; i++)
         {
@@ -152,15 +153,15 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
                     // save TOW history
                     d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol);
 
-                    if (d_carrier_doppler_queue_hz[i].size() > GALILEO_E1_HISTORY_DEEP)
+                    if (d_carrier_doppler_queue_hz[i].size() > history_deep)
                         {
                             d_carrier_doppler_queue_hz[i].pop_front();
                         }
-                    if (d_acc_carrier_phase_queue_rads[i].size() > GALILEO_E1_HISTORY_DEEP)
+                    if (d_acc_carrier_phase_queue_rads[i].size() > history_deep)
                         {
                             d_acc_carrier_phase_queue_rads[i].pop_front();
                         }
-                    if (d_symbol_TOW_queue_s[i].size() > GALILEO_E1_HISTORY_DEEP)
+                    if (d_symbol_TOW_queue_s[i].size() > history_deep)
                         {
                             d_symbol_TOW_queue_s[i].pop_front();
                         }
@@ -215,15 +216,15 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
                     current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
                     current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference * 1000.0) / 1000.0 + GALILEO_STARTOFFSET_ms / 1000.0;
 
-                    if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= GALILEO_E1_HISTORY_DEEP)
+                    if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep)
                         {
                             // compute interpolated observation values for Doppler and Accumulate carrier phase
                             symbol_TOW_vec_s = arma::vec(std::vector<double>(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end()));
                             acc_phase_vec_rads = arma::vec(std::vector<double>(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end()));
                             dopper_vec_hz = arma::vec(std::vector<double>(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end()));
-                            desired_symbol_TOW[0] = symbol_TOW_vec_s[GALILEO_E1_HISTORY_DEEP - 1] + delta_rx_time_ms / 1000.0;
+                            desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_rx_time_ms / 1000.0;
                             // Curve fitting to cuadratic function
-                            arma::mat A = arma::ones<arma::mat>(GALILEO_E1_HISTORY_DEEP, 2);
+                            arma::mat A = arma::ones<arma::mat>(history_deep, 2);
                             A.col(1) = symbol_TOW_vec_s;
                             //A.col(2)=symbol_TOW_vec_s % symbol_TOW_vec_s;
                             arma::mat coef_acc_phase(1,3);
@@ -278,4 +279,3 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
         }
     return 1;
 }
-

src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h

@@ -44,7 +44,7 @@ class galileo_e1_observables_cc;
 typedef boost::shared_ptr<galileo_e1_observables_cc> galileo_e1_observables_cc_sptr;
 
 galileo_e1_observables_cc_sptr
-galileo_e1_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename);
+galileo_e1_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename, unsigned int deep_history);
 
 /*!
  * \brief This class implements a block that computes Galileo observables
@@ -59,8 +59,8 @@ public:
 
 private:
     friend galileo_e1_observables_cc_sptr
-    galileo_e1_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename);
+    galileo_e1_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history);
-    galileo_e1_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename);
+    galileo_e1_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history);
 
     //Tracking observable history
     std::vector<std::deque<double>> d_acc_carrier_phase_queue_rads;
@@ -70,6 +70,7 @@ private:
     // class private vars
     bool d_dump;
     unsigned int d_nchannels;
+    unsigned int history_deep;
     std::string d_dump_filename;
     std::ofstream d_dump_file;
 };

src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc

@@ -47,13 +47,13 @@ using google::LogMessage;
 
 
 gps_l1_ca_observables_cc_sptr
-gps_l1_ca_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename)
+gps_l1_ca_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history)
 {
-    return gps_l1_ca_observables_cc_sptr(new gps_l1_ca_observables_cc(nchannels, dump, dump_filename));
+    return gps_l1_ca_observables_cc_sptr(new gps_l1_ca_observables_cc(nchannels, dump, dump_filename, deep_history));
 }
 
 
-gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename) :
+gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) :
                                 gr::block("gps_l1_ca_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
                                 gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)))
 {
@@ -61,6 +61,7 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, bool
     d_dump = dump;
     d_nchannels = nchannels;
     d_dump_filename = dump_filename;
+    history_deep = deep_history;
 
     for (unsigned int i = 0; i < d_nchannels; i++)
         {
@@ -146,15 +147,15 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
                     // save TOW history
                     d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol);
 
-                    if (d_carrier_doppler_queue_hz[i].size() > GPS_L1_CA_HISTORY_DEEP)
+                    if (d_carrier_doppler_queue_hz[i].size() > history_deep)
                         {
                             d_carrier_doppler_queue_hz[i].pop_front();
                         }
-                    if (d_acc_carrier_phase_queue_rads[i].size() > GPS_L1_CA_HISTORY_DEEP)
+                    if (d_acc_carrier_phase_queue_rads[i].size() > history_deep)
                         {
                             d_acc_carrier_phase_queue_rads[i].pop_front();
                         }
-                    if (d_symbol_TOW_queue_s[i].size() > GPS_L1_CA_HISTORY_DEEP)
+                    if (d_symbol_TOW_queue_s[i].size() > history_deep)
                         {
                             d_symbol_TOW_queue_s[i].pop_front();
                         }
@@ -209,19 +210,19 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
                     current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
                     current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000.0)/1000.0 + GPS_STARTOFFSET_ms/1000.0;
 
-                    if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size()>=GPS_L1_CA_HISTORY_DEEP)
+                    if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep)
                         {
                             // compute interpolated observation values for Doppler and Accumulate carrier phase
                             symbol_TOW_vec_s = arma::vec(std::vector<double>(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end()));
                             acc_phase_vec_rads = arma::vec(std::vector<double>(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end()));
                             dopper_vec_hz = arma::vec(std::vector<double>(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end()));
 
-                            desired_symbol_TOW[0] = symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP - 1] + delta_rx_time_ms / 1000.0;
+                            desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_rx_time_ms / 1000.0;
                             //    arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz);
                             //    arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads);
 
                             // Curve fitting to cuadratic function
-                            arma::mat A = arma::ones<arma::mat> (GPS_L1_CA_HISTORY_DEEP, 2);
+                            arma::mat A = arma::ones<arma::mat> (history_deep, 2);
                             A.col(1) = symbol_TOW_vec_s;
 
                             arma::mat coef_acc_phase(1,3);
@@ -284,4 +285,3 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
         }
     return 1;
 }
-

src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h

@@ -44,7 +44,7 @@ class gps_l1_ca_observables_cc;
 typedef boost::shared_ptr<gps_l1_ca_observables_cc> gps_l1_ca_observables_cc_sptr;
 
 gps_l1_ca_observables_cc_sptr
-gps_l1_ca_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename);
+gps_l1_ca_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename, unsigned int deep_history);
 
 /*!
  * \brief This class implements a block that computes GPS L1 C/A observables
@@ -59,8 +59,8 @@ public:
 
 private:
     friend gps_l1_ca_observables_cc_sptr
-    gps_l1_ca_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename);
+    gps_l1_ca_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history);
-    gps_l1_ca_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename);
+    gps_l1_ca_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history);
 
 
     //Tracking observable history
@@ -71,6 +71,7 @@ private:
     // class private vars
     bool d_dump;
     unsigned int d_nchannels;
+    unsigned int history_deep;
     std::string d_dump_filename;
     std::ofstream d_dump_file;
 };

src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc

@@ -36,6 +36,7 @@
 #include <map>
 #include <utility>
 #include <vector>
+#include <armadillo>
 #include <gnuradio/io_signature.h>
 #include <glog/logging.h>
 #include "gnss_synchro.h"
@@ -48,13 +49,13 @@ using google::LogMessage;
 
 
 hybrid_observables_cc_sptr
-hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename)
+hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history)
 {
-    return hybrid_observables_cc_sptr(new hybrid_observables_cc(nchannels, dump, dump_filename));
+    return hybrid_observables_cc_sptr(new hybrid_observables_cc(nchannels, dump, dump_filename, deep_history));
 }
 
 
-hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename) :
+hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) :
                                 gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
                                 gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)))
 {
@@ -62,6 +63,14 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump,
     d_dump = dump;
     d_nchannels = nchannels;
     d_dump_filename = dump_filename;
+    history_deep = deep_history;
+
+    for (unsigned int i = 0; i < d_nchannels; i++)
+        {
+            d_acc_carrier_phase_queue_rads.push_back(std::deque<double>(d_nchannels));
+            d_carrier_doppler_queue_hz.push_back(std::deque<double>(d_nchannels));
+            d_symbol_TOW_queue_s.push_back(std::deque<double>(d_nchannels));
+        }
 
     // ############# ENABLE DATA FILE LOG #################
     if (d_dump == true)
@@ -91,10 +100,10 @@ hybrid_observables_cc::~hybrid_observables_cc()
 
 
 
-bool Hybrid_pairCompare_gnss_synchro_Prn_delay_ms(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b)
+//bool Hybrid_pairCompare_gnss_synchro_Prn_delay_ms(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b)
-{
+//{
-    return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms);
+//    return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms);
-}
+//}
 
 
 bool Hybrid_pairCompare_gnss_synchro_d_TOW_hybrid_at_current_symbol(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b)
@@ -108,15 +117,17 @@ bool Hybrid_pairCompare_gnss_synchro_d_TOW_at_current_symbol(const std::pair<int
 }
 
 
-int hybrid_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
+int hybrid_observables_cc::general_work (int noutput_items,
-        gr_vector_const_void_star &input_items,    gr_vector_void_star &output_items)
+    gr_vector_int &ninput_items,
+    gr_vector_const_void_star &input_items,
+    gr_vector_void_star &output_items)
 {
     Gnss_Synchro **in = (Gnss_Synchro **)  &input_items[0];   // Get the input pointer
     Gnss_Synchro **out = (Gnss_Synchro **)  &output_items[0]; // Get the output pointer
 
     Gnss_Synchro current_gnss_synchro[d_nchannels];
     std::map<int,Gnss_Synchro> current_gnss_synchro_map;
-    std::map<int,Gnss_Synchro> current_gnss_synchro_map_gps_only;
+    //std::map<int,Gnss_Synchro> current_gnss_synchro_map_gps_only;
     std::map<int,Gnss_Synchro>::iterator gnss_synchro_iter;
 
     if (d_nchannels != ninput_items.size())
@@ -140,9 +151,36 @@ int hybrid_observables_cc::general_work (int noutput_items, gr_vector_int &ninpu
                 {
                     //record the word structure in a map for pseudorange computation
                     current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
-                    if (current_gnss_synchro[i].System == 'G')
+    //                if (current_gnss_synchro[i].System == 'G')
+    //                    {
+    //                        current_gnss_synchro_map_gps_only.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
+    //                    }
+                    //################### SAVE DOPPLER AND ACC CARRIER PHASE HISTORIC DATA FOR INTERPOLATION IN OBSERVABLE MODULE #######
+                    d_carrier_doppler_queue_hz[i].push_back(current_gnss_synchro[i].Carrier_Doppler_hz);
+                    d_acc_carrier_phase_queue_rads[i].push_back(current_gnss_synchro[i].Carrier_phase_rads);
+                    // save TOW history
+                    d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol);
+                    if (d_carrier_doppler_queue_hz[i].size() > history_deep)
                                   {
-                            current_gnss_synchro_map_gps_only.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
+                                      d_carrier_doppler_queue_hz[i].pop_front();
+                                  }
+                    if (d_acc_carrier_phase_queue_rads[i].size() > history_deep)
+                                  {
+                                      d_acc_carrier_phase_queue_rads[i].pop_front();
+                                  }
+                    if (d_symbol_TOW_queue_s[i].size() > history_deep)
+                                  {
+                                      d_symbol_TOW_queue_s[i].pop_front();
+                                  }
+                }
+                else
+                {
+                  // Clear the observables history for this channel
+                        if (d_symbol_TOW_queue_s[i].size() > 0)
+                            {
+                                d_symbol_TOW_queue_s[i].clear();
+                                d_carrier_doppler_queue_hz[i].clear();
+                                d_acc_carrier_phase_queue_rads[i].clear();
                             }
                 }
         }
@@ -172,14 +210,28 @@ int hybrid_observables_cc::general_work (int noutput_items, gr_vector_int &ninpu
             double pseudorange_m;
             double delta_rx_time_ms;
             double delta_TOW_ms;
+            arma::vec symbol_TOW_vec_s;
+            arma::vec dopper_vec_hz;
+            arma::vec dopper_vec_interp_hz;
+            arma::vec acc_phase_vec_rads;
+            arma::vec acc_phase_vec_interp_rads;
+            arma::vec desired_symbol_TOW(1);
+            double start_offset_ms = 0.0;
             for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
                 {
                     // check and correct synchronization in cross-system pseudoranges!
                     delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms;
                     delta_TOW_ms = (d_TOW_reference - gnss_synchro_iter->second.d_TOW_hybrid_at_current_symbol) * 1000.0;
-
+                    if(gnss_synchro_iter->second.System == 'E')
+                    {
+                        start_offset_ms = GALILEO_STARTOFFSET_ms;
+                    }
+                    if(gnss_synchro_iter->second.System == 'G')
+                    {
+                        start_offset_ms = GPS_STARTOFFSET_ms;
+                    }
                     //compute the pseudorange
-                    traveltime_ms =  delta_TOW_ms + delta_rx_time_ms + GALILEO_STARTOFFSET_ms;
+                    traveltime_ms = delta_TOW_ms + delta_rx_time_ms + start_offset_ms;
                     pseudorange_m = traveltime_ms * GALILEO_C_m_ms; // [m]
                     DLOG(INFO) << "CH " << gnss_synchro_iter->second.Channel_ID << " tracking GNSS System "
                                << gnss_synchro_iter->second.System << " has PRN start at= " << gnss_synchro_iter->second.Prn_timestamp_ms
@@ -192,7 +244,36 @@ int hybrid_observables_cc::general_work (int noutput_items, gr_vector_int &ninpu
                     //current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second;
                     current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
                     current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
-                    current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_hybrid_at_current_symbol = round(d_TOW_reference * 1000) / 1000 + GALILEO_STARTOFFSET_ms / 1000.0;
+                    current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_hybrid_at_current_symbol = round(d_TOW_reference * 1000) / 1000 + GPS_STARTOFFSET_ms / 1000.0;
+                    if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep)
+                        {
+                            // compute interpolated observation values for Doppler and Accumulate carrier phase
+                            symbol_TOW_vec_s = arma::vec(std::vector<double>(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end()));
+                            acc_phase_vec_rads = arma::vec(std::vector<double>(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end()));
+                            dopper_vec_hz = arma::vec(std::vector<double>(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end()));
+
+                            desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_rx_time_ms / 1000.0;
+                            //    arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz);
+                            //    arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads);
+
+                            // Curve fitting to cuadratic function
+                            arma::mat A = arma::ones<arma::mat> (history_deep, 2);
+                            A.col(1) = symbol_TOW_vec_s;
+
+                            arma::mat coef_acc_phase(1,3);
+                            arma::mat pinv_A = arma::pinv(A.t() * A) * A.t();
+                            coef_acc_phase = pinv_A * acc_phase_vec_rads;
+                            arma::mat coef_doppler(1,3);
+                            coef_doppler = pinv_A * dopper_vec_hz;
+                            arma::vec acc_phase_lin;
+                            arma::vec carrier_doppler_lin;
+                            acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0];   // +coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
+                            carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0]; // +coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
+                            //std::cout<<"acc_phase_vec_interp_rads="<<acc_phase_vec_interp_rads[0]<<std::endl;
+                            //std::cout<<"dopper_vec_interp_hz="<<dopper_vec_interp_hz[0]<<std::endl;
+                            current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads = acc_phase_lin[0];
+                            current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz = carrier_doppler_lin[0];
+                        }
                 }
         }
 
@@ -237,4 +318,3 @@ int hybrid_observables_cc::general_work (int noutput_items, gr_vector_int &ninpu
         }
     return 1;
 }
-

src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h

@@ -43,7 +43,7 @@ class hybrid_observables_cc;
 typedef boost::shared_ptr<hybrid_observables_cc> hybrid_observables_cc_sptr;
 
 hybrid_observables_cc_sptr
-hybrid_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename);
+hybrid_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename, unsigned int deep_history);
 
 /*!
  * \brief This class implements a block that computes Galileo observables
@@ -57,12 +57,18 @@ public:
 
 private:
     friend hybrid_observables_cc_sptr
-    hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename);
+    hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history);
-    hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename);
+    hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history);
+
+    //Tracking observable history
+    std::vector<std::deque<double>> d_acc_carrier_phase_queue_rads;
+    std::vector<std::deque<double>> d_carrier_doppler_queue_hz;
+    std::vector<std::deque<double>> d_symbol_TOW_queue_s;
 
     // class private vars
     bool d_dump;
     unsigned int d_nchannels;
+    unsigned int history_deep;
     std::string d_dump_filename;
     std::ofstream d_dump_file;
 };