mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 15:23:04 +00:00
Code Issues Releases Wiki Activity

Bug correction in the Dilution of Precision (DOP) computation of Least Squares PVT solver: Missing transformation from ECEF to ENU in the covariance matrix.

Browse Source 
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@414 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas
2013-09-02 15:43:00 +00:00
parent 02eccce680
commit 31f73ed40d
3 changed files with 48 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
View File

@@ -217,7 +217,9 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; << " [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) 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; << " is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = " <<
d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP <<
" GDOP = " << d_ls_pvt->d_GDOP <<std::endl;
} }
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
if(d_dump == true) if(d_dump == true)

57
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
View File

@@ -216,21 +216,12 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
try try
{ {
//-- compute the Dilution Of Precision values //-- compute the Dilution Of Precision values
arma::mat Q; //arma::mat Q;
Q = arma::inv(arma::htrans(A)*A); d_Q = arma::inv(arma::htrans(A)*A);
d_GDOP = sqrt(arma::trace(Q)); // Geometric DOP
d_PDOP = sqrt(Q(0,0) + Q(1,1) + Q(2,2)); // PDOP
d_HDOP = sqrt(Q(0,0) + Q(1,1)); // HDOP
d_VDOP = sqrt(Q(2,2)); // VDOP
d_TDOP = sqrt(Q(3,3)); // TDOP
} }
catch(std::exception e) catch(std::exception& e)
{ {
d_GDOP = -1; d_Q=arma::zeros(4,4);
d_PDOP = -1;
d_HDOP = -1;
d_VDOP = -1;
d_TDOP = -1;
} }
return pos; return pos;
} }
@@ -353,6 +344,46 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << std::endl; << " [deg], Height= " << d_height_m << " [m]" << std::endl;
// ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat F=arma::zeros(3,3);
F(0,0)=-sin(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,1)=-sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,2)=cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(1,0)=cos((GPS_TWO_PI*d_longitude_d)/360.0);
F(1,1)=-sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(1,2)=cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(2,0)=0;
F(2,1)=cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(2,2)=sin((GPS_TWO_PI*d_latitude_d/360.0));
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF=d_Q.submat( 0, 0, 2, 2);
arma::mat DOP_ENU=arma::zeros(3,3);
try
{
DOP_ENU=arma::htrans(F)*Q_ECEF*F;
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP
d_TDOP = sqrt(d_Q(3,3)); // TDOP
}catch(std::exception& ex)
{
d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
// ######## LOG FILE ######### // ######## LOG FILE #########
if(d_flag_dump_enabled == true) if(d_flag_dump_enabled == true)
{ {

1
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h
View File

@@ -106,6 +106,7 @@ public:
// DOP estimations // DOP estimations
arma::mat d_Q;
double d_GDOP; double d_GDOP;
double d_PDOP; double d_PDOP;
double d_HDOP; double d_HDOP;