diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9ec32eefac1059a24af1a451a0352f80f51d4e83..37953bb9fe7012aa1d457d3389bdac417077b304 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -77,17 +77,18 @@ SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src)
# driver source files
SET(SOURCES
- src/utils/utils.cpp
- src/utils/transformations.cpp
- src/utils/rcv_position.cpp
- src/utils/satellite.cpp
- src/observations.cpp
src/navigation.cpp
- src/snapshot.cpp
- src/tdcp.cpp
+ src/observations.cpp
+ src/range.cpp
src/receiver_raw_base.cpp
src/receivers/ublox_raw.cpp
- src/receivers/novatel_raw.cpp)
+ src/receivers/novatel_raw.cpp
+ src/snapshot.cpp
+ src/tdcp.cpp
+ src/utils/utils.cpp
+ src/utils/transformations.cpp
+ src/utils/rcv_position.cpp
+ src/utils/satellite.cpp)
SET(RTKLIB_SRC
${RTKLIB_SRC_DIR}/pntpos.c
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index a7714735bd61f66c23240896b476390848b3c004..0102a6c72bdafe08efeb4e3ae61e9d112221948c 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -27,20 +27,6 @@ namespace GnssUtils
{
// Structs
-struct PseudoRange
-{
- int sat = 0;
- double p = -1;
- double prange_var = 1;
- double prange = -1;
- double iono_corr = 0;
- double tropo_corr = 0;
- double sat_clock_corr = 0;
- double L = -1;
- double carrier_range = -1;
- double carrier_range_var = 1;
-};
-
struct ComputePosOutput
{
time_t time;
@@ -114,6 +100,7 @@ struct TdcpOptions
bool enabled; // TDCP enabled
bool corr_iono; // apply correction also in TDCP
bool corr_tropo; // apply correction also in TDCP
+ bool corr_clock;
bool loss_function; // apply loss function in TDCP factors
double sigma_atm;
double sigma_carrier;
@@ -179,10 +166,12 @@ class Observations;
class Navigation;
class Snapshot;
struct Satellite;
+class Range;
// Typedefs
typedef std::map<int,Satellite> Satellites;
-typedef std::map<int,PseudoRange> PseudoRanges;
+typedef std::map<int,Range> Ranges;
+typedef std::map<int,Eigen::Vector2d> Azels;
// pointer typedefs
typedef std::shared_ptr<Observations> ObservationsPtr;
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index 91deec41d86cc40b6e2ef0b6aa5d77083deb873d..468c418d2e618647e423a76d42c428c361a4908a 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -47,7 +47,7 @@ public:
const snrmask_t& snrmask,
const double& elmin,
const bool& multi_freq);
- std::set<int> filterByElevationSnr(const std::map<int,Eigen::Vector2d>& azels,
+ std::set<int> filterByElevationSnr(const Azels& azels,
const snrmask_t& snrmask,
const double& elmin,
const bool& multi_freq);
@@ -66,21 +66,21 @@ public:
const snrmask_t& snrmask,
const double& elmin,
const bool& multi_freq);
- 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 Options& 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,
- const bool& multi_freq);
+ std::set<int> filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt);
+ std::set<int> filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const int& navsys,
+ const snrmask_t& snrmask,
+ const double& elmin,
+ const bool& multi_freq);
// Others
static std::set<int> findCommonObservations(const Observations& obs_1, const Observations& obs_2);
diff --git a/include/gnss_utils/range.h b/include/gnss_utils/range.h
new file mode 100644
index 0000000000000000000000000000000000000000..caf9e9db23c97692ba3e4bb1a438e8162b5c24ea
--- /dev/null
+++ b/include/gnss_utils/range.h
@@ -0,0 +1,46 @@
+/*
+ * range.h
+ *
+ * Created on: May 28, 2020
+ * Author: joanvallve
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_RANGE_H_
+#define INCLUDE_GNSS_UTILS_RANGE_H_
+
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+
+namespace GnssUtils
+{
+
+class Range
+{
+ public:
+ int sat = 0;
+ double P = -1;
+ double P_var = 1;
+ double P_corrected = -1;
+ double iono_corr = 0;
+ double tropo_corr = 0;
+ double sat_clock_corr = 0;
+ double L = -1;
+ double L_corrected = -1;
+ double L_var = 1;
+
+ public:
+ Range();
+ virtual ~Range();
+
+ static Ranges computeRanges(ObservationsPtr obs,
+ NavigationPtr nav,
+ const Satellites& sats,
+ const Azels& azel,
+ const Eigen::Vector3d& latlonalt,
+ const Options& opt);
+};
+
+} /* namespace GnssUtils */
+
+#endif /* INCLUDE_GNSS_UTILS_RANGE_H_ */
diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h
index 04e2d6da55156844c9636cbd0bdfdf05a0f95996..732d1913292feb8c3a7c48ee6091e172beaac10f 100644
--- a/include/gnss_utils/snapshot.h
+++ b/include/gnss_utils/snapshot.h
@@ -38,24 +38,24 @@ public:
void computeSatellites(const int& eph_opt); // see rtklib.h L396);
bool satellitesComputed() const;
- std::set<int> filterObservations(const std::set<int> &discarded_sats,
- const Eigen::Vector3d &x_r,
- const bool &check_code,
- const bool &check_carrier_phase,
- const Options &opt);
- std::set<int> filterObservations(const std::set<int> &discarded_sats,
- const std::map<int,Eigen::Vector2d>& azels,
- const bool &check_code,
- const bool &check_carrier_phase,
- const Options &opt);
+ std::set<int> filterObservations(const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt);
+ std::set<int> filterObservations(const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& 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 Options& opt);
+ Ranges& getRanges();
+ const Ranges& getRanges() const;
+ bool rangesComputed() const;
+ void computeRanges(const Azels& azel,
+ const Eigen::Vector3d& latlonalt,
+ const Options& opt);
void print();
@@ -64,7 +64,7 @@ private:
Satellites sats_;
ObservationsPtr obs_;
NavigationPtr nav_;
- PseudoRanges pranges_;
+ Ranges ranges_;
// Private methods
};
@@ -76,6 +76,7 @@ private:
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
#include "gnss_utils/utils/utils.h"
+#include "gnss_utils/range.h"
namespace GnssUtils
{
@@ -115,37 +116,37 @@ inline bool Snapshot::satellitesComputed() const
return !sats_.empty();
}
-inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded_sats,
- const Eigen::Vector3d &x_r,
- const bool &check_code,
- const bool &check_carrier_phase,
- const Options &opt)
+inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt)
{
return obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt);
}
-inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded_sats,
- const std::map<int,Eigen::Vector2d>& azels,
- const bool &check_code,
- const bool &check_carrier_phase,
- const Options &opt)
+inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt)
{
return obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt);
}
-inline const PseudoRanges& Snapshot::getPseudoRanges() const
+inline const Ranges& Snapshot::getRanges() const
{
- return pranges_;
+ return ranges_;
}
-inline PseudoRanges& Snapshot::getPseudoRanges()
+inline Ranges& Snapshot::getRanges()
{
- return pranges_;
+ return ranges_;
}
-inline bool Snapshot::pseudoRangesComputed() const
+inline bool Snapshot::rangesComputed() const
{
- return !pranges_.empty();
+ return !ranges_.empty();
}
} // namespace GnssUtils
diff --git a/src/observations.cpp b/src/observations.cpp
index a6b856a351de30b8aa0dedd2cecdf0cef6c32f17..37ae10e6bbdc2672346c088560cda165fbde1940 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -373,38 +373,6 @@ std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r,
}
return filterByElevationSnr(azels, snrmask, elmin, multi_freq);
-
-// 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
-// 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;
-// remove_sats.insert(sat_number);
-// continue;
-// }
-//
-// // snr TODO: multifrequency (2nd param and 3rd idx)
-// 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);
-// }
-// }
-//
-// // 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::filter(const Satellites& sats,
@@ -479,12 +447,12 @@ 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,Eigen::Vector2d>& azels,
- const bool& check_code,
- const bool& check_carrier_phase,
- const Options& opt)
+std::set<int> Observations::filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt)
{
return filter(sats,
discarded_sats,
@@ -497,15 +465,15 @@ std::set<int> Observations::filter(const Satellites& sats,
opt.ionoopt == IONOOPT_IFLC or (opt.tdcp.enabled and opt.tdcp.multi_freq));
}
-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,
- const bool& multi_freq)
+std::set<int> Observations::filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const int& navsys,
+ const snrmask_t& snrmask,
+ const double& elmin,
+ const bool& multi_freq)
{
//std::cout << "filter: initial size: " << obs_.size() << std::endl;
// Ephemeris
diff --git a/src/range.cpp b/src/range.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..09730a8a22eda9dc673cfcc12d5505ec397d6a89
--- /dev/null
+++ b/src/range.cpp
@@ -0,0 +1,155 @@
+/*
+ * pseudo_range.cpp
+ *
+ * Created on: May 28, 2020
+ * Author: joanvallve
+ */
+
+#include "gnss_utils/range.h"
+
+namespace GnssUtils
+{
+
+Range::Range()
+{
+ // TODO Auto-generated constructor stub
+
+}
+
+Range::~Range()
+{
+ // TODO Auto-generated destructor stub
+}
+
+Ranges Range::computeRanges(ObservationsPtr obs,
+ NavigationPtr nav,
+ const Satellites& sats,
+ const Azels& azel,
+ const Eigen::Vector3d& latlonalt,
+ const Options& opt)
+{
+ Ranges ranges;
+
+ double dion,dtrp,vmeas,vion,vtrp,P,lam_L1;
+ prcopt_t prcopt = opt.getPrcopt();
+
+ //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
+ ranges.emplace(sat,Range());
+
+ /* ------------------- Pseudo range ------------------- */
+ /* psudorange with code bias correction */
+ //std::cout << "prange...\n";
+ if ((P=prange(&obs_i,&nav->getNavigation(),azel_i,1,&prcopt,&vmeas))==0.0)
+ {
+ //std::cout << " error in prange\n";
+ continue;
+ }
+
+ /* ionospheric corrections */
+ //std::cout << "iono...\n";
+ //std::cout << "\ttime: " << obs_i.time.time << " + " << obs_i.time.sec << "\n";
+ //std::cout << "\tnav: \n";
+ //nav->print();
+ //std::cout << "\tsat: " << sat << "\n";
+ //std::cout << "\tlatlonalt: " << latlonalt.transpose() << "\n";
+ //std::cout << "\tazel_i: " << azel_i[0] << " " << azel_i[1] << "\n";
+ //std::cout << "\topt.ionoopt: " << opt.ionoopt << "\n";
+ 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 */
+ //std::cout << "iono2...\n";
+ if ((lam_L1=nav->getNavigation().lam[sat-1][0])>0.0)
+ dion*=std::pow(lam_L1/lam_carr[0],2);
+
+ /* tropospheric corrections */
+ //std::cout << "tropo...\n";
+ if (!tropcorr(obs_i.time,&nav->getNavigation(),latlonalt.data(),azel_i,opt.tropopt,&dtrp,&vtrp))
+ {
+ //std::cout << " error in tropcorr\n";
+ continue;
+ }
+
+ /* Store in PseudoRange struct */
+ //std::cout << "storing\n";
+ ranges[sat].sat = sat;
+ ranges[sat].P = P;
+ ranges[sat].iono_corr = -dion;
+ ranges[sat].tropo_corr = -dtrp;
+ ranges[sat].sat_clock_corr = CLIGHT*sat_i.clock_bias;
+
+ /* pseudorange corrected */
+ ranges[sat].P_corrected = ranges[sat].P +
+ ranges[sat].iono_corr +
+ ranges[sat].tropo_corr +
+ ranges[sat].sat_clock_corr;
+
+ /* error variance */
+ ranges[sat].P_var = varerr(&prcopt,azel_i[1],prcopt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp;
+
+ /* ------------------- Carrier phase ------------------- */
+ ranges[sat].L = obs_i.L[0]*nav->getNavigation().lam[sat-1][0];
+ ranges[sat].L_corrected = ranges[sat].L;
+
+ /* ionospheric corrections */
+ if (opt.tdcp.corr_iono)
+ {
+ if (opt.ionoopt==IONOOPT_IFLC)
+ {
+ // TODO formulation
+ //ranges[sat].L_corrected = obs_i.L[0]*nav->getNavigation().lam[sat-1][0];
+ }
+ else
+ ranges[sat].L_corrected -= ranges[sat].iono_corr;
+ }
+
+ /* tropospheric corrections */
+ if (opt.tdcp.corr_tropo)
+ ranges[sat].L_corrected += ranges[sat].tropo_corr;
+
+ /* sat clock corrections */
+ if (opt.tdcp.corr_clock)
+ ranges[sat].L_corrected += ranges[sat].sat_clock_corr;
+
+ /* carrier phase variance */
+ ranges[sat].L_var = opt.tdcp.sigma_carrier * opt.tdcp.sigma_carrier;
+
+ //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;
+ }
+
+ return ranges;
+}
+
+} /* namespace GnssUtils */
diff --git a/src/snapshot.cpp b/src/snapshot.cpp
index 5ff3285b5255469cdebeae80102f4ac4bf9f6569..828372362d389cd8fd6d1e123204f6ae33456c99 100644
--- a/src/snapshot.cpp
+++ b/src/snapshot.cpp
@@ -33,103 +33,11 @@ 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 Options& opt)
+void Snapshot::computeRanges(const Azels& azel,
+ const Eigen::Vector3d& latlonalt,
+ const Options& opt)
{
- assert(pranges_.empty() && "pseudo ranges already computed!");
+ assert(!rangesComputed() && "pseudo ranges already computed!");
- double dion,dtrp,vmeas,vion,vtrp,P,lam_L1;
- prcopt_t prcopt = opt.getPrcopt();
-
- //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 */
- //std::cout << "prange...\n";
- if ((P=prange(&obs_i,&nav_->getNavigation(),azel_i,1,&prcopt,&vmeas))==0.0)
- {
- //std::cout << " error in prange\n";
- continue;
- }
-
- /* ionospheric corrections */
- //std::cout << "iono...\n";
- //std::cout << "\ttime: " << obs_i.time.time << " + " << obs_i.time.sec << "\n";
- //std::cout << "\tnav: \n";
- //nav_->print();
- //std::cout << "\tsat: " << sat << "\n";
- //std::cout << "\tlatlonalt: " << latlonalt.transpose() << "\n";
- //std::cout << "\tazel_i: " << azel_i[0] << " " << azel_i[1] << "\n";
- //std::cout << "\topt.ionoopt: " << opt.ionoopt << "\n";
- 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 */
- //std::cout << "iono2...\n";
- if ((lam_L1=nav_->getNavigation().lam[sat-1][0])>0.0)
- dion*=std::pow(lam_L1/lam_carr[0],2);
-
- /* tropospheric corrections */
- //std::cout << "tropo...\n";
- if (!tropcorr(obs_i.time,&nav_->getNavigation(),latlonalt.data(),azel_i,opt.tropopt,&dtrp,&vtrp))
- {
- //std::cout << " error in tropcorr\n";
- continue;
- }
-
- /* Store in PseudoRange struct */
- //std::cout << "storing\n";
- pranges_[sat].sat = sat;
- pranges_[sat].p = P;
- pranges_[sat].iono_corr = -dion;
- pranges_[sat].tropo_corr = -dtrp;
- pranges_[sat].sat_clock_corr = CLIGHT*sat_i.clock_bias;
-
- /* pseudorange corrected */
- pranges_[sat].prange = pranges_[sat].p +
- pranges_[sat].iono_corr +
- pranges_[sat].tropo_corr +
- pranges_[sat].sat_clock_corr;
-
- /* error variance */
- pranges_[sat].prange_var = varerr(&prcopt,azel_i[1],prcopt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp;
-
- // todo: fill L and carrier_range
-
- //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;
- }
+ ranges_ = Range::computeRanges(obs_, nav_, sats_, azel, latlonalt, opt);
}