[hamradio-commits] [gnss-sdr] 135/303: Add moving average parameter in observables
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Mon Feb 13 22:35:55 UTC 2017
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch master
in repository gnss-sdr.
commit fe6b2387f43f04bf76983b22a308b81170551b0d
Author: Carles Fernandez <carlesfernandez at gmail.com>
Date: Wed Nov 2 17:35:40 2016 +0100
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 ++-
.../observables/adapters/galileo_e1_observables.cc | 7 +-
.../observables/adapters/gps_l1_ca_observables.cc | 5 +-
.../observables/adapters/hybrid_observables.cc | 15 ++-
.../gnuradio_blocks/galileo_e1_observables_cc.cc | 20 ++--
.../gnuradio_blocks/galileo_e1_observables_cc.h | 7 +-
.../gnuradio_blocks/gps_l1_ca_observables_cc.cc | 22 ++--
.../gnuradio_blocks/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 af08e9a..dcd202b 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<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
}
-
diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
index ffb6978..ea3290e 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<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;
}
-
diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h
index 243d75c..c085d29 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<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;
diff --git a/src/algorithms/observables/adapters/galileo_e1_observables.cc b/src/algorithms/observables/adapters/galileo_e1_observables.cc
index 148cacc..28edee2 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 <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_;
}
-
diff --git a/src/algorithms/observables/adapters/gps_l1_ca_observables.cc b/src/algorithms/observables/adapters/gps_l1_ca_observables.cc
index 6b176d0..b75f1c3 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 <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_;
}
-
diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc
index d3a4d98..7168531 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 <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_;
}
-
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 21bae01..ed75b4c 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<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;
}
-
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 1516ea2..1cb3ef4 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> 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;
};
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 3dfda72..4d61794 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<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;
}
-
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 30670ef..38a1e7f 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> 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 6a68032..ab22fd8 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 <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;
}
-
diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
index 6ff2846..20154c7 100644
--- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
+++ b/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_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;
};
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list