[hamradio-commits] [gnss-sdr] 282/303: Upgrading Galileo PVT and Hybrid PVT chains with the latest bug fixes for GPS

Carles Fernandez carles_fernandez-guest at moszumanska.debian.org
Mon Feb 13 22:36:08 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 fc7ff7ba0bf3ca984313af1975a4bb9eed6bcc41
Author: Javier Arribas <javiarribas at gmail.com>
Date:   Fri Feb 3 13:00:50 2017 +0100

    Upgrading Galileo PVT and Hybrid PVT chains with the latest bug fixes for GPS
---
 .../PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc       |   5 +
 .../PVT/gnuradio_blocks/hybrid_pvt_cc.cc           |  16 +
 src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc       | 282 +++++------
 src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc        |   2 +-
 src/algorithms/PVT/libs/hybrid_ls_pvt.cc           | 515 +++++++++++----------
 .../gnuradio_blocks/hybrid_observables_cc.cc       |   9 +-
 6 files changed, 444 insertions(+), 385 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 35e1478..e7c1f98 100644
--- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
+++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
@@ -287,6 +287,11 @@ int galileo_e1_pvt_cc::general_work (int noutput_items __attribute__((unused)),
 
                     if (pvt_result == true)
                         {
+                            // 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;
+                                }
                             if( first_fix == true)
                                 {
                                     std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
index cf14524..946a4e9 100644
--- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
+++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
@@ -455,6 +455,22 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v
 
                     if (pvt_result == true)
                         {
+                            // 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;
+                                 }
+                             if(first_fix == true)
+                                 {
+                                     std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
+                                               << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
+                                               << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
+                                     ttff_msgbuf ttff;
+                                     ttff.mtype = 1;
+                                     ttff.ttff = d_sample_counter;
+                                     send_sys_v_ttff_msg(ttff);
+                                     first_fix = false;
+                                 }
                             d_kml_dump->print_position(d_ls_pvt, d_flag_averaging);
                             d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging);
                             d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
index 070503c..8f60917 100644
--- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
@@ -48,21 +48,21 @@ galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels, std::string dump_filename, b
 
     // ############# ENABLE DATA FILE LOG #################
     if (d_flag_dump_enabled == true)
+    {
+        if (d_dump_file.is_open() == false)
         {
-            if (d_dump_file.is_open() == false)
-                {
-                    try
-                    {
-                            d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
-                            d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
-                            LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
-                    }
-                    catch (const std::ifstream::failure &e)
-                    {
-                            LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
-                    }
-                }
+            try
+            {
+                d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
+                d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
+                LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
+            }
+            catch (const std::ifstream::failure &e)
+            {
+                LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
+            }
         }
+    }
 }
 
 
@@ -78,11 +78,10 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
 {
     std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
     std::map<int,Galileo_Ephemeris>::iterator galileo_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;      // channels weight vector
+    arma::vec obs;    // pseudoranges observation vector
+    arma::mat satpos; // satellite positions matrix
 
     int Galileo_week_number = 0;
     double utc = 0.0;
@@ -96,60 +95,62 @@ bool galileo_e1_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++)
+    {
+        // 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
+        galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first);
+        if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
+        {
+            /*!
+             * \todo Place here the satellite CN0 (power level, or weight factor)
+             */
+            W.resize(valid_obs + 1, 1);
+            W(valid_obs) = 1;
+
+            // COMMON RX TIME PVT ALGORITHM
+            double Rx_time = galileo_current_time;
+            double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GALILEO_C_m_s;
+
+            // 2- compute the clock drift using the clock model (broadcast) for this SV, including relativistic effect
+            SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
+
+            // 3- compute the current ECEF position for this SV using corrected TX time
+            TX_time_corrected_s = Tx_time - SV_clock_bias_s;
+            galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
+
+            //store satellite positions in a matrix
+            satpos.resize(3, valid_obs + 1);
+            satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X;
+            satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
+            satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
+
+            // 4- fill the observations vector with the corrected pseudoranges
+            obs.resize(valid_obs + 1, 1);
+            obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s;
+            d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
+            d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
+
+
+            Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
+            GST = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first)->second.Galileo_System_Time(Galileo_week_number, galileo_current_time);
+
+            // SV ECEF DEBUG OUTPUT
+            DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
+                    << " X=" << galileo_ephemeris_iter->second.d_satpos_X
+                    << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
+                    << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
+                    << " [m] PR_obs=" << obs(valid_obs) << " [m]";
+
+            valid_obs++;
+        }
+        else // the ephemeris are not available for this SV
         {
-            // 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
-            galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first);
-            if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
-                {
-                    /*!
-                     * \todo Place here the satellite CN0 (power level, or weight factor)
-                     */
-                    W(obs_counter, obs_counter) = 1.0;
-
-                    // COMMON RX TIME PVT ALGORITHM
-                    double Rx_time = galileo_current_time;
-                    double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GALILEO_C_m_s;
-
-                    // 2- compute the clock drift using the clock model (broadcast) for this SV, including relativistic effect
-                    SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
-
-                    // 3- compute the current ECEF position for this SV using corrected TX time
-                    TX_time_corrected_s = Tx_time - SV_clock_bias_s;
-                    galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
-
-                    satpos(0,obs_counter) = galileo_ephemeris_iter->second.d_satpos_X;
-                    satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y;
-                    satpos(2,obs_counter) = galileo_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 * GALILEO_C_m_s;
-                    d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
-                    d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
-                    valid_obs++;
-
-                    Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
-                    GST = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first)->second.Galileo_System_Time(Galileo_week_number, galileo_current_time);
-
-                    // SV ECEF DEBUG OUTPUT
-                    DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
-                               << " X=" << galileo_ephemeris_iter->second.d_satpos_X
-                               << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
-                               << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
-                               << " [m] PR_obs=" << obs(obs_counter) << " [m]";
-                }
-            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++;
+            DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first;
         }
+    }
 
     // ********************************************************************************
     // ****** SOLVE LEAST SQUARES******************************************************
@@ -158,78 +159,91 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
     LOG(INFO) << "Galileo PVT: valid observations=" << valid_obs;
 
     if (valid_obs >= 4)
+    {
+        arma::vec rx_position_and_time;
+        DLOG(INFO) << "satpos=" << satpos;
+        DLOG(INFO) << "obs="<< obs;
+        DLOG(INFO) << "W=" << W;
+
+        // check if this is the initial position computation
+        if (d_rx_dt_s == 0)
         {
-            arma::vec mypos;
-            DLOG(INFO) << "satpos=" << satpos;
-            DLOG(INFO) << "obs="<< obs;
-            DLOG(INFO) << "W=" << W;
-
-            mypos = leastSquarePos(satpos, obs, W);
-
-            // Compute Gregorian time
-            utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
-            // get time string Gregorian calendar
-            boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
-            // 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
-            boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
-            d_position_UTC_time = p_time;
-
-            DLOG(INFO) << "Galileo Position at TOW=" << galileo_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)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds
-            DLOG(INFO) << "Galileo 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 ########
-            compute_DOP();
-
-            // ######## LOG FILE #########
-            if(d_flag_dump_enabled == true)
-                {
-                    // MULTIPLEXED FILE RECORDING - Record results to file
-                    try
-                    {
-                            double tmp_double;
-                            //  PVT GPS time
-                            tmp_double = galileo_current_time;
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // ECEF User Position East [m]
-                            tmp_double = mypos(0);
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // ECEF User Position North [m]
-                            tmp_double = mypos(1);
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // ECEF User Position Up [m]
-                            tmp_double = mypos(2);
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // User clock offset [s]
-                            tmp_double = mypos(3);
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // GEO user position Latitude [deg]
-                            tmp_double = d_latitude_d;
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // GEO user position Longitude [deg]
-                            tmp_double = d_longitude_d;
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // GEO user position Height [m]
-                            tmp_double = d_height_m;
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                    }
-                    catch (const std::ifstream::failure& e)
-                    {
-                            LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
-                    }
-                }
-
-            // MOVING AVERAGE PVT
-            galileo_e1_ls_pvt::pos_averaging(flag_averaging);
+            // execute Bancroft's algorithm to estimate initial receiver position and time
+            DLOG(INFO) << " Executing Bancroft algorithm...";
+            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) / GALILEO_C_m_s; // save time for the next iteration [meters]->[seconds]
         }
-    else
+        // Execute WLS using previous 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) / GALILEO_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
+
+        // Compute Gregorian time
+        utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
+        // get time string Gregorian calendar
+        boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
+        // 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
+        boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
+        d_position_UTC_time = p_time;
+
+        DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << rx_position_and_time;
+
+        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);
+        d_rx_dt_s = rx_position_and_time(3)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds
+        DLOG(INFO) << "Galileo 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 ########
+        compute_DOP();
+
+        // ######## LOG FILE #########
+        if(d_flag_dump_enabled == true)
         {
-            b_valid_position = false;
+            // MULTIPLEXED FILE RECORDING - Record results to file
+            try
+            {
+                double tmp_double;
+                //  PVT GPS time
+                tmp_double = galileo_current_time;
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // ECEF User Position East [m]
+                tmp_double = rx_position_and_time(0);
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // ECEF User Position North [m]
+                tmp_double = rx_position_and_time(1);
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // ECEF User Position Up [m]
+                tmp_double = rx_position_and_time(2);
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // User clock offset [s]
+                tmp_double = rx_position_and_time(3);
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // GEO user position Latitude [deg]
+                tmp_double = d_latitude_d;
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // GEO user position Longitude [deg]
+                tmp_double = d_longitude_d;
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // GEO user position Height [m]
+                tmp_double = d_height_m;
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+            }
+            catch (const std::ifstream::failure& e)
+            {
+                LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
+            }
         }
+
+        // MOVING AVERAGE PVT
+        galileo_e1_ls_pvt::pos_averaging(flag_averaging);
+    }
+    else
+    {
+        b_valid_position = false;
+    }
     return b_valid_position;
 }
 
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 a9e60f4..1633f6f 100644
--- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
@@ -133,7 +133,6 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
                     obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + dtr * 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++;
 
                     // SV ECEF DEBUG OUTPUT
                     DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
@@ -142,6 +141,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
                                << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
                                << " [m] PR_obs=" << obs(valid_obs) << " [m]";
 
+                    valid_obs++;
                     // compute the UTC time for this SV (just to print the associated UTC timestamp)
                     GPS_week = gps_ephemeris_iter->second.i_GPS_week;
                     utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
index 3629bac..87dae62 100644
--- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
@@ -47,21 +47,21 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
     d_flag_averaging = false;
     // ############# ENABLE DATA FILE LOG #################
     if (d_flag_dump_enabled == true)
+    {
+        if (d_dump_file.is_open() == false)
         {
-            if (d_dump_file.is_open() == false)
-                {
-                    try
-                    {
-                            d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
-                            d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
-                            LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
-                    }
-                    catch (const std::ifstream::failure &e)
-                    {
-                            LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
-                    }
-                }
+            try
+            {
+                d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
+                d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
+                LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
+            }
+            catch (const std::ifstream::failure &e)
+            {
+                LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
+            }
         }
+    }
 }
 
 
@@ -78,11 +78,9 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
     std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
     std::map<int,Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
 
-    int valid_observables = gnss_observables_map.size();
-
-    arma::mat W = arma::eye(valid_observables, valid_observables); // channels weights matrix
-    arma::vec obs = arma::zeros(valid_observables);                 // observables observation vector
-    arma::mat satpos = arma::zeros(3, valid_observables);           // satellite positions matrix
+    arma::vec W;      // channels weight vector
+    arma::vec obs;    // pseudoranges observation vector
+    arma::mat satpos; // satellite positions matrix
 
     int Galileo_week_number = 0;
     int GPS_week = 0;
@@ -98,168 +96,184 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
     // ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
     // ********************************************************************************
     int valid_obs = 0; //valid observations counter
-    int obs_counter = 0;
 
     for(gnss_observables_iter = gnss_observables_map.begin();
             gnss_observables_iter != gnss_observables_map.end();
             gnss_observables_iter++)
+    {
+        switch(gnss_observables_iter->second.System)
+        {
+        case 'E':
         {
-            if(gnss_observables_iter->second.System == 'E')
+            // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
+            galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
+            if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
+            {
+                /*!
+                 * \todo Place here the satellite CN0 (power level, or weight factor)
+                 */
+                W.resize(valid_obs + 1, 1);
+                W(valid_obs) = 1;
+
+                // COMMON RX TIME PVT ALGORITHM
+                double Rx_time = hybrid_current_time;
+                double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s;
+
+                // 2- compute the clock drift using the clock model (broadcast) for this SV
+                SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
+
+                // 3- compute the current ECEF position for this SV using corrected TX time
+                TX_time_corrected_s = Tx_time - SV_clock_bias_s;
+                galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
+
+                //store satellite positions in a matrix
+                satpos.resize(3, valid_obs + 1);
+                satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X;
+                satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
+                satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
+
+
+                // 4- fill the observations vector with the corrected observables
+                obs.resize(valid_obs + 1, 1);
+                obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s;
+                d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
+                d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
+
+
+
+                Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
+                GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
+
+                // SV ECEF DEBUG OUTPUT
+                DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
+                        << " X=" << galileo_ephemeris_iter->second.d_satpos_X
+                        << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
+                        << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
+                        << " [m] PR_obs=" << obs(valid_obs) << " [m]";
+
+                valid_obs++;
+            }
+
+            else // the ephemeris are not available for this SV
+            {
+                DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
+            }
+            break;
+        }
+        case 'G':
+        {
+            // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
+            std::string sig_(gnss_observables_iter->second.Signal);
+            if(sig_.compare("1C") == 0)
+            {
+                gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
+                if (gps_ephemeris_iter != gps_ephemeris_map.end())
                 {
-                    // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
-                    galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
-                    if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
-                        {
-                            /*!
-                             * \todo Place here the satellite CN0 (power level, or weight factor)
-                             */
-                            W(obs_counter, obs_counter) = 1;
-
-                            // COMMON RX TIME PVT ALGORITHM
-                            double Rx_time = hybrid_current_time;
-                            double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s;
-
-                            // 2- compute the clock drift using the clock model (broadcast) for this SV
-                            SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
-
-                            // 3- compute the current ECEF position for this SV using corrected TX time
-                            TX_time_corrected_s = Tx_time - SV_clock_bias_s;
-                            galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
-
-                            satpos(0,obs_counter) = galileo_ephemeris_iter->second.d_satpos_X;
-                            satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y;
-                            satpos(2,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Z;
-
-                            // 5- fill the observations vector with the corrected observables
-                            obs(obs_counter) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s;
-                            d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
-                            d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
-                            valid_obs++;
-
-                            Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
-                            GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
-
-                            // SV ECEF DEBUG OUTPUT
-                            DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
-                                    << " X=" << galileo_ephemeris_iter->second.d_satpos_X
-                                    << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
-                                    << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
-                                    << " [m] PR_obs=" << obs(obs_counter) << " [m]";
-                        }
-
-                    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_observables_iter->second.PRN;
-                        }
+                    /*!
+                     * \todo Place here the satellite CN0 (power level, or weight factor)
+                     */
+                    W.resize(valid_obs + 1, 1);
+                    W(valid_obs) = 1;
+
+                    // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
+                    // first estimate of transmit time
+                    double Rx_time = hybrid_current_time;
+                    double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
+
+                    // 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect
+                    SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
+
+                    // 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect
+                    TX_time_corrected_s = Tx_time - SV_clock_bias_s;
+                    double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
+
+                    //store satellite positions in a matrix
+                    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.resize(valid_obs + 1, 1);
+                    obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * 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_observables_iter->second.CN0_dB_hz;
+
+                    // SV ECEF DEBUG OUTPUT
+                    DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
+                               << " 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(valid_obs) << " [m]";
+
+                    valid_obs++;
+                    // compute the UTC time for this SV (just to print the associated UTC timestamp)
+                    GPS_week = gps_ephemeris_iter->second.i_GPS_week;
+                    utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
                 }
-
-            else if(gnss_observables_iter->second.System == 'G')
+            else // the ephemeris are not available for this SV
                 {
-                    //std::cout << "Satellite System: " << gnss_observables_iter->second.System <<std::endl;
-                    // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
-                    std::string sig_(gnss_observables_iter->second.Signal);
-                    if(sig_.compare("1C") == 0)
-                        {
-                            gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
-                            if (gps_ephemeris_iter != gps_ephemeris_map.end())
-                                {
-                                    /*!
-                                     * \todo Place here the satellite CN0 (power level, or weight factor)
-                                     */
-                                    W(obs_counter, obs_counter) = 1;
-
-                                    // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
-                                    // first estimate of transmit time
-                                    double Rx_time = hybrid_current_time;
-                                    double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
-
-                                    // 2- compute the clock drift using the clock model (broadcast) for this SV
-                                    SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time);
-
-                                    // 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;
-
-                                    // 5- fill the observations vector with the corrected observables
-                                    obs(obs_counter) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_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_observables_iter->second.CN0_dB_hz;
-                                    valid_obs++;
-                                    GPS_week = gps_ephemeris_iter->second.i_GPS_week;
-
-                                    // SV ECEF DEBUG OUTPUT
-                                    DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
-                                            << " 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]";
-                                }
-                            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_observables_iter->second.PRN;
-                                }
-                        }
-                    if(sig_.compare("2S") == 0)
-                        {
-                            gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
-                            if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
-                                {
-                                    /*!
-                                     * \todo Place here the satellite CN0 (power level, or weight factor)
-                                     */
-                                    W(obs_counter, obs_counter) = 1;
-
-                                    // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
-                                    // first estimate of transmit time
-                                    double Rx_time = hybrid_current_time;
-                                    double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
-
-                                    // 2- compute the clock drift using the clock model (broadcast) for this SV
-                                    SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time);
-
-                                    // 3- compute the current ECEF position for this SV using corrected TX time
-                                    TX_time_corrected_s = Tx_time - SV_clock_bias_s;
-                                    gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
-
-                                    satpos(0, obs_counter) = gps_cnav_ephemeris_iter->second.d_satpos_X;
-                                    satpos(1, obs_counter) = gps_cnav_ephemeris_iter->second.d_satpos_Y;
-                                    satpos(2, obs_counter) = gps_cnav_ephemeris_iter->second.d_satpos_Z;
-
-                                    // 5- fill the observations vector with the corrected observables
-                                    obs(obs_counter) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s;
-                                    d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN;
-                                    d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
-                                    valid_obs++;
-                                    GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
-
-                                    // SV ECEF DEBUG OUTPUT
-                                    DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
-                                            << " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X
-                                            << " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y
-                                            << " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z
-                                            << " [m] PR_obs=" << obs(obs_counter) << " [m]";
-                                }
-                            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_observables_iter->second.PRN;
-                                }
-                        }
+                    DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
                 }
-            obs_counter++;
+            }
+            if(sig_.compare("2S") == 0)
+            {
+                gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
+                if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
+                {
+                    /*!
+                     * \todo Place here the satellite CN0 (power level, or weight factor)
+                     */
+                    W.resize(valid_obs + 1, 1);
+                    W(valid_obs) = 1;
+
+                    // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
+                    // first estimate of transmit time
+                    double Rx_time = hybrid_current_time;
+                    double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
+
+                    // 2- compute the clock drift using the clock model (broadcast) for this SV
+                    SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time);
+
+                    // 3- compute the current ECEF position for this SV using corrected TX time
+                    TX_time_corrected_s = Tx_time - SV_clock_bias_s;
+                    gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
+
+                    //store satellite positions in a matrix
+                    satpos.resize(3, valid_obs + 1);
+                    satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X;
+                    satpos(1, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Y;
+                    satpos(2, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Z;
+
+                    // 4- fill the observations vector with the corrected observables
+                    obs.resize(valid_obs + 1, 1);
+                    obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s;
+                    d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN;
+                    d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
+
+                    GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
+
+                    // SV ECEF DEBUG OUTPUT
+                    DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
+                            << " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X
+                            << " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y
+                            << " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z
+                            << " [m] PR_obs=" << obs(valid_obs) << " [m]";
+
+                    valid_obs++;
+                }
+                else // the ephemeris are not available for this SV
+                {
+                    DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
+                }
+            }
+            break;
         }
+        default :
+            DLOG(INFO) << "Hybrid observables: Unknown GNSS";
+            break;
+        }
+    }
 
     // ********************************************************************************
     // ****** SOLVE LEAST SQUARES******************************************************
@@ -269,86 +283,101 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
     LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
 
     if(valid_obs >= 4)
+    {
+        arma::vec rx_position_and_time;
+        DLOG(INFO) << "satpos=" << satpos;
+        DLOG(INFO) << "obs=" << obs;
+        DLOG(INFO) << "W=" << W;
+
+        // 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
+                DLOG(INFO) << " Executing Bancroft algorithm...";
+                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 previous 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) << "Hybrid Position at TOW=" << hybrid_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]";
+
+        double secondsperweek = 604800.0;
+        // Compute GST and Gregorian time
+        if( GST != 0.0)
         {
-            arma::vec mypos;
-            DLOG(INFO) << "satpos=" << satpos;
-            DLOG(INFO) << "obs=" << obs;
-            DLOG(INFO) << "W=" << W;
-
-            mypos = leastSquarePos(satpos, obs, W);
-            d_rx_dt_s = mypos(3) / GPS_C_m_s; // Convert RX time offset from meters to seconds
-            double secondsperweek = 604800.0;
-            // Compute GST and Gregorian time
-            if( GST != 0.0)
-                {
-                    utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
-                }
-            else
-                {
-                    utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
-                }
-
-            // get time string Gregorian calendar
-            boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
-            // 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
-            boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
-            d_position_UTC_time = p_time;
+            utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
+        }
+        else
+        {
+            utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
+        }
 
-            DLOG(INFO) << "HYBRID Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z) = " << mypos;
+        // get time string Gregorian calendar
+        boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
+        // 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
+        boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
+        d_position_UTC_time = p_time;
 
-            cart2geo(static_cast<double>(mypos(0)), static_cast<double>(mypos(1)), static_cast<double>(mypos(2)), 4);
+        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);
 
-            DLOG(INFO) << "Hybrid 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]";
+        DLOG(INFO) << "Hybrid 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 ########
-            hybrid_ls_pvt::compute_DOP();
+        // ###### Compute DOPs ########
+        hybrid_ls_pvt::compute_DOP();
 
-            // ######## LOG FILE #########
-            if(d_flag_dump_enabled == true)
-                {
-                    // MULTIPLEXED FILE RECORDING - Record results to file
-                    try
-                    {
-                            double tmp_double;
-                            //  PVT GPS time
-                            tmp_double = hybrid_current_time;
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // ECEF User Position East [m]
-                            tmp_double = mypos(0);
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // ECEF User Position North [m]
-                            tmp_double = mypos(1);
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // ECEF User Position Up [m]
-                            tmp_double = mypos(2);
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // User clock offset [s]
-                            tmp_double = mypos(3);
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // GEO user position Latitude [deg]
-                            tmp_double = d_latitude_d;
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // GEO user position Longitude [deg]
-                            tmp_double = d_longitude_d;
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                            // GEO user position Height [m]
-                            tmp_double = d_height_m;
-                            d_dump_file.write((char*)&tmp_double, sizeof(double));
-                    }
-                    catch (const std::ifstream::failure& e)
-                    {
-                            LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
-                    }
-                }
-
-            // MOVING AVERAGE PVT
-            pos_averaging(flag_averaging);
-        }
-    else
+        // ######## LOG FILE #########
+        if(d_flag_dump_enabled == true)
         {
-            b_valid_position = false;
+            // MULTIPLEXED FILE RECORDING - Record results to file
+            try
+            {
+                double tmp_double;
+                //  PVT GPS time
+                tmp_double = hybrid_current_time;
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // ECEF User Position East [m]
+                tmp_double = rx_position_and_time(0);
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // ECEF User Position North [m]
+                tmp_double = rx_position_and_time(1);
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // ECEF User Position Up [m]
+                tmp_double = rx_position_and_time(2);
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // User clock offset [s]
+                tmp_double = rx_position_and_time(3);
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // GEO user position Latitude [deg]
+                tmp_double = d_latitude_d;
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // GEO user position Longitude [deg]
+                tmp_double = d_longitude_d;
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+                // GEO user position Height [m]
+                tmp_double = d_height_m;
+                d_dump_file.write((char*)&tmp_double, sizeof(double));
+            }
+            catch (const std::ifstream::failure& e)
+            {
+                LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
+            }
         }
+
+        // MOVING AVERAGE PVT
+        pos_averaging(flag_averaging);
+    }
+    else
+    {
+        b_valid_position = false;
+    }
     return b_valid_position;
 }
diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
index 9a88419..90fd8c6 100644
--- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
+++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
@@ -196,7 +196,6 @@ int hybrid_observables_cc::general_work (int noutput_items,
             DLOG(INFO) << "d_TOW_hybrid_reference [ms] = " << d_TOW_reference * 1000;
             double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms;
             DLOG(INFO) << "ref_PRN_rx_time_ms [ms] = " << d_ref_PRN_rx_time_ms;
-            //int reference_channel= gnss_synchro_iter->second.Channel_ID;
 
             // Now compute RX time differences due to the PRN alignment in the correlators
             for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
@@ -235,8 +234,6 @@ int hybrid_observables_cc::general_work (int noutput_items,
                             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);
@@ -249,10 +246,8 @@ int hybrid_observables_cc::general_work (int noutput_items,
                             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];
                         }

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