[hamradio-commits] [gnss-sdr] 295/303: Code cleaning

Carles Fernandez carles_fernandez-guest at moszumanska.debian.org
Mon Feb 13 22:36:09 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 dbe08ade8c73d55f992f0411b3729c1333914ad5
Author: Carles Fernandez <carles.fernandez at gmail.com>
Date:   Mon Feb 6 19:24:17 2017 +0100

    Code cleaning
---
 src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc | 270 +++++++-------
 src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc  | 274 +++++++-------
 src/algorithms/PVT/libs/hybrid_ls_pvt.cc     | 520 +++++++++++++--------------
 src/algorithms/PVT/libs/ls_pvt.cc            | 272 +++++++-------
 4 files changed, 669 insertions(+), 667 deletions(-)

diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
index 4b08cd9..7041b83 100644
--- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
@@ -99,58 +99,58 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
     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
         {
-            DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first;
+            // 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
+                {
+                    DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first;
+                }
         }
-    }
 
     // ********************************************************************************
     // ****** SOLVE LEAST SQUARES******************************************************
@@ -159,97 +159,99 @@ 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;
-        try{
-            // check if this is the initial position computation
-            if (d_rx_dt_s == 0)
+        {
+            arma::vec rx_position_and_time;
+            DLOG(INFO) << "satpos=" << satpos;
+            DLOG(INFO) << "obs="<< obs;
+            DLOG(INFO) << "W=" << W;
+            try
             {
-                // 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]
+                    // 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) / GALILEO_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) / 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)
+                        {
+                            // 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);
             }
-            // 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)
+            catch(const std::exception & e)
             {
-                // 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();
-                }
+                    d_rx_dt_s = 0; //reset rx time estimation
+                    LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
+                    b_valid_position = false;
             }
-
-            // MOVING AVERAGE PVT
-            galileo_e1_ls_pvt::pos_averaging(flag_averaging);
-        }catch(const std::exception & e)
+        }
+    else
         {
-            d_rx_dt_s=0;//reset rx time estimation
-            LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
             b_valid_position = false;
         }
-    }
-    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 72d87d8..a0d197d 100644
--- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
@@ -99,58 +99,58 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
     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
-        gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->first);
-        if (gps_ephemeris_iter != gps_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 = GPS_current_time;
-            double Tx_time = Rx_time - gnss_pseudoranges_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_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;
-
-            // 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 // the ephemeris are not available for this SV
         {
-            DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first;
+            // 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
+            gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->first);
+            if (gps_ephemeris_iter != gps_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 = GPS_current_time;
+                    double Tx_time = Rx_time - gnss_pseudoranges_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_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;
+
+                    // 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 // the ephemeris are not available for this SV
+                {
+                    DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first;
+                }
         }
-    }
 
     // ********************************************************************************
     // ****** SOLVE LEAST SQUARES******************************************************
@@ -159,99 +159,101 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
     LOG(INFO) << "(new)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;
-
-        try{
-            // 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) << "(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]";
-
-            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]";
+        {
+            arma::vec rx_position_and_time;
+            DLOG(INFO) << "satpos=" << satpos;
+            DLOG(INFO) << "obs=" << obs;
+            DLOG(INFO) << "W=" << W;
 
-            // ###### Compute DOPs ########
-            compute_DOP();
+            try
+            {
+                    // 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) << "(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]";
+
+                    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 ########
+                    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 = GPS_current_time;
+                                    d_dump_file.write((char*)&tmp_double, sizeof(double));
+                                    // ECEF User Position East [m]
+                                    tmp_double = d_rx_pos(0);
+                                    d_dump_file.write((char*)&tmp_double, sizeof(double));
+                                    // ECEF User Position North [m]
+                                    tmp_double = d_rx_pos(1);
+                                    d_dump_file.write((char*)&tmp_double, sizeof(double));
+                                    // ECEF User Position Up [m]
+                                    tmp_double = d_rx_pos(2);
+                                    d_dump_file.write((char*)&tmp_double, sizeof(double));
+                                    // User clock offset [s]
+                                    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;
+                                    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);
 
-            // ######## LOG FILE #########
-            if(d_flag_dump_enabled == true)
+            }
+            catch(const std::exception & e)
             {
-                // MULTIPLEXED FILE RECORDING - Record results to file
-                try
-                {
-                    double tmp_double;
-                    //  PVT GPS time
-                    tmp_double = GPS_current_time;
-                    d_dump_file.write((char*)&tmp_double, sizeof(double));
-                    // ECEF User Position East [m]
-                    tmp_double = d_rx_pos(0);
-                    d_dump_file.write((char*)&tmp_double, sizeof(double));
-                    // ECEF User Position North [m]
-                    tmp_double = d_rx_pos(1);
-                    d_dump_file.write((char*)&tmp_double, sizeof(double));
-                    // ECEF User Position Up [m]
-                    tmp_double = d_rx_pos(2);
-                    d_dump_file.write((char*)&tmp_double, sizeof(double));
-                    // User clock offset [s]
-                    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;
-                    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();
-                }
+                    d_rx_dt_s = 0; //reset rx time estimation
+                    LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
+                    b_valid_position = false;
             }
-
-            // MOVING AVERAGE PVT
-            pos_averaging(flag_averaging);
-
-        }catch(const std::exception & e)
+        }
+    else
         {
-            d_rx_dt_s=0;//reset rx time estimation
-            LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
             b_valid_position = false;
         }
-    }
-    else
-    {
-        b_valid_position = false;
-    }
     return b_valid_position;
 }
 
diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
index c55d055..09fc2b6 100644
--- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
@@ -100,180 +100,176 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
     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':
-        {
-            // 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())
-                {
-                    /*!
-                     * \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 // the ephemeris are not available for this SV
-                {
-                    DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
-                }
-            }
-            if(sig_.compare("2S") == 0)
+            switch(gnss_observables_iter->second.System)
             {
-                gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
-                if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
+            case 'E':
                 {
-                    /*!
-                     * \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++;
+                    // 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;
                 }
-                else // the ephemeris are not available for this SV
+            case 'G':
                 {
-                    DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
+                    // 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.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 // the ephemeris are not available for this SV
+                                {
+                                    DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
+                                }
+                        }
+                    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;
             }
-            break;
-        }
-        default :
-            DLOG(INFO) << "Hybrid observables: Unknown GNSS";
-            break;
         }
-    }
 
     // ********************************************************************************
     // ****** SOLVE LEAST SQUARES******************************************************
@@ -283,107 +279,109 @@ 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;
-        try{
-            // 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 rx_position_and_time;
+            DLOG(INFO) << "satpos=" << satpos;
+            DLOG(INFO) << "obs=" << obs;
+            DLOG(INFO) << "W=" << W;
+            try
             {
-                utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
+                    // 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)
+                        {
+                            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;
+
+                    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]";
+
+                    // ###### 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 = 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
+            catch(const std::exception & e)
             {
-                utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
+                    d_rx_dt_s = 0; //reset rx time estimation
+                    LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
+                    b_valid_position = false;
             }
-
-            // 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>(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]";
-
-            // ###### 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 = 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);
-        }catch(const std::exception & e)
+        }
+    else
         {
-            d_rx_dt_s=0;//reset rx time estimation
-            LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
             b_valid_position = false;
         }
-    }
-    else
-    {
-        b_valid_position = false;
-    }
     return b_valid_position;
 }
diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc
index ec97328..e4bdfdd 100644
--- a/src/algorithms/PVT/libs/ls_pvt.cc
+++ b/src/algorithms/PVT/libs/ls_pvt.cc
@@ -82,80 +82,80 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
     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);
-        }
+            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
 
-        arma::vec abs_omc = arma::zeros(2,1);
-        for (int j = 0; j < m; j++)
-        {
+            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++)
-            {
-                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
+                {
+                    possible_pos.col(i) = r(i) * BBBe + BBBalpha;
+                    possible_pos(3,i) = -possible_pos(3,i);
+                }
 
-        //discrimination between roots
-        if (abs_omc(0) > abs_omc(1))
-        {
-            pos = possible_pos.col(1);
-        }
-        else
-        {
-            pos = possible_pos.col(0);
-        }
-    }// % iter loop
+            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;
 }
 
@@ -164,7 +164,7 @@ 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
-    // Based ob code by:
+    // Based on code by:
     // Kai Borre 04-22-95
     // Copyright (c) by Kai Borre
     // $Revision: 1.0 $ $Date: 1997/09/26  $
@@ -182,7 +182,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
      *   Inputs:
      *       satpos      - Satellites positions in ECEF system: [X; Y; Z;]
      *       obs         - Observations - the pseudorange measurements to each satellite
-     *       w           - weigths vector
+     *       w           - weight vector
      *
      *   Returns:
      *       pos         - receiver position and receiver clock error
@@ -218,82 +218,82 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
 
     //=== Iteratively find receiver position ===================================
     for (int iter = 0; iter < nmbOfIterations; iter++)
-    {
-        for (int i = 0; i < nmbOfSatellites; i++)
         {
-            if (iter == 0)
-            {
-                //--- Initialize variables at the first iteration --------------
-                Rot_X = X.col(i); //Armadillo
-                trop = 0.0;
-            }
-            else
-            {
-                //--- Update equations -----------------------------------------
-                rho2 = (X(0, i) - pos(0)) *
-                        (X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
-                        (X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
-                        (X(2, i) - pos(2));
-                traveltime = sqrt(rho2) / GPS_C_m_s;
-
-                //--- Correct satellite position (do to earth rotation) --------
-                Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
-
-                //--- Find DOA and range of satellites
-                Ls_Pvt::topocent(&d_visible_satellites_Az[i],
-                        &d_visible_satellites_El[i],
-                        &d_visible_satellites_Distance[i],
-                        pos.subvec(0,2),
-                        Rot_X - pos.subvec(0, 2));
-                if(traveltime < 0.1 && nmbOfSatellites > 3)
+            for (int i = 0; i < nmbOfSatellites; i++)
                 {
-                    //--- Find receiver's height
-                    Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
-                    // Add troposphere correction if the receiver is below the troposphere
-                    if (h > 15000)
-                    {
-                        //receiver is above the troposphere
-                        trop = 0.0;
-                    }
+                    if (iter == 0)
+                        {
+                            //--- Initialize variables at the first iteration --------------
+                            Rot_X = X.col(i); //Armadillo
+                            trop = 0.0;
+                        }
                     else
-                    {
-                        //--- Find delay due to troposphere (in meters)
-                        Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
-                        if(trop > 5.0 ) trop = 0.0; //check for erratic values
-                    }
-                }
-            }
-            //--- Apply the corrections ----------------------------------------
-            omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
+                        {
+                            //--- Update equations -----------------------------------------
+                            rho2 = (X(0, i) - pos(0)) *
+                                   (X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
+                                   (X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
+                                   (X(2, i) - pos(2));
+                            traveltime = sqrt(rho2) / GPS_C_m_s;
 
-            //--- Construct the A matrix ---------------------------------------
-            //Armadillo
-            A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
-            A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
-            A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
-            A(i,3) = 1.0;
-        }
+                            //--- Correct satellite position (do to earth rotation) --------
+                            Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
 
-        //--- Find position update ---------------------------------------------
-        x = arma::solve(w*A, w*omc); // Armadillo
+                            //--- Find DOA and range of satellites
+                            Ls_Pvt::topocent(&d_visible_satellites_Az[i],
+                                    &d_visible_satellites_El[i],
+                                    &d_visible_satellites_Distance[i],
+                                    pos.subvec(0,2),
+                                    Rot_X - pos.subvec(0, 2));
+                            if(traveltime < 0.1 && nmbOfSatellites > 3)
+                                {
+                                    //--- Find receiver's height
+                                    Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
+                                    // Add troposphere correction if the receiver is below the troposphere
+                                    if (h > 15000)
+                                        {
+                                            //receiver is above the troposphere
+                                            trop = 0.0;
+                                        }
+                                    else
+                                        {
+                                            //--- Find delay due to troposphere (in meters)
+                                            Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
+                                            if(trop > 5.0 ) trop = 0.0; //check for erratic values
+                                        }
+                                }
+                        }
+                    //--- Apply the corrections ----------------------------------------
+                    omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
 
-        //--- Apply position update --------------------------------------------
-        pos = pos + x;
-        if (arma::norm(x,2) < 1e-4)
-        {
-            break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
+                    //--- Construct the A matrix ---------------------------------------
+                    //Armadillo
+                    A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
+                    A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
+                    A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
+                    A(i,3) = 1.0;
+                }
+
+            //--- Find position update ---------------------------------------------
+            x = arma::solve(w*A, w*omc); // Armadillo
+
+            //--- Apply position update --------------------------------------------
+            pos = pos + x;
+            if (arma::norm(x,2) < 1e-4)
+                {
+                    break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
+                }
         }
-    }
 
     //-- compute the Dilution Of Precision values
     d_Q = arma::inv(arma::htrans(A) * A);
 
     // check the consistency of the PVT solution
-    if (((fabs(pos(3))*1000.0)/ GPS_C_m_s)>GPS_STARTOFFSET_ms*2)
-    {
-        LOG(WARNING)<<"Receiver time offset out of range! Estimated RX Time error [s]:"<<pos(3)/ GPS_C_m_s;
-        throw std::runtime_error("Receiver time offset out of range!");
-    }
+    if (((fabs(pos(3)) * 1000.0) / GPS_C_m_s) > GPS_STARTOFFSET_ms * 2)
+        {
+            LOG(WARNING) << "Receiver time offset out of range! Estimated RX Time error [s]:" << pos(3) / GPS_C_m_s;
+            throw std::runtime_error("Receiver time offset out of range!");
+        }
     return pos;
 }
 

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