[hamradio-commits] [gnss-sdr] 292/303: Adding consistency checks to the PVT solutions

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 8c2f1f992f34af2fd41d75ecf0b1fa653292ed51
Author: Javier Arribas <javiarribas at gmail.com>
Date:   Mon Feb 6 17:51:11 2017 +0100

    Adding consistency checks to the PVT solutions
---
 src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc | 136 ++++++-------
 src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc  | 232 ++++++++++++-----------
 src/algorithms/PVT/libs/hybrid_ls_pvt.cc     | 150 ++++++++-------
 src/algorithms/PVT/libs/ls_pvt.cc            | 274 +++++++++++++--------------
 4 files changed, 406 insertions(+), 386 deletions(-)

diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
index 8f60917..4b08cd9 100644
--- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
@@ -164,81 +164,87 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
         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) / 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);
 
-        // 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]
+            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;
+            // 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;
+            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]";
+            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();
+            // ###### 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 FILE #########
+            if(d_flag_dump_enabled == true)
             {
-                LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
+                // 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);
+            // MOVING AVERAGE PVT
+            galileo_e1_ls_pvt::pos_averaging(flag_averaging);
+        }catch(const std::exception & e)
+        {
+            d_rx_dt_s=0;//reset rx time estimation
+            LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
+            b_valid_position = false;
+        }
     }
     else
     {
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 1633f6f..72d87d8 100644
--- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
@@ -49,21 +49,21 @@ gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels, std::string dump_filename, boo
 
     // ############# 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();
+            }
         }
+    }
 }
 
 
@@ -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())
         {
-            // 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;
-                }
+            /*!
+             * \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,21 +159,22 @@ 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;
+    {
+        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 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);
@@ -193,57 +194,64 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
             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]";
+            << " 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)
                 {
-                    // 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();
-                    }
+                    LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
                 }
+            }
 
             // MOVING AVERAGE PVT
             pos_averaging(flag_averaging);
-        }
-    else
+
+        }catch(const std::exception & e)
         {
+            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 87dae62..c55d055 100644
--- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
@@ -201,17 +201,17 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
 
                     // 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]";
+                            << " 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
+                else // the ephemeris are not available for this SV
                 {
                     DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
                 }
@@ -288,9 +288,9 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
         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)
+        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...";
@@ -299,81 +299,87 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
                 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);
+            // 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]
+            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]";
+            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);
-        }
+            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;
+            // 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);
+            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 = 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 FILE #########
+            if(d_flag_dump_enabled == true)
             {
-                LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
+                // 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);
+            // MOVING AVERAGE PVT
+            pos_averaging(flag_averaging);
+        }catch(const std::exception & e)
+        {
+            d_rx_dt_s=0;//reset rx time estimation
+            LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
+            b_valid_position = false;
+        }
     }
     else
     {
diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc
index ead1e10..ec97328 100644
--- a/src/algorithms/PVT/libs/ls_pvt.cc
+++ b/src/algorithms/PVT/libs/ls_pvt.cc
@@ -31,6 +31,7 @@
 
 #include "ls_pvt.h"
 #include <exception>
+#include <stdexcept>
 #include "GPS_L1_CA.h"
 #include <gflags/gflags.h>
 #include <glog/logging.h>
@@ -81,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++)
         {
-            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();
-                }
+            int x = B(i,0);
+            int y = B(i,1);
+            if (iter == 0)
+            {
+                traveltime = 0.072;
+            }
             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);
-                }
+            {
+                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++)
-                {
-                    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
+        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);
+        }
 
-            //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;
 }
 
@@ -217,83 +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++)
         {
-            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
+            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;
 
-                            //--- 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
+                //--- Correct satellite position (do to earth rotation) --------
+                Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
 
-                    //--- 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 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
+                    }
                 }
-
-            //--- 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)
             }
+            //--- Apply the corrections ----------------------------------------
+            omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
+
+            //--- 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;
         }
 
-    try
-    {
-            //-- compute the Dilution Of Precision values
-            d_Q = arma::inv(arma::htrans(A) * A);
+        //--- 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)
+        }
     }
-    catch(std::exception& e)
+
+    //-- 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)
     {
-            d_Q = arma::zeros(4,4);
+        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