Skip to content
Snippets Groups Projects
Commit 0fd3b53d authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

wip

parent 8189d819
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/gnss_utils.h
+ 28
− 1
View file @ 0fd3b53d
......@@ -18,12 +18,39 @@ extern "C" {
namespace GnssUtils
{
// Structs
struct PseudoRange
{
double prange = -1;
double var = 1;
};
struct ComputePosOutput
{
time_t time;
double sec;
Eigen::Vector3d pos; // position (m)
Eigen::Vector3d vel; // velocity (m/s)
Eigen::Matrix3d pos_covar; // position covariance (m^2)
// {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s)
int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
int stat; // solution status (SOLQ_???)
int ns; // number of valid satellites
double age; // age of differential (s)
double ratio; // AR ratio factor for valiation
int pos_stat; // return from pntpos
Eigen::Vector3d lat_lon; // latitude_longitude_altitude
std::map<int,Eigen::Vector2d> sat_azel; // azimuth and elevation of each satellite provided
};
// forward declarations
class Observations;
class Navigation;
class Snapshot;
class Satellite;
class PseudoRange;
// Typedefs
typedef std::map<int,Satellite> Satellites;
......
include/gnss_utils/observations.h
+ 17
− 17
View file @ 0fd3b53d
......@@ -46,9 +46,9 @@ public:
const Satellites& sats,
const snrmask_t& snrmask,
const double& elmin);
std::set<int> filterByElevationSnr(const std::map<int,double>& elevations,
const snrmask_t& snrmask,
const double& elmin);
std::set<int> filterByElevationSnr(const std::map<int,Eigen::Vector2d>& azels,
const snrmask_t& snrmask,
const double& elmin);
std::set<int> filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
......@@ -63,20 +63,20 @@ public:
const int& navsys,
const snrmask_t& snrmask,
const double& elmin);
std::set<int> filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const std::map<int,double>& elevations,
const bool& check_code,
const bool& check_carrier_phase,
const prcopt_t& opt);
std::set<int> filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const std::map<int,double>& elevations,
const bool& check_code,
const bool& check_carrier_phase,
const int& navsys,
const snrmask_t& snrmask,
const double& elmin);
std::set<int> filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const std::map<int,Eigen::Vector2d>& azels,
const bool& check_code,
const bool& check_carrier_phase,
const prcopt_t& opt);
std::set<int> filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const std::map<int,Eigen::Vector2d>& azels,
const bool& check_code,
const bool& check_carrier_phase,
const int& navsys,
const snrmask_t& snrmask,
const double& elmin);
// Others
static std::set<int> findCommonObservations(const Observations& obs_1, const Observations& obs_2);
......
include/gnss_utils/snapshot.h
+ 0
− 7
View file @ 0fd3b53d
......@@ -14,12 +14,6 @@
namespace GnssUtils
{
struct PseudoRange
{
double prange = -1;
double var = -1;
};
class Snapshot
{
......@@ -56,7 +50,6 @@ public:
bool pseudoRangesComputed() const;
void computePseudoRanges(const std::map<int,Eigen::Vector2d> azel,
const Eigen::Vector3d latlonalt,
const double rcv_clock_bias,
const prcopt_t& opt);
void print();
......
include/gnss_utils/utils/rcv_position.h
+ 0
− 20
View file @ 0fd3b53d
......@@ -19,26 +19,6 @@
namespace GnssUtils
{
struct ComputePosOutput
{
time_t time;
double sec;
Eigen::Vector3d pos; // position (m)
Eigen::Vector3d vel; // velocity (m/s)
Eigen::Matrix3d pos_covar; // position covariance (m^2)
// {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s)
int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
int stat; // solution status (SOLQ_???)
int ns; // number of valid satellites
double age; // age of differential (s)
double ratio; // AR ratio factor for valiation
int pos_stat; // return from pntpos
Eigen::Vector3d lat_lon; // latitude_longitude_altitude
std::map<int,Eigen::Vector2d> sat_azel; // azimuth and elevation of each satellite provided
};
ComputePosOutput computePos(const Observations& _observations, Navigation& _navigation, const prcopt_t& _prcopt);
......
include/gnss_utils/utils/satellite.h
+ 1
− 19
View file @ 0fd3b53d
......@@ -19,8 +19,6 @@ namespace GnssUtils
class Observations;
class Navigation;
typedef std::map<int,Eigen::Vector3d> SatellitesPositions;
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
Satellites computeSatellites(const Observations& obs,
......@@ -36,23 +34,7 @@ namespace GnssUtils
double var;
double clock_bias;
double clock_drift;
Satellite(const int& _sys,
const int& _sat,
const Eigen::Vector3d& _pos,
const Eigen::Vector3d& _vel,
const double& _var,
const double& _clock_bias,
const double& _clock_drift) :
sys(_sys),
sat(_sat),
pos(_pos),
vel(_vel),
var(_var),
clock_bias(_clock_bias),
clock_drift(_clock_drift)
{
};
int svh;
};
} // namespace GnssUtils
......
src/observations.cpp
+ 38
− 21
View file @ 0fd3b53d
......@@ -170,14 +170,14 @@ std::set<int> Observations::filterByEphemeris(const Satellites& sats)
// inexistent satellite (satellite is not included in the discarded list)
if (sats.count(sat_number) == 0)
{
std::cout << "Discarding sat " << sat_number << ": no information available" << std::endl;
//std::cout << "Discarding sat " << sat_number << ": no information available" << std::endl;
remove_sats.insert(sat_number);
}
// bad satellite position (satellite is not included in the discarded list)
if (sats.at(sat_number).pos.isApprox(Eigen::Vector3d::Zero(), 1e-3) or
sats.at(sat_number).pos.norm() < 2.5e7)
{
std::cout << "Discarding sat " << sat_number << ": wrong satellite position: \n\t" << sats.at(sat_number).pos.transpose() << std::endl;
//std::cout << "Discarding sat " << sat_number << ": wrong satellite position: " << sats.at(sat_number).pos.transpose() << std::endl;
remove_sats.insert(sat_number);
}
}
......@@ -304,7 +304,7 @@ std::set<int> Observations::filterByConstellations(const int& navsys)
return remove_sats;
}
std::set<int> Observations::filterByElevationSnr(const std::map<int,double>& elevations,
std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vector2d>& azels,
const snrmask_t& snrmask,
const double& elmin)
{
......@@ -314,9 +314,10 @@ std::set<int> Observations::filterByElevationSnr(const std::map<int,double>& ele
{
auto&& obs_sat = getObservationByIdx(obs_i);
const int& sat_number = obs_sat.sat;
const double& elevation(azels.at(sat_number)(1));
// check elevation
if (elevations.at(sat_number) < elmin)
if (elevation < elmin)
{
//std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << elmin << std::endl;
remove_sats.insert(sat_number);
......@@ -324,7 +325,7 @@ std::set<int> Observations::filterByElevationSnr(const std::map<int,double>& ele
}
// snr TODO: multifrequency (2nd param and 3rd idx)
if (testsnr(0, 0, elevations.at(sat_number), obs_sat.SNR[0] * 0.25, &snrmask) == 1)
if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &snrmask) == 1)
{
//std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
remove_sats.insert(sat_number);
......@@ -430,6 +431,14 @@ std::set<int> Observations::filter(const Satellites& sats,
remove_sats.insert(remove_sats_carrier.begin(), remove_sats_carrier.end());
}
// satellite flag and ephemeris variance
for (auto sat_pair : sats)
if (satexclude(sat_pair.first,sat_pair.second.var, sat_pair.second.svh, NULL))
{
//std::cout << "Discarding sat " << sat_pair.first << ": unhealthy or huge variance svh = " << sat_pair.second.svh << std::endl;
remove_sats.insert(sat_pair.first);
}
// Constellations
std::set<int> remove_sats_constellations = filterByConstellations(navsys);
remove_sats.insert(remove_sats_constellations.begin(), remove_sats_constellations.end());
......@@ -444,16 +453,16 @@ std::set<int> Observations::filter(const Satellites& sats,
// std::cout << "final size: " << obs_.size() << std::endl;
}
std::set<int> Observations::filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const std::map<int,double>& elevations,
const bool& check_code,
const bool& check_carrier_phase,
const prcopt_t& opt)
std::set<int> Observations::filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const std::map<int,Eigen::Vector2d>& azels,
const bool& check_code,
const bool& check_carrier_phase,
const prcopt_t& opt)
{
return filter(sats,
discarded_sats,
elevations,
azels,
check_code,
check_carrier_phase,
opt.navsys,
......@@ -461,14 +470,14 @@ std::set<int> Observations::filter(const Satellites& sats,
opt.elmin);
}
std::set<int> Observations::filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const std::map<int,double>& elevations,
const bool& check_code,
const bool& check_carrier_phase,
const int& navsys,
const snrmask_t& snrmask,
const double& elmin)
std::set<int> Observations::filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const std::map<int,Eigen::Vector2d>& azels,
const bool& check_code,
const bool& check_carrier_phase,
const int& navsys,
const snrmask_t& snrmask,
const double& elmin)
{
//std::cout << "filter: initial size: " << obs_.size() << std::endl;
// Ephemeris
......@@ -492,12 +501,20 @@ std::set<int> Observations::filter(const Satellites& sats,
remove_sats.insert(remove_sats_carrier.begin(), remove_sats_carrier.end());
}
// satellite flag and ephemeris variance
for (auto sat_pair : sats)
if (satexclude(sat_pair.first,sat_pair.second.var, sat_pair.second.svh, NULL))
{
//std::cout << "Discarding sat " << sat_pair.first << ": unhealthy or huge variance svh = " << sat_pair.second.svh << std::endl;
remove_sats.insert(sat_pair.first);
}
// Constellations
std::set<int> remove_sats_constellations = filterByConstellations(navsys);
remove_sats.insert(remove_sats_constellations.begin(), remove_sats_constellations.end());
// Elevation and SNR
std::set<int> remove_sats_elevation = filterByElevationSnr(elevations, snrmask, elmin);
std::set<int> remove_sats_elevation = filterByElevationSnr(azels, snrmask, elmin);
remove_sats.insert(remove_sats_elevation.begin(), remove_sats_elevation.end());
return remove_sats;
......
src/snapshot.cpp
+ 37
− 6
View file @ 0fd3b53d
......@@ -33,32 +33,52 @@ void Snapshot::computeSatellites(const int& eph_opt)
void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d> azel,
const Eigen::Vector3d latlonalt,
const double rcv_clock_bias,
const prcopt_t& opt)
{
assert(pranges_.empty() && "pseudo ranges already computed!");
double dion,dtrp,vmeas,vion,vtrp,P,lam_L1;
//std::cout << "compute pseudo ranges: \n";
for (auto i = 0; i<obs_->size(); i++)
{
const obsd_t& obs_i(obs_->getObservationByIdx(i));
int sat = obs_i.sat;
const Satellite& sat_i(sats_.at(sat));
assert(azel.count(sat) != 0 && "azimuth and elevation not provided for this satellite");
double azel_i[2] = {azel.at(sat)(0),azel.at(sat)(1)};
//std::cout << "\tsat " << sat << "...";
// initialize with error values
pranges_.emplace(sat,PseudoRange());
/* psudorange with code bias correction */
if ((P=prange(&obs_i,&nav_->getNavigation(),azel_i,1,&opt,&vmeas))==0.0)
{
//std::cout << " error in prange\n";
continue;
}
/* ionospheric corrections */
if (!ionocorr(obs_i.time,&nav_->getNavigation(),sat,latlonalt.data(),azel_i, opt.ionoopt,&dion,&vion))
continue;
if (!ionocorr(obs_i.time,&nav_->getNavigation(),sat,latlonalt.data(),azel_i,opt.ionoopt,&dion,&vion))
{
if (opt.ionoopt == IONOOPT_SBAS)
{
//std::cout << "error IONOOPT_SBAS ionocorr, trying with IONOOPT_BRDC...";
if (!ionocorr(obs_i.time,&nav_->getNavigation(),sat,latlonalt.data(),azel_i,IONOOPT_BRDC,&dion,&vion))
{
//std::cout << " error in ionocorr\n";
continue;
}
}
else
{
//std::cout << " error in ionocorr\n";
continue;
}
}
/* GPS-L1 -> L1/B1 */
if ((lam_L1=nav_->getNavigation().lam[sat-1][0])>0.0)
......@@ -66,12 +86,23 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d> azel,
/* tropospheric corrections */
if (!tropcorr(obs_i.time,&nav_->getNavigation(),latlonalt.data(),azel_i,opt.tropopt,&dtrp,&vtrp))
{
//std::cout << " error in tropcorr\n";
continue;
}
/* pseudorange corrected */
pranges_.at(sat).prange = P-(rcv_clock_bias-CLIGHT*sat_i.clock_bias+dion+dtrp);
pranges_[sat].prange = P+CLIGHT*sat_i.clock_bias-dion-dtrp;
/* error variance */
pranges_.at(sat).var = varerr(&opt,azel_i[1],opt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp;
pranges_[sat].var = varerr(&opt,azel_i[1],opt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp;
//std::cout << std::endl
// << "\t\tprange: " << pranges_[sat].prange << std::endl
// << "\t\tvar: " << pranges_[sat].var << std::endl
// << "\t\tP: " << P << std::endl
// << "\t\tsat_i.clock_bias: " << sat_i.clock_bias << std::endl
// << "\t\tdion: " << dion << std::endl
// << "\t\tdtrp: " << dtrp << std::endl;
}
}
src/utils/satellite.cpp
+ 18
− 18
View file @ 0fd3b53d
......@@ -52,30 +52,30 @@ Satellites computeSatellites(const Observations& obs,
// fill Satellites
Satellites sats;
std::cout << "filling satellites: \n";
//std::cout << "filling satellites: \n";
for (int i = 0; i < obs.size(); i++)
{
auto sat_pair = sats.emplace(obs.getObservationByIdx(i).sat, // Key
Satellite(satsys(obs.getObservationByIdx(i).sat,NULL),
obs.getObservationByIdx(i).sat, // Constructor...
(Eigen::Vector3d() << rs[6*i], rs[6*i+1], rs[6*i+2]).finished(),
(Eigen::Vector3d() << rs[6*i+3], rs[6*i+4], rs[6*i+5]).finished(),
var[i],
dts[2*i],
dts[2*i+1]));
Satellite({satsys(obs.getObservationByIdx(i).sat,NULL),
obs.getObservationByIdx(i).sat, // Constructor...
(Eigen::Vector3d() << rs[6*i], rs[6*i+1], rs[6*i+2]).finished(),
(Eigen::Vector3d() << rs[6*i+3], rs[6*i+4], rs[6*i+5]).finished(),
var[i],
dts[2*i],
dts[2*i+1],
(svh[i]==-2 ? 0 : svh[i])})); // -2 = ephemeris without SBAS correction (remove negative error code)
assert(sat_pair.second && "satellite already computed");
const Satellite& sat_i(sat_pair.first->second);
std::cout << "\tsat: " << sat_i.sat << std::endl
<< "\t\tpos: " << sat_i.pos.transpose() << std::endl
<< "\t\tvel: " << sat_i.vel.transpose() << std::endl
<< "\t\tvar: " << sat_i.var << std::endl
<< "\t\tclock bias: " << sat_i.clock_bias << std::endl
<< "\t\tclock drift: " << sat_i.clock_drift << std::endl;
// if (sat_i.pos_ == Eigen::Vector3d::Zero())
// std::cout << "ephemeris not available for sat " << sat_i.sat_ << std::endl;
//std::cout << "\tsat: " << sat_pair.first->second.sat << std::endl
// << "\t\tpos: " << sat_pair.first->second.pos.transpose() << std::endl
// << "\t\tvel: " << sat_pair.first->second.vel.transpose() << std::endl
// << "\t\tvar: " << sat_pair.first->second.var << std::endl
// << "\t\tclock bias: " << sat_pair.first->second.clock_bias << std::endl
// << "\t\tclock drift: " << sat_pair.first->second.clock_drift << std::endl
// << "\t\tsvh: " << sat_pair.first->second.svh << std::endl;
// if (sat_pair.first->second.pos_ == Eigen::Vector3d::Zero())
// std::cout << "ephemeris not available for sat " << sat_pair.first->second.sat_ << std::endl;
}
return sats;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment