From fe6b2387f43f04bf76983b22a308b81170551b0d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 2 Nov 2016 17:35:40 +0100 Subject: [PATCH] Add moving average parameter in observables --- src/algorithms/PVT/adapters/hybrid_pvt.cc | 7 +- src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 3 +- src/algorithms/PVT/libs/hybrid_ls_pvt.h | 12 +- .../adapters/galileo_e1_observables.cc | 7 +- .../adapters/gps_l1_ca_observables.cc | 5 +- .../adapters/hybrid_observables.cc | 15 ++- .../galileo_e1_observables_cc.cc | 20 +-- .../galileo_e1_observables_cc.h | 7 +- .../gps_l1_ca_observables_cc.cc | 22 ++-- .../gps_l1_ca_observables_cc.h | 7 +- .../gnuradio_blocks/hybrid_observables_cc.cc | 116 +++++++++++++++--- .../gnuradio_blocks/hybrid_observables_cc.h | 12 +- 12 files changed, 168 insertions(+), 65 deletions(-) diff --git a/src/algorithms/PVT/adapters/hybrid_pvt.cc b/src/algorithms/PVT/adapters/hybrid_pvt.cc index af08e9a65..dcd202bc4 100644 --- a/src/algorithms/PVT/adapters/hybrid_pvt.cc +++ b/src/algorithms/PVT/adapters/hybrid_pvt.cc @@ -108,8 +108,8 @@ HybridPvt::HybridPvt(ConfigurationInterface* configuration, //std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model.xml", utc_default_xml_filename); //std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); //std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); - //std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); - + //std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); + // make PVT object pvt_ = hybrid_make_pvt_cc(in_streams_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname); DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; @@ -120,7 +120,7 @@ bool HybridPvt::save_assistance_to_XML() { LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename_; std::map eph_map = pvt_->get_GPS_L1_ephemeris_map(); - + if (eph_map.size() > 0) { try @@ -177,4 +177,3 @@ gr::basic_block_sptr HybridPvt::get_right_block() { return pvt_; // this is a sink, nothing downstream } - diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index ffb697835..ea3290e90 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -82,6 +82,8 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, do std::map::iterator gnss_pseudoranges_iter; std::map::iterator galileo_ephemeris_iter; std::map::iterator gps_ephemeris_iter; + std::map::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 gnss_pseudoranges_map, do } return b_valid_position; } - diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h index 243d75cdd..c085d29e4 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h +++ b/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_utc_model.h" -#include "gps_ephemeris.h" -#include "gps_utc_model.h" +//#include "galileo_ephemeris.h" +//#include "galileo_utc_model.h" +//#include "gps_ephemeris.h" +//#include "gps_utc_model.h" /*! @@ -64,7 +65,8 @@ public: Gps_Navigation_Message* d_GPS_ephemeris; std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris - std::map gps_ephemeris_map; //!< Map storing new Galileo_Ephemeris + std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris + std::map gps_cnav_ephemeris_map; Galileo_Utc_Model galileo_utc_model; Galileo_Iono galileo_iono; Galileo_Almanac galileo_almanac; diff --git a/src/algorithms/observables/adapters/galileo_e1_observables.cc b/src/algorithms/observables/adapters/galileo_e1_observables.cc index 148cacc53..28edee2d5 100644 --- a/src/algorithms/observables/adapters/galileo_e1_observables.cc +++ b/src/algorithms/observables/adapters/galileo_e1_observables.cc @@ -31,8 +31,9 @@ #include "galileo_e1_observables.h" -#include "configuration_interface.h" #include +#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_; } - diff --git a/src/algorithms/observables/adapters/gps_l1_ca_observables.cc b/src/algorithms/observables/adapters/gps_l1_ca_observables.cc index 6b176d006..b75f1c3ef 100644 --- a/src/algorithms/observables/adapters/gps_l1_ca_observables.cc +++ b/src/algorithms/observables/adapters/gps_l1_ca_observables.cc @@ -33,6 +33,7 @@ #include "gps_l1_ca_observables.h" #include "configuration_interface.h" #include +#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_; } - diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index d3a4d9899..716853193 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -33,6 +33,8 @@ #include "hybrid_observables.h" #include "configuration_interface.h" #include +#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_; } - diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc index 21bae018b..ed75b4c46 100644 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc +++ b/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(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(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(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(GALILEO_E1_HISTORY_DEEP, 2); + arma::mat A = arma::ones(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; } - diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h index 1516ea239..1cb3ef4c4 100644 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h +++ b/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_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_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, unsigned int deep_history); //Tracking observable history std::vector> 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; }; diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc index 3dfda72a5..4d617946c 100644 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc +++ b/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++) { @@ -117,7 +118,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni Gnss_Synchro current_gnss_synchro[d_nchannels]; std::map current_gnss_synchro_map; std::map::iterator gnss_synchro_iter; - + if (d_nchannels != ninput_items.size()) { LOG(WARNING) << "The Observables block is not well connected"; @@ -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(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(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(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 (GPS_L1_CA_HISTORY_DEEP, 2); + arma::mat A = arma::ones (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; } - diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h index 30670efb5..38a1e7f4d 100644 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h +++ b/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_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_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, 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; }; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 6a68032b1..ab22fd881 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #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(d_nchannels)); + d_carrier_doppler_queue_hz.push_back(std::deque(d_nchannels)); + d_symbol_TOW_queue_s.push_back(std::deque(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& a, const std::pair& b) -{ - return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms); -} +//bool Hybrid_pairCompare_gnss_synchro_Prn_delay_ms(const std::pair& a, const std::pair& b) +//{ +// 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& a, const std::pair& b) @@ -108,15 +117,17 @@ bool Hybrid_pairCompare_gnss_synchro_d_TOW_at_current_symbol(const std::pair current_gnss_synchro_map; - std::map current_gnss_synchro_map_gps_only; + //std::map current_gnss_synchro_map_gps_only; std::map::iterator gnss_synchro_iter; if (d_nchannels != ninput_items.size()) @@ -140,10 +151,37 @@ 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(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); - if (current_gnss_synchro[i].System == 'G') - { - current_gnss_synchro_map_gps_only.insert(std::pair(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); - } + // if (current_gnss_synchro[i].System == 'G') + // { + // current_gnss_synchro_map_gps_only.insert(std::pair(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) + { + 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(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(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(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 (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="<