diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc index 71f26a4a1..439d43604 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc @@ -223,6 +223,8 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) << " is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; + std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " is HDOP = " << d_ls_pvt->d_HDOP << " and VDOP = " << d_ls_pvt->d_VDOP << std::endl; } if(d_dump == true) 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 09e931408..e2fb97245 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -216,6 +216,24 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma //--- Apply position update -------------------------------------------- pos = pos + x; } + + try{ + //-- compute the Dilution Of Precision values + arma::mat Q; + Q = arma::inv(arma::htrans(A)*A); + d_GDOP = sqrt(arma::trace(Q)); // GDOP + d_PDOP = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); // PDOP + d_HDOP = sqrt(Q(1,1) + Q(2,2)); // HDOP + d_VDOP = sqrt(Q(3,3)); // VDOP + d_TDOP = sqrt(Q(4,4)); // TDOP + }catch(std::exception e) + { + d_GDOP = -1; + d_PDOP = -1; + d_HDOP = -1; + d_VDOP = -1; + d_TDOP = -1; + } return pos; } diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h index 63526873d..3fe375259 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h @@ -68,6 +68,7 @@ public: double d_latitude_d; //! Latitude in degrees double d_longitude_d; //! Longitude in degrees double d_height_m; //! Height [m] + //averaging std::deque d_hist_latitude_d; std::deque d_hist_longitude_d; @@ -82,6 +83,14 @@ public: double d_y_m; double d_z_m; + // DOP estimations + + double d_GDOP; + double d_PDOP; + double d_HDOP; + double d_VDOP; + double d_TDOP; + bool d_flag_dump_enabled; std::string d_dump_filename; std::ofstream d_dump_file;