[hamradio-commits] [gnss-sdr] 244/303: Adding Bancroft's algorithm implementation for PVT initialization
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Mon Feb 13 22:36:05 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 d2c7bb62a1316629b5d01f112969d07280075e00
Author: Javier Arribas <javiarribas at gmail.com>
Date: Fri Jan 27 19:21:51 2017 +0100
Adding Bancroft's algorithm implementation for PVT initialization
---
.../PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc | 3 -
.../PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc | 14 +-
.../PVT/gnuradio_blocks/hybrid_pvt_cc.cc | 3 -
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc | 69 +++++-----
src/algorithms/PVT/libs/ls_pvt.cc | 143 ++++++++++++++++++++-
src/algorithms/PVT/libs/ls_pvt.h | 19 ++-
src/algorithms/PVT/libs/pvt_solution.cc | 1 +
src/algorithms/PVT/libs/pvt_solution.h | 2 +
.../gnuradio_blocks/galileo_e1_observables_cc.cc | 14 --
.../gnuradio_blocks/galileo_e1_observables_cc.h | 2 -
.../gnuradio_blocks/gps_l1_ca_observables_cc.cc | 29 +----
.../gnuradio_blocks/gps_l1_ca_observables_cc.h | 3 +-
.../gnuradio_blocks/hybrid_observables_cc.cc | 14 --
.../gnuradio_blocks/hybrid_observables_cc.h | 2 -
src/core/receiver/gnss_flowgraph.cc | 2 -
15 files changed, 206 insertions(+), 114 deletions(-)
diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
index 28be90a..35e1478 100644
--- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
+++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
@@ -123,9 +123,6 @@ galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, bool dump, std::str
this->message_port_register_in(pmt::mp("telemetry"));
this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&galileo_e1_pvt_cc::msg_handler_telemetry, this, _1));
- // Receiver time feedback to observables block
- this->message_port_register_out(pmt::mp("rx_dt_s"));
-
//initialize kml_printer
std::string kml_dump_filename;
kml_dump_filename = d_dump_filename;
diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
index c16124e..8c774fa 100644
--- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
+++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
@@ -227,9 +227,6 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels,
this->set_msg_handler(pmt::mp("telemetry"),
boost::bind(&gps_l1_ca_pvt_cc::msg_handler_telemetry, this, _1));
- // Receiver time feedback to observables block
- this->message_port_register_out(pmt::mp("rx_dt_s"));
-
//initialize kml_printer
std::string kml_dump_filename;
kml_dump_filename = d_dump_filename;
@@ -376,11 +373,16 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g
pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging);
if (pvt_result == true)
{
- //feedback the receiver time offset estimation to observables block
+ //correct the observable to account for the receiver clock offset
+
+ for (std::map<int,Gnss_Synchro>::iterator it=gnss_observables_map.begin(); it!=gnss_observables_map.end(); ++it)
+ {
+ it->second.Pseudorange_m=it->second.Pseudorange_m-d_ls_pvt->d_rx_dt_s*GPS_C_m_s;
+ }
// send asynchronous message to observables block
// time offset is expressed as the equivalent travel distance [m]
- pmt::pmt_t value = pmt::from_double(d_ls_pvt->d_rx_dt_s);
- this->message_port_pub(pmt::mp("rx_dt_s"), value);
+ //pmt::pmt_t value = pmt::from_double(d_ls_pvt->d_rx_dt_s);
+ //this->message_port_pub(pmt::mp("rx_dt_s"), value);
//std::cout<<"d_rx_dt_s*GPS_C_m_s="<<d_ls_pvt->d_rx_dt_s*GPS_C_m_s<<std::endl;
if( first_fix == true)
{
diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
index fd0cf80..cf14524 100644
--- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
+++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
@@ -216,9 +216,6 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump
this->message_port_register_in(pmt::mp("telemetry"));
this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&hybrid_pvt_cc::msg_handler_telemetry, this, _1));
- // Receiver time feedback to observables block
- this->message_port_register_out(pmt::mp("rx_dt_s"));
-
//initialize kml_printer
std::string kml_dump_filename;
kml_dump_filename = d_dump_filename;
diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
index b5fe755..4f5cc12 100644
--- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
@@ -80,11 +80,10 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
{
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
- int valid_pseudoranges = gnss_pseudoranges_map.size();
- arma::mat W = arma::eye(valid_pseudoranges, valid_pseudoranges); //channels weights matrix
- arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
- arma::mat satpos = arma::zeros(3, valid_pseudoranges); //satellite positions matrix
+ arma::vec W;//= arma::eye(valid_pseudoranges, valid_pseudoranges); //channels weights matrix
+ arma::vec obs;// = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
+ arma::mat satpos;// = arma::zeros(3, valid_pseudoranges); //satellite positions matrix
int GPS_week = 0;
double utc = 0;
@@ -97,7 +96,6 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
// ********************************************************************************
int valid_obs = 0; //valid observations counter
- int obs_counter = 0;
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++)
@@ -109,7 +107,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
- W(obs_counter, obs_counter) = 1;
+ W.resize(valid_obs+1,1);
+ W(valid_obs)=1;
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
@@ -122,13 +121,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
-
- satpos(0, obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
- satpos(1, obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
- satpos(2, obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
-
+ satpos.resize(3,valid_obs+1);
+ satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X;
+ satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y;
+ satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected pseudoranges
- obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s;
+ obs.resize(valid_obs+1,1);
+ obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s-d_rx_dt_s*GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++;
@@ -138,7 +137,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
- << " [m] PR_obs=" << obs(obs_counter) << " [m]";
+ << " [m] PR_obs=" << obs(valid_obs) << " [m]";
// compute the UTC time for this SV (just to print the associated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
@@ -146,12 +145,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
}
else // the ephemeris are not available for this SV
{
- // no valid pseudorange for the current SV
- W(obs_counter, obs_counter) = 0; // SV de-activated
- obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first;
}
- obs_counter++;
}
// ********************************************************************************
@@ -162,32 +157,44 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
if (valid_obs >= 4)
{
- arma::vec mypos;
+ arma::vec rx_position_and_time;
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W;
- mypos = leastSquarePos(satpos, obs, W);
- DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos;
-
- cart2geo(static_cast<double>(mypos(0)), static_cast<double>(mypos(1)), static_cast<double>(mypos(2)), 4);
-
- d_rx_dt_s = mypos(3)/GPS_C_m_s; // Convert RX time offset from meters to seconds
-
+ //check if this is the initial position computation
+ if (d_rx_dt_s==0)
+ {
+ //execute Bancroft's algorithm to estimate initial receiver position and time
+ std::cout<<"Executing Bancroft algorithm...\n";
+ rx_position_and_time =bancroftPos(satpos.t(), obs);
+ d_rx_pos=rx_position_and_time.rows(0,2); //save ECEF position for the next iteration
+ d_rx_dt_s=rx_position_and_time(3)/GPS_C_m_s; //save time for the next iteration [meters]->[seconds]
+ }
+
+ //Execute WLS using previos position as the initialization point
+ rx_position_and_time = leastSquarePos(satpos, obs, W);
+ d_rx_pos=rx_position_and_time.rows(0,2); //save ECEF position for the next iteration
+ d_rx_dt_s+=rx_position_and_time(3)/GPS_C_m_s; //accumulate the rx time error for the next iteration [meters]->[seconds]
+
+ DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
+ DLOG(INFO) <<"Accumulated rx clock error="<<d_rx_dt_s<<" clock error for this iteration="<<rx_position_and_time(3)/GPS_C_m_s<<" [s]"<<std::endl;
+
+ cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
// Compute UTC time and print PVT solution
double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60)
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek * static_cast<double>(GPS_week));
// 22 August 1999 last GPS time roll over
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
-
DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
// ###### Compute DOPs ########
+ std::cout<<"c\r";
compute_DOP();
-
+ std::cout<<"d\r";
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
@@ -199,16 +206,16 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
tmp_double = GPS_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
- tmp_double = mypos(0);
+ tmp_double = d_rx_pos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
- tmp_double = mypos(1);
+ tmp_double = d_rx_pos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
- tmp_double = mypos(2);
+ tmp_double = d_rx_pos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
- tmp_double = mypos(3);
+ tmp_double = d_rx_dt_s;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;
diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc
index 1c92d4d..a4b315f 100644
--- a/src/algorithms/PVT/libs/ls_pvt.cc
+++ b/src/algorithms/PVT/libs/ls_pvt.cc
@@ -41,12 +41,139 @@ using google::LogMessage;
Ls_Pvt::Ls_Pvt() : Pvt_Solution()
{
- d_x_m = 0.0;
- d_y_m = 0.0;
- d_z_m = 0.0;
+
+}
+
+arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs) {
+
+// %BANCROFT Calculation of preliminary coordinates
+// % for a GPS receiver based on pseudoranges
+// % to 4 or more satellites. The ECEF
+// % coordinates are stored in satpos. The observed pseudoranges are stored in obs
+// %Reference: Bancroft, S. (1985) An Algebraic Solution
+// % of the GPS Equations, IEEE Trans. Aerosp.
+// % and Elec. Systems, AES-21, 56--59
+// %Kai Borre 04-30-95, improved by C.C. Goad 11-24-96
+// %Copyright (c) by Kai Borre
+// %$Revision: 1.0 $ $Date: 1997/09/26 $
+//
+// % Test values to use in debugging
+// % B_pass =[ -11716227.778 -10118754.628 21741083.973 22163882.029;
+// % -12082643.974 -20428242.179 11741374.154 21492579.823;
+// % 14373286.650 -10448439.349 19596404.858 21492492.771;
+// % 10278432.244 -21116508.618 -12689101.970 25284588.982];
+// % Solution: 595025.053 -4856501.221 4078329.981
+//
+// % Test values to use in debugging
+// % B_pass = [14177509.188 -18814750.650 12243944.449 21119263.116;
+// % 15097198.146 -4636098.555 21326705.426 22527063.486;
+// % 23460341.997 -9433577.991 8174873.599 23674159.579;
+// % -8206498.071 -18217989.839 17605227.065 20951643.862;
+// % 1399135.830 -17563786.820 19705534.862 20155386.649;
+// % 6995655.459 -23537808.269 -9927906.485 24222112.972];
+// % Solution: 596902.683 -4847843.316 4088216.740
+
+ arma::vec pos = arma::zeros(4,1);
+ arma::mat B_pass=arma::zeros(obs.size(),4);
+ B_pass.submat(0,0,obs.size()-1,2)=satpos;
+ B_pass.col(3)=obs;
+
+ arma::mat B;
+ arma::mat BBB;
+ double traveltime=0;
+ for (int iter = 0; iter<2; iter++)
+ {
+ B = B_pass;
+ int m=arma::size(B,0);
+ for (int i=0;i<m;i++)
+ {
+ int x = B(i,0);
+ int y = B(i,1);
+ if (iter == 0)
+ {
+ traveltime = 0.072;
+ }
+ else
+ {
+ int z = B(i,2);
+ double rho = (x-pos(0))*(x-pos(0))+(y-pos(1))*(y-pos(1))+(z-pos(2))*(z-pos(2));
+ traveltime = sqrt(rho)/GPS_C_m_s;
+ }
+ double angle = traveltime*7.292115147e-5;
+ double cosa = cos(angle);
+ double sina = sin(angle);
+ B(i,0) = cosa*x + sina*y;
+ B(i,1) = -sina*x + cosa*y;
+ }// % i-loop
+
+ if (m > 3)
+ {
+ BBB = arma::inv(B.t()*B)*B.t();
+ }
+ else
+ {
+ BBB = arma::inv(B);
+ }
+ arma::vec e = arma::ones(m,1);
+ arma::vec alpha = arma::zeros(m,1);
+ for (int i =0; i<m;i++)
+ {
+ alpha(i) = lorentz(B.row(i).t(),B.row(i).t())/2.0;
+ }
+ arma::mat BBBe = BBB*e;
+ arma::mat BBBalpha = BBB*alpha;
+ double a = lorentz(BBBe,BBBe);
+ double b = lorentz(BBBe,BBBalpha)-1;
+ double c = lorentz(BBBalpha,BBBalpha);
+ double root = sqrt(b*b-a*c);
+ arma::vec r = {{(-b-root)/a},{(-b+root)/a}};
+ arma::mat possible_pos = arma::zeros(4,2);
+ for (int i =0;i<2; i++)
+ {
+ possible_pos.col(i) = r(i)*BBBe+BBBalpha;
+ possible_pos(3,i) = -possible_pos(3,i);
+ }
+
+ arma::vec abs_omc=arma::zeros(2,1);
+ for (int j=0; j<m; j++)
+ {
+ for (int i =0;i<2;i++)
+ {
+ double c_dt = possible_pos(3,i);
+ double calc = arma::norm(satpos.row(i).t() -possible_pos.col(i).rows(0,2))+c_dt;
+ double omc = obs(j)-calc;
+ abs_omc(i) = std::abs(omc);
+ }
+ }// % j-loop
+
+ //discrimination between roots
+ if (abs_omc(0) > abs_omc(1))
+ {
+ pos = possible_pos.col(1);
+ }
+ else
+ {
+ pos = possible_pos.col(0);
+ }
+ }// % iter loop
+ return pos;
+}
+
+double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y) {
+// %LORENTZ Calculates the Lorentz inner product of the two
+// % 4 by 1 vectors x and y
+//
+// %Kai Borre 04-22-95
+// %Copyright (c) by Kai Borre
+// %$Revision: 1.0 $ $Date: 1997/09/26 $
+//
+// % M = diag([1 1 1 -1]);
+// % p = x'*M*y;
+
+ return(x(0)*y(0) + x(1)*y(1) + x(2)*y(2) - x(3)*y(3));
}
-arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w)
+arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::vec & w_vec)
{
/* Computes the Least Squares Solution.
* Inputs:
@@ -63,7 +190,10 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
int nmbOfIterations = 10; // TODO: include in config
int nmbOfSatellites;
nmbOfSatellites = satpos.n_cols; //Armadillo
- arma::vec pos = "0.0 0.0 0.0 0.0";
+ arma::mat w=arma::zeros(nmbOfSatellites,nmbOfSatellites);
+ w.diag()=w_vec; //diagonal weight matrix
+
+ arma::vec pos = {{d_rx_pos(0)},{d_rx_pos(0)},{d_rx_pos(0)},0}; //time error in METERS (time x speed)
arma::mat A;
arma::mat omc;
arma::mat az;
@@ -160,5 +290,8 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
{
d_Q = arma::zeros(4,4);
}
+
return pos;
}
+
+
diff --git a/src/algorithms/PVT/libs/ls_pvt.h b/src/algorithms/PVT/libs/ls_pvt.h
index 385d6e4..9332c19 100644
--- a/src/algorithms/PVT/libs/ls_pvt.h
+++ b/src/algorithms/PVT/libs/ls_pvt.h
@@ -41,13 +41,24 @@
*/
class Ls_Pvt : public Pvt_Solution
{
+private:
+ /*!
+ * \brief Computes the Lorentz inner product between two vectors
+ */
+ double lorentz(const arma::vec & x,const arma::vec & y);
public:
Ls_Pvt();
- arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w);
- double d_x_m;
- double d_y_m;
- double d_z_m;
+ /*!
+ * \brief Computes the initial position solution based on the Bancroft algorithm
+ */
+ arma::vec bancroftPos(const arma::mat & satpos, const arma::vec & obs);
+
+ /*!
+ * \brief Computes the Weighted Least Squares position solution
+ */
+ arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::vec & w_vec);
+
};
#endif
diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc
index d4312e0..a3913dc 100644
--- a/src/algorithms/PVT/libs/pvt_solution.cc
+++ b/src/algorithms/PVT/libs/pvt_solution.cc
@@ -57,6 +57,7 @@ Pvt_Solution::Pvt_Solution()
b_valid_position = false;
d_averaging_depth = 0;
d_valid_observations = 0;
+ d_rx_pos=arma::zeros(3,1);
d_rx_dt_s = 0.0;
}
diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h
index 5f6d3a9..3b06813 100644
--- a/src/algorithms/PVT/libs/pvt_solution.h
+++ b/src/algorithms/PVT/libs/pvt_solution.h
@@ -51,6 +51,8 @@ public:
double d_latitude_d; //!< RX position Latitude WGS84 [deg]
double d_longitude_d; //!< RX position Longitude WGS84 [deg]
double d_height_m; //!< RX position height WGS84 [m]
+
+ arma::vec d_rx_pos;
double d_rx_dt_s; //!< RX time offset [s]
boost::posix_time::ptime d_position_UTC_time;
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 faed78b..ed75b4c 100644
--- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc
+++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc
@@ -56,24 +56,10 @@ galileo_e1_make_observables_cc(unsigned int nchannels, bool dump, std::string du
}
-void galileo_e1_observables_cc::msg_handler_rx_dt_s(pmt::pmt_t msg)
-{
- //pmt::print(msg);
-
- d_rx_dt_s = pmt::to_double(msg);
-}
-
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)))
{
-
- // Telemetry bit synchronization message port input
- this->message_port_register_in(pmt::mp("rx_dt_s"));
-
- this->set_msg_handler(pmt::mp("rx_dt_s"),
- boost::bind(&galileo_e1_observables_cc::msg_handler_rx_dt_s, this, _1));
-
// initialize internal vars
d_dump = dump;
d_nchannels = nchannels;
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 4779099..1cb3ef4 100644
--- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h
+++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h
@@ -61,7 +61,6 @@ private:
friend galileo_e1_observables_cc_sptr
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);
- void msg_handler_rx_dt_s(pmt::pmt_t msg);
//Tracking observable history
std::vector<std::deque<double>> d_acc_carrier_phase_queue_rads;
@@ -69,7 +68,6 @@ private:
std::vector<std::deque<double>> d_symbol_TOW_queue_s;
// class private vars
- double d_rx_dt_s;
bool d_dump;
unsigned int d_nchannels;
unsigned int history_deep;
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 36afa76..f1ce131 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
@@ -58,13 +58,6 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, bool
gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)))
{
- // Telemetry bit synchronization message port input
- this->message_port_register_in(pmt::mp("rx_dt_s"));
-
- this->set_msg_handler(pmt::mp("rx_dt_s"),
- boost::bind(&gps_l1_ca_observables_cc::msg_handler_rx_dt_s, this, _1));
-
- d_rx_dt_s=0;
// initialize internal vars
d_dump = dump;
d_nchannels = nchannels;
@@ -104,15 +97,6 @@ gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc()
d_dump_file.close();
}
-
-void gps_l1_ca_observables_cc::msg_handler_rx_dt_s(pmt::pmt_t msg)
-{
- //pmt::print(msg);
- //accumulate the receiver time offset
- d_rx_dt_s = d_rx_dt_s+pmt::to_double(msg);
-}
-
-
bool 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);
@@ -220,7 +204,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
//compute the pseudorange
traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol) * 1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms;
//convert to meters and remove the receiver time offset in meters
- pseudorange_m = traveltime_ms * GPS_C_m_ms-d_rx_dt_s*GPS_C_m_s; // [m]
+ pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m]
// update the pseudorange object
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;
@@ -237,7 +221,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
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;
@@ -249,10 +232,8 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
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;
+ acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0];
+ carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0];
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];
}
@@ -277,10 +258,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
- //tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true);
- //tmp_double = current_gnss_synchro[i].debug_var1;
- //tmp_double = current_gnss_synchro[i].debug_var2;
- //d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].PRN;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
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 387a926..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
@@ -61,7 +61,7 @@ private:
friend gps_l1_ca_observables_cc_sptr
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);
- void msg_handler_rx_dt_s(pmt::pmt_t msg);
+
//Tracking observable history
std::vector<std::deque<double>> d_acc_carrier_phase_queue_rads;
@@ -69,7 +69,6 @@ private:
std::vector<std::deque<double>> d_symbol_TOW_queue_s;
// class private vars
- double d_rx_dt_s;
bool d_dump;
unsigned int d_nchannels;
unsigned int history_deep;
diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
index 89a7fd9..9a88419 100644
--- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
+++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
@@ -55,24 +55,10 @@ hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_f
}
-void hybrid_observables_cc::msg_handler_rx_dt_s(pmt::pmt_t msg)
-{
- //pmt::print(msg);
-
- d_rx_dt_s = pmt::to_double(msg);
-}
-
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)))
{
-
- // Telemetry bit synchronization message port input
- this->message_port_register_in(pmt::mp("rx_dt_s"));
-
- this->set_msg_handler(pmt::mp("rx_dt_s"),
- boost::bind(&hybrid_observables_cc::msg_handler_rx_dt_s, this, _1));
-
// initialize internal vars
d_dump = dump;
d_nchannels = nchannels;
diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
index ea916af..20154c7 100644
--- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
+++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
@@ -59,7 +59,6 @@ private:
friend hybrid_observables_cc_sptr
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);
- void msg_handler_rx_dt_s(pmt::pmt_t msg);
//Tracking observable history
std::vector<std::deque<double>> d_acc_carrier_phase_queue_rads;
@@ -67,7 +66,6 @@ private:
std::vector<std::deque<double>> d_symbol_TOW_queue_s;
// class private vars
- double d_rx_dt_s;
bool d_dump;
unsigned int d_nchannels;
unsigned int history_deep;
diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc
index ecdaf4e..138140d 100644
--- a/src/core/receiver/gnss_flowgraph.cc
+++ b/src/core/receiver/gnss_flowgraph.cc
@@ -324,8 +324,6 @@ void GNSSFlowgraph::connect()
top_block_->connect(observables_->get_right_block(), i, pvt_->get_left_block(), i);
top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("telemetry"), pvt_->get_left_block(), pmt::mp("telemetry"));
}
- //asynchronous feedback of receiver time estimation from PVT to observables
- top_block_->msg_connect(pvt_->get_left_block(), pmt::mp("rx_dt_s"), observables_->get_right_block(), pmt::mp("rx_dt_s"));
}
catch (std::exception& e)
{
--
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