mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-29 02:14:51 +00:00
Code Issues Releases Wiki Activity

Add moving average parameter in observables

Browse Source
This commit is contained in:
Carles Fernandez 2016-11-02 17:35:40 +01:00
parent d4a14c3f2d
commit fe6b2387f4
12 changed files with 168 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/algorithms/PVT/adapters/hybrid_pvt.cc
View File

@ -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<int,Gps_Ephemeris> 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
}

3
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@ -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;
}

12
src/algorithms/PVT/libs/hybrid_ls_pvt.h
View File

@ -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<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;

7
src/algorithms/observables/adapters/galileo_e1_observables.cc
View File

@ -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_;
}

5
src/algorithms/observables/adapters/gps_l1_ca_observables.cc
View File

@ -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_;
}

15
src/algorithms/observables/adapters/hybrid_observables.cc
View File

@ -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_;
}

20
src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc
View File

@ -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;
}

7
src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h
View File

@ -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_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<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;
};

22
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
View File

@ -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<int,Gnss_Synchro> current_gnss_synchro_map;
std::map<int,Gnss_Synchro>::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<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;
}

7
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
View File

@ -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_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;
};

116
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -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)
{
return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms);
}
//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);
//}
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,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
int hybrid_observables_cc::general_work (int noutput_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,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<int, Gnss_Synchro>(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<int, Gnss_Synchro>(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<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)
{
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;
}

12
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
View File

@ -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_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, 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;
};