mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-26 20:53:20 +00:00
Code Issues Releases Wiki Activity

Adding consistency checks to the PVT solutions

Browse Source
This commit is contained in:
Javier Arribas 2017-02-06 17:51:11 +01:00
parent 0eaea3d563
commit 8c2f1f992f
4 changed files with 405 additions and 385 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
View File

@ -164,7 +164,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
DLOG(INFO) << "satpos=" << satpos; DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs="<< obs; DLOG(INFO) << "obs="<< obs;
DLOG(INFO) << "W=" << W; DLOG(INFO) << "W=" << W;
try{
// check if this is the initial position computation // check if this is the initial position computation
if (d_rx_dt_s == 0) if (d_rx_dt_s == 0)
{ {
@ -239,6 +239,12 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
// MOVING AVERAGE PVT // MOVING AVERAGE PVT
galileo_e1_ls_pvt::pos_averaging(flag_averaging); 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 else
{ {

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

@ -165,6 +165,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
DLOG(INFO) << "obs=" << obs; DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W; DLOG(INFO) << "W=" << W;
try{
// check if this is the initial position computation // check if this is the initial position computation
if (d_rx_dt_s == 0) if (d_rx_dt_s == 0)
{ {
@ -239,6 +240,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
// MOVING AVERAGE PVT // MOVING AVERAGE PVT
pos_averaging(flag_averaging); 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 else
{ {

8
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@ -288,7 +288,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
DLOG(INFO) << "satpos=" << satpos; DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs=" << obs; DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W; DLOG(INFO) << "W=" << W;
try{
// check if this is the initial position computation // check if this is the initial position computation
if (d_rx_dt_s == 0) if (d_rx_dt_s == 0)
{ {
@ -374,6 +374,12 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// MOVING AVERAGE PVT // MOVING AVERAGE PVT
pos_averaging(flag_averaging); 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 else
{ {

14
src/algorithms/PVT/libs/ls_pvt.cc
View File

@ -31,6 +31,7 @@
#include "ls_pvt.h" #include "ls_pvt.h"
#include <exception> #include <exception>
#include <stdexcept>
#include "GPS_L1_CA.h" #include "GPS_L1_CA.h"
#include <gflags/gflags.h> #include <gflags/gflags.h>
#include <glog/logging.h> #include <glog/logging.h>
@ -284,16 +285,15 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
} }
} }
try
{
//-- compute the Dilution Of Precision values //-- compute the Dilution Of Precision values
d_Q = arma::inv(arma::htrans(A) * A); d_Q = arma::inv(arma::htrans(A) * A);
}
catch(std::exception& e)
{
d_Q = arma::zeros(4,4);
}
// 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!");
}
return pos; return pos;
} }