From 235aa7735795d8ae32e0d70f820e1cb81097f161 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 11 Jan 2017 17:31:22 +0100 Subject: [PATCH] Bug fix in LS PVT solver in troposphere corrections causing erratic position fixes in high altitude GNSS receiver operations (>15 km) --- src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc | 6 ------ src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc | 9 +-------- src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 11 +---------- src/algorithms/PVT/libs/ls_pvt.cc | 13 ++++++++++--- 4 files changed, 12 insertions(+), 27 deletions(-) diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc index 5a7bb82b2..7517cae5f 100644 --- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc +++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc @@ -178,12 +178,6 @@ bool galileo_e1_ls_pvt::get_PVT(std::map gnss_pseudoranges_map cart2geo(static_cast(mypos(0)), static_cast(mypos(1)), static_cast(mypos(2)), 4); d_rx_dt_m = mypos(3)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds - //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km) - if (d_height_m > 50000) - { - b_valid_position = false; - return false; - } 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_m << " [s]"; 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 89f73c9e1..d888948cb 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -174,13 +174,6 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, d_rx_dt_m = mypos(3)/GPS_C_m_s; // Convert RX time offset from meters to seconds - //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km) - if (d_height_m > 50000) - { - DLOG(INFO)<<"LS: bad height\n"; - b_valid_position = false; - return false; - } // 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(GPS_week)); @@ -188,7 +181,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); d_position_UTC_time = p_time; - LOG(INFO) << "(new)Position at " << boost::posix_time::to_simple_string(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_m << " [s]"; diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 12bb7c231..ea16dbfc0 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -297,17 +297,8 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou DLOG(INFO) << "HYBRID Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z) = " << mypos; cart2geo(static_cast(mypos(0)), static_cast(mypos(1)), static_cast(mypos(2)), 4); - //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km) - if (d_height_m > 50000) - { - b_valid_position = false; - LOG(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= " << mypos(3) << " [s]"; - return false; - } - LOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time) + 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_m << " [s]"; diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc index d2251e29a..1c92d4def 100644 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ b/src/algorithms/PVT/libs/ls_pvt.cc @@ -116,10 +116,17 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs { //--- 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 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 > 50.0 ) trop = 0.0; } } //--- Apply the corrections ----------------------------------------