diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index c10e5e4de876e40bc0c163022ab15c9bad7714e4..9e29038544ed6edc5fdef4620223c86e47ab27e8 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -23,9 +23,11 @@ class Observations;
class Navigation;
class Snapshot;
class Satellite;
+class PseudoRange;
// Typedefs
typedef std::map<int,Satellite> Satellites;
+typedef std::map<int,PseudoRange> PseudoRanges;
// pointer typedefs
typedef std::shared_ptr<Observations> ObservationsPtr;
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index ab2a5d21bc9c90c20f89e3587897dd59e907e770..189c8798f6a6b52332d682e8fddc2ccffb538838 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -5,6 +5,7 @@
namespace GnssUtils
{
+
class Observations
{
public:
@@ -45,6 +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> filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
@@ -59,7 +63,22 @@ 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);
+
+ // Others
static std::set<int> findCommonObservations(const Observations& obs_1, const Observations& obs_2);
bool operator==(const Observations& other_obs) const;
@@ -104,13 +123,13 @@ inline const obsd_t& Observations::getObservationBySat(const unsigned char& sat_
inline obsd_t& Observations::getObservationByIdx(const int& idx)
{
- assert(obs_.size() > idx);
+ assert(obs_.size() > idx && idx >= 0);
return obs_.at(idx);
}
inline const obsd_t& Observations::getObservationByIdx(const int& idx) const
{
- assert(obs_.size() > idx);
+ assert(obs_.size() > idx && idx >= 0);
return obs_.at(idx);
}
diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h
index 065abc92201cb0d3e84cda4e82f27e85acdd47cc..7c65853fdaabf4fd31cd3aedb731793062619169 100644
--- a/include/gnss_utils/snapshot.h
+++ b/include/gnss_utils/snapshot.h
@@ -14,6 +14,12 @@
namespace GnssUtils
{
+struct PseudoRange
+{
+ double prange = -1;
+ double var = -1;
+};
+
class Snapshot
{
@@ -44,6 +50,15 @@ public:
const bool &check_carrier_phase,
const prcopt_t &opt);
+ // Pseudo-ranges
+ PseudoRanges& getPseudoRanges();
+ const PseudoRanges& getPseudoRanges() const;
+ 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();
private:
@@ -51,6 +66,7 @@ private:
Satellites sats_;
ObservationsPtr obs_;
NavigationPtr nav_;
+ PseudoRanges pranges_;
// Private methods
};
@@ -110,5 +126,20 @@ inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded
return obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt);
}
+inline const PseudoRanges& Snapshot::getPseudoRanges() const
+{
+ return pranges_;
+}
+
+inline PseudoRanges& Snapshot::getPseudoRanges()
+{
+ return pranges_;
+}
+
+inline bool Snapshot::pseudoRangesComputed() const
+{
+ return !pranges_.empty();
+}
+
} // namespace GnssUtils
#endif // INCLUDE_GNSS_UTILS_SNAPSHOT_H_
diff --git a/include/gnss_utils/utils/rcv_position.h b/include/gnss_utils/utils/rcv_position.h
index 9755c14f8f453758df77e432ad30d40b740d9441..13357a8930025db21c98e3525743e0ea25b418d0 100644
--- a/include/gnss_utils/utils/rcv_position.h
+++ b/include/gnss_utils/utils/rcv_position.h
@@ -36,6 +36,8 @@ struct ComputePosOutput
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);
@@ -62,4 +64,4 @@ int estposOwn(const obsd_t* obs,
double* resp,
char* msg);
} // namespace GnssUtils
-#endif // INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
\ No newline at end of file
+#endif // INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
diff --git a/include/gnss_utils/utils/satellite.h b/include/gnss_utils/utils/satellite.h
index 14ab5397af16d392634a08b8fd3ea76fe0a4e03a..64aa5c3fa09094f25863219ec62c5e8b3f73059d 100644
--- a/include/gnss_utils/utils/satellite.h
+++ b/include/gnss_utils/utils/satellite.h
@@ -29,25 +29,28 @@ namespace GnssUtils
struct Satellite
{
- int sat_;
- Eigen::Vector3d pos_;
- Eigen::Vector3d vel_;
- double var_;
- double clock_bias_;
- double clock_drift_;
-
- Satellite(const int& _sat,
+ int sys;
+ int sat;
+ Eigen::Vector3d pos;
+ Eigen::Vector3d vel;
+ 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) :
- sat_(_sat),
- pos_(_pos),
- vel_(_vel),
- var_(_var),
- clock_bias_(_clock_bias),
- clock_drift_(_clock_drift)
+ sys(_sys),
+ sat(_sat),
+ pos(_pos),
+ vel(_vel),
+ var(_var),
+ clock_bias(_clock_bias),
+ clock_drift(_clock_drift)
{
};
};
diff --git a/src/observations.cpp b/src/observations.cpp
index bbc0e86c87c8741947462382390a2ff5ff4540cd..733f20366dbaf5b1eeb5dc6e9f13442b233a9d9f 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -1,4 +1,5 @@
#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
#include "gnss_utils/utils/satellite.h"
using namespace GnssUtils;
@@ -173,10 +174,10 @@ std::set<int> Observations::filterByEphemeris(const Satellites& sats)
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)
+ 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: \n\t" << sats.at(sat_number).pos.transpose() << std::endl;
remove_sats.insert(sat_number);
}
}
@@ -303,6 +304,44 @@ std::set<int> Observations::filterByConstellations(const int& navsys)
return remove_sats;
}
+std::set<int> Observations::filterByElevationSnr(const std::map<int,double>& elevations,
+ const snrmask_t& snrmask,
+ const double& elmin)
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // check elevation
+ if (elevations.at(sat_number) < elmin)
+ {
+ //std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << elmin << std::endl;
+ remove_sats.insert(sat_number);
+ continue;
+ }
+
+ // snr TODO: multifrequency (2nd param and 3rd idx)
+ if (testsnr(0, 0, elevations.at(sat_number), obs_sat.SNR[0] * 0.25, &snrmask) == 1)
+ {
+ //std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r,
const Satellites& sats,
const snrmask_t& snrmask,
@@ -316,7 +355,7 @@ std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r,
const int& sat_number = obs_sat.sat;
// check elevation
- double elevation = computeSatElevation(x_r, sats.at(sat_number).pos_);
+ double elevation = computeSatElevation(x_r, sats.at(sat_number).pos);
if (elevation < elmin)
{
//std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << elmin << std::endl;
@@ -405,6 +444,66 @@ 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)
+{
+ return filter(sats,
+ discarded_sats,
+ elevations,
+ check_code,
+ check_carrier_phase,
+ opt.navsys,
+ opt.snrmask,
+ 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::cout << "filter: initial size: " << obs_.size() << std::endl;
+ // Ephemeris
+ std::set<int> remove_sats = filterByEphemeris(sats);
+
+ // Discarded sats
+ std::set<int> remove_sats_discarded = filterBySatellites(discarded_sats);
+ remove_sats.insert(remove_sats_discarded.begin(), remove_sats_discarded.end());
+
+ // Code
+ if (check_code)
+ {
+ std::set<int> remove_sats_code = filterByCode();
+ remove_sats.insert(remove_sats_code.begin(), remove_sats_code.end());
+ }
+
+ // Carrier phase
+ if (check_carrier_phase)
+ {
+ std::set<int> remove_sats_carrier = filterByCarrierPhase();
+ remove_sats.insert(remove_sats_carrier.begin(), remove_sats_carrier.end());
+ }
+
+ // 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);
+ remove_sats.insert(remove_sats_elevation.begin(), remove_sats_elevation.end());
+
+ return remove_sats;
+ // std::cout << "final size: " << obs_.size() << std::endl;
+}
+
std::set<int> Observations::findCommonObservations(const Observations& obs_1,
const Observations& obs_2)
{
diff --git a/src/snapshot.cpp b/src/snapshot.cpp
index 26a7251a76f7c9be4010cb32d1b59ff74e226e57..c98807f10ca6936f45eb030e6d85ba0e99d2d0cd 100644
--- a/src/snapshot.cpp
+++ b/src/snapshot.cpp
@@ -30,3 +30,48 @@ void Snapshot::computeSatellites(const int& eph_opt)
sats_ = GnssUtils::computeSatellites(*obs_, *nav_, 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;
+
+ 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)};
+
+ // 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)
+ continue;
+
+ /* ionospheric corrections */
+ if (!ionocorr(obs_i.time,&nav_->getNavigation(),sat,latlonalt.data(),azel_i, opt.ionoopt,&dion,&vion))
+ continue;
+
+ /* GPS-L1 -> L1/B1 */
+ if ((lam_L1=nav_->getNavigation().lam[sat-1][0])>0.0)
+ dion*=std::pow(lam_L1/lam_carr[0],2);
+
+ /* tropospheric corrections */
+ if (!tropcorr(obs_i.time,&nav_->getNavigation(),latlonalt.data(),azel_i,opt.tropopt,&dtrp,&vtrp))
+ continue;
+
+ /* pseudorange corrected */
+ pranges_.at(sat).prange = P-(rcv_clock_bias-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;
+ }
+}
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 9a230ef599e9c2c5bf5da5d84a5df2c3e4110c77..cd1b82c09aa9dc438f136d38e4e87ad63332477e 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -210,8 +210,8 @@ bool Tdcp(SnapshotPtr snapshot_r,
auto nav_k = snapshot_k->getNavigation()->getNavigation();
// Satellite's positions
- s_r.col(row) = snapshot_r->getSatellites().at(sat_number).pos_;
- s_k.col(row) = snapshot_r->getSatellites().at(sat_number).pos_;
+ s_r.col(row) = snapshot_r->getSatellites().at(sat_number).pos;
+ s_k.col(row) = snapshot_r->getSatellites().at(sat_number).pos;
nav_k.ion_gps;
if (sd_params.use_carrier_phase) // TODO: add iono and tropo corrections (optionally)
diff --git a/src/utils/rcv_position.cpp b/src/utils/rcv_position.cpp
index da0d1320b1ef8dd16c89b3ddc1016590f8053428..77fa3130f39d28ec65f200558703b89189b23e76 100644
--- a/src/utils/rcv_position.cpp
+++ b/src/utils/rcv_position.cpp
@@ -28,13 +28,13 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
sol_t sol;
sol = { { 0 } };
+ std::vector<double> sat_elevations(2*_observations.size());
+
output.pos_stat = pntpos(
- _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
+ _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, sat_elevations.data(), NULL, msg);
if (output.pos_stat == 0)
- {
std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
- }
output.time = sol.time.time;
output.sec = sol.time.sec;
@@ -44,13 +44,8 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
// std::cout << "Compute pos: " << output.pos.transpose() << "\n";
// std::cout << "Covariance:\n" << output.pos_covar << "\n";
- // XXX: segmentation fault here.
if (sol.dtr != NULL)
- {
output.rcv_bias = (Eigen::Matrix<double,6,1>() << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5]).finished();
- //output.rcv_bias = Eigen::Matrix<double, 6,1>(sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5]);
- //output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5];
- }
output.type = sol.type;
output.stat = sol.stat;
output.ns = sol.ns;
@@ -58,6 +53,9 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
output.ratio = sol.ratio;
output.lat_lon = ecefToLatLonAlt(output.pos);
+ for (auto i = 0; i < _observations.size(); i++)
+ output.sat_azel.emplace(_observations.getObservationByIdx(i).sat,Eigen::Vector2d(sat_elevations.at(2*i),sat_elevations.at(2*i+1)));
+
return output;
}
diff --git a/src/utils/satellite.cpp b/src/utils/satellite.cpp
index 51266efa1e254909fe392a287a985813006da22c..c3c4d27b0225cddb3449a0bad16709e033b772c3 100644
--- a/src/utils/satellite.cpp
+++ b/src/utils/satellite.cpp
@@ -56,7 +56,8 @@ Satellites computeSatellites(const Observations& obs,
for (int i = 0; i < obs.size(); i++)
{
auto sat_pair = sats.emplace(obs.getObservationByIdx(i).sat, // Key
- Satellite(obs.getObservationByIdx(i).sat, // Constructor...
+ 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],
@@ -67,12 +68,12 @@ Satellites computeSatellites(const Observations& obs,
const Satellite& sat_i(sat_pair.first->second);
- std::cout << "\tsat: " << sat_i.sat_ << ": "
- << "\t\tpos: " << sat_i.pos_.transpose()
- << "\t\tvel: " << sat_i.vel_.transpose()
- << "\t\tvar: " << sat_i.var_
- << "\t\tclock bias: " << sat_i.clock_bias_
- << "\t\tclock drift: " << sat_i.clock_drift_ << std::endl;
+ 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;
}