added pseudoranges and satellite class

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;

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);
 }
 

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_

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_
+#endif  // INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
\ No newline at end of file

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)
         {
         };
     };

src/observations.cpp

 #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)
 {

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;
+    }
+}

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)

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;
 }
 

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;
   }