diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 06a0d310ee6006809bea5a03aeed5feddd8eb85e..d6429ef7f3ea39318ce1248285ea0d6373677248 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -10,8 +10,8 @@
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Sparse>
-#include "observations.h"
-#include "navigation.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
extern "C"
{
@@ -53,9 +53,16 @@ namespace GNSSUtils
int estposOwn(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
- double *resp, char *msg)
+ double *resp, char *msg);
Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef);
+
+ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
+
+ void computeSatellitesPositions(const Observations& obs,
+ const Navigation& nav,
+ const prcopt_t& opt,
+ std::map<unsigned char,Eigen::Vector3d>& sats_pos);
}
#endif
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index e84820c2ab4dc86ef80ee79d06acc59da5047812..3e7f56e1b116f1d5680680d78f728cdd9e00c8dc 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -2,6 +2,7 @@
#define OBSERVATIONS_H
#include <vector>
+#include <map>
#include <iostream>
#include <memory>
@@ -14,43 +15,123 @@ extern "C"
namespace GNSSUtils
{
- class Observations
- {
+class Observations
+{
public:
- // Constructor & Destructor
- Observations();
- ~Observations();
+ // Constructor & Destructor
+ Observations();
+ ~Observations();
+
+ // Public objects
+
+ // Public methods
+
+
+ /* - Observations - */
- // Public objects
+ void clearObservations();
- // Public methods
+ void addObservation(const obsd_t & obs);
+ void removeObservationByIdx(const int& _idx);
+ void removeObservationBySat(const int& _sat);
- /* - Observations - */
+ std::vector<obsd_t>& getObservations();
+ const std::vector<obsd_t> & getObservations() const;
- void clearObservations();
+ obsd_t& getObservationBySat(const unsigned char& sat_number);
+ const obsd_t& getObservationBySat(const unsigned char& sat_number) const;
- void pushObservation(obsd_t & obs);
+ obsd_t& getObservationByIdx(const int& idx);
+ const obsd_t& getObservationByIdx(const int& idx) const;
- void reserveObservations(unsigned int n);
+ obsd_t* data();
+ const obsd_t* data() const;
- const std::vector<obsd_t> & getObservations() const;
+ size_t size() const;
- void print(obsd_t & _obs);
- void print(int _obs_idx);
- void print();
+ bool hasSatellite(const unsigned char& i) const;
+ void print(obsd_t & _obs);
+ void printBySat(const int& _sat);
+ void printByIdx(const int& _idx);
+ void print();
+ static void findCommonObservations(const Observations& obs_1, const Observations& obs_2,
+ Observations& common_obs_1, Observations& common_obs_2);
private:
- // Private objects
- // rtklib-like attribute to represent the different observation msgs for a given epoch
- std::vector<obsd_t> obs_vector_;
+ // Private objects
+ std::map<unsigned char,int> sat_2_idx_; //< key: corresponding sat number, value: idx in obs_ vector
+ std::map<int,unsigned char> idx_2_sat_; //< key: idx in obs_ vector, value: corresponding sat number
+ std::vector<obsd_t> obs_; //< vector of RTKLIB observations for a given epoch
+
+ // Private methods
+};
+
+inline void Observations::removeObservationByIdx(const int& _idx)
+{
+ obs_.erase(obs_.begin() + _idx);
+ sat_2_idx_.erase(idx_2_sat_.at(_idx));
+ idx_2_sat_.erase(_idx);
+}
+
+inline void Observations::removeObservationBySat(const int& _sat)
+{
+ obs_.erase(obs_.begin() + sat_2_idx_.at(_sat));
+ idx_2_sat_.erase(sat_2_idx_.at(_sat));
+ sat_2_idx_.erase(_sat);
+}
+
+inline const std::vector<obsd_t>& Observations::getObservations() const
+{
+ return this->obs_;
+}
+inline std::vector<obsd_t>& Observations::getObservations()
+{
+ return this->obs_;
+}
- // Private methods
+inline obsd_t& Observations::getObservationBySat(const unsigned char& sat_number)
+{
+ return obs_.at(sat_2_idx_.at(sat_number));
+}
+inline const obsd_t& Observations::getObservationBySat(const unsigned char& sat_number) const
+{
+ return obs_.at(sat_2_idx_.at(sat_number));
+}
+
+inline obsd_t& Observations::getObservationByIdx(const int& idx)
+{
+ return obs_.at(idx);
+}
+
+inline const obsd_t& Observations::getObservationByIdx(const int& idx) const
+{
+ return obs_.at(idx);
+}
+
+inline obsd_t* Observations::data()
+{
+ return obs_.data();
+}
+
+inline const obsd_t* Observations::data() const
+{
+ return obs_.data();
+}
+
+inline size_t Observations::size() const
+{
+ return obs_.size();
+}
+
+inline bool Observations::hasSatellite(const unsigned char& i) const
+{
+ return sat_2_idx_.count(i) != 0;
+}
- };
}
#endif
diff --git a/include/gnss_utils/single_differences.h b/include/gnss_utils/single_differences.h
index 9c48af4a3390ad33e930b7274d384b5a7391a6af..3c1ee0851477856c01879781644d47da08cbef4b 100644
--- a/include/gnss_utils/single_differences.h
+++ b/include/gnss_utils/single_differences.h
@@ -8,7 +8,12 @@
#ifndef INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_
#define INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_
-#include "gnss_utils.h"
+#define GNSS_UTILS_SD_DEBUG 1
+
+#include <set>
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
extern "C"
{
#include "rtklib.h"
@@ -18,7 +23,7 @@ namespace GNSSUtils
{
struct SingleDifferencesParams
{
- int common_sat_min;
+ int min_common_sats;
int raim_n;
double raim_min_residual;
bool use_carrier_phase;
@@ -28,26 +33,32 @@ namespace GNSSUtils
double sigma_atm;
double sigma_code;
double sigma_carrier;
+ bool use_old_nav;
+ bool use_multi_freq;
};
- bool singleDifferences(const Observations& obs_r, const Navigation& nav_r,
- const Observations& obs_k, const Navigation& nav_r,
+ bool singleDifferences(const Observations& obs_r, Navigation& nav_r,
+ const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::list<int>& discarded_sats,
+ double& residual, std::set<unsigned char>& discarded_sats,
const SingleDifferencesParams& sd_opt, const prcopt_t& opt);
- bool singleDifferences(const Observations& obs_r, const Eigen::Vector3d& pos_r,
- const Observations& obs_k, const Navigation& nav_r,
+ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r,
+ const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::list<int>& discarded_sats,
+ double& residual, std::set<unsigned char>& discarded_sats,
const SingleDifferencesParams& sd_opt, const prcopt_t& opt);
- bool singleDifferences(const Observations& common_obs_r, const Eigen::Vector3d& pos_r,
- const Observations& common_obs_k, const std::map<int,Eigen::Vector3d>& common_sats_pos,
+ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r, const std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_r, const Eigen::Vector3d& x_r,
+ const Observations& common_obs_k, const Navigation& nav_k, const std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::list<int>& discarded_sats,
+ double& residual, std::set<unsigned char>& discarded_sats,
const SingleDifferencesParams& sd_opt, const prcopt_t& opt);
+ void filterCommonSatellites(Observations& common_obs_r, std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_r,
+ Observations& common_obs_k, std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_k,
+ std::set<unsigned char>& discarded_sats, const Eigen::Vector3d& x_r,
+ const SingleDifferencesParams& sd_params, const prcopt_t& opt);
}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index bf6f31f6e439b57c78c3d7a9ed52da3c7462a125..fc63b6c916b1826537c58b40beef0752e8283b2c 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -7,6 +7,7 @@ SET(SOURCES
gnss_utils.cpp
observations.cpp
navigation.cpp
+ single_differences.cpp
utils.cpp)
SET(RTKLIB_SRC
@@ -30,6 +31,7 @@ SET(HEADERS
../include/gnss_utils/utils.h
../include/gnss_utils/observations.h
../include/gnss_utils/navigation.h
+ ../include/gnss_utils/single_differences.h
${RTKLIB_SRC_DIR}/rtklib.h)
# Eigen #######
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index fd5589b48b65ae24c960a19b75900cd8c419cc00..829192b42efaa91ce47317b7d5faabecf9eb6a43 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -68,10 +68,10 @@ int createObsAndNav(Observations* observations, char* obs_path, Navigation* nav
for (int i=0; i < obs.n; i++)
{
- std::cout << "time: " << time_str(obs.data[i].time, 3) << " | sat: " << int(obs.data[i].sat) << " | rcv: " << obs.data[i].rcv <<
- " | SNR: " << obs.data[i].SNR[0] << " | LLI: " << obs.data[i].LLI[0] << " | code: " << obs.data[i].code[0] <<
+ std::cout << "time: " << time_str(obs.data[i].time, 3) << " | sat: " << int(obs.data[i].sat) << " | rcv: " << int(obs.data[i].rcv) <<
+ " | SNR: " << int(obs.data[i].SNR[0]) << " | LLI: " << int(obs.data[i].LLI[0]) << " | code: " << int(obs.data[i].code[0]) <<
" | L: " << obs.data[i].L[0] << " | P: " << obs.data[i].P[0] << " | D: " << obs.data[i].D[0] << std::endl;
- observations->pushObservation(obs.data[i]);
+ observations->addObservation(obs.data[i]);
}
free(obs.data);
@@ -191,8 +191,8 @@ int main(int argc, char *argv[])
for (int i=0; i < obs.size(); i++)
{
- std::cout << "time: " << time_str(obs[i].time, 3) << " | sat: " << int(obs[i].sat) << " | rcv: " << obs[i].rcv <<
- " | SNR: " << obs[i].SNR[0] << " | LLI: " << obs[i].LLI[0] << " | code: " << obs[i].code[0] <<
+ std::cout << "time: " << time_str(obs[i].time, 3) << " | sat: " << int(obs[i].sat) << " | rcv: " << int(obs[i].rcv) <<
+ " | SNR: " << int(obs[i].SNR[0]) << " | LLI: " << int(obs[i].LLI[0]) << " | code: " << int(obs[i].code[0]) <<
" | L: " << obs[i].L[0] << " | P: " << obs[i].P[0] << " | D: " << obs[i].D[0] << std::endl;
}
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 1398feceb9ee850c6801c58ace41eef0830c697e..c7b69bc3fa6cb1ac8a061477ad8ab614efa0926b 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -19,7 +19,7 @@ namespace GNSSUtils
sol_t sol;
sol = {{0}};
- output.pos_stat = pntpos(_observations.getObservations().data(), _observations.getObservations().size(),
+ output.pos_stat = pntpos(_observations.data(), _observations.size(),
&(_navigation.getNavigation()),
&_prcopt, &sol, NULL, NULL, msg);
@@ -68,7 +68,7 @@ namespace GNSSUtils
sol_t sol;
sol = {{0}};
- output.pos_stat = pntposOwn(_observations.getObservations().data(), _observations.getObservations().size(),
+ output.pos_stat = pntposOwn(_observations.data(), _observations.size(),
&(_navigation.getNavigation()),
&_prcopt, &sol, NULL, NULL, msg);
@@ -184,75 +184,120 @@ namespace GNSSUtils
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
- double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
- int i,j,k,info,stat,nv,ns;
+// double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
+// int i,j,k,info,stat,nv,ns;
+//
+// trace(3,"estpos : n=%d\n",n);
+//
+// v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
+//
+// for (i=0;i<3;i++) x[i]=sol->rr[i];
+//
+// for (i=0;i<MAXITR;i++) {
+//
+// /* pseudorange residuals */
+// nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
+// &ns);
+//
+// if (nv<NX) {
+// sprintf(msg,"lack of valid sats ns=%d",nv);
+// break;
+// }
+// /* weight by variance */
+// for (j=0;j<nv;j++) {
+// sig=sqrt(var[j]);
+// v[j]/=sig;
+// for (k=0;k<NX;k++) H[k+j*NX]/=sig;
+// }
+// /* least square estimation */
+// if ((info=lsq(H,v,NX,nv,dx,Q))) {
+// sprintf(msg,"lsq error info=%d",info);
+// break;
+// }
+// for (j=0;j<NX;j++) x[j]+=dx[j];
+//
+// if (norm(dx,NX)<1E-4) {
+// sol->type=0;
+// sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
+// sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
+// sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
+// sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
+// sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
+// for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
+// for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
+// sol->qr[3]=(float)Q[1]; /* cov xy */
+// sol->qr[4]=(float)Q[2+NX]; /* cov yz */
+// sol->qr[5]=(float)Q[2]; /* cov zx */
+// sol->ns=(unsigned char)ns;
+// sol->age=sol->ratio=0.0;
+//
+// /* validate solution */
+// if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
+// sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
+// }
+// free(v); free(H); free(var);
+//
+// return stat;
+// }
+// }
+// if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
+//
+// free(v); free(H); free(var);
- trace(3,"estpos : n=%d\n",n);
-
- v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
-
- for (i=0;i<3;i++) x[i]=sol->rr[i];
+ return 0;
+ }
- for (i=0;i<MAXITR;i++) {
+Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
+{
+ Eigen::Vector3d pos;
+ ecef2pos(_ecef.data(), pos.data());
- /* pseudorange residuals */
- nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
- &ns);
+ return pos;
+}
- if (nv<NX) {
- sprintf(msg,"lack of valid sats ns=%d",nv);
- break;
- }
- /* weight by variance */
- for (j=0;j<nv;j++) {
- sig=sqrt(var[j]);
- v[j]/=sig;
- for (k=0;k<NX;k++) H[k+j*NX]/=sig;
- }
- /* least square estimation */
- if ((info=lsq(H,v,NX,nv,dx,Q))) {
- sprintf(msg,"lsq error info=%d",info);
- break;
- }
- for (j=0;j<NX;j++) x[j]+=dx[j];
-
- if (norm(dx,NX)<1E-4) {
- sol->type=0;
- sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
- sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
- sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
- sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
- sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
- for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
- for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
- sol->qr[3]=(float)Q[1]; /* cov xy */
- sol->qr[4]=(float)Q[2+NX]; /* cov yz */
- sol->qr[5]=(float)Q[2]; /* cov zx */
- sol->ns=(unsigned char)ns;
- sol->age=sol->ratio=0.0;
-
- /* validate solution */
- if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
- sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
- }
- free(v); free(H); free(var);
-
- return stat;
- }
- }
- if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
+double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
+{
+ // ecef 2 geodetic
+ Eigen::Vector3d receiver_geo;
+ ecef2pos(receiver_ecef.data(), receiver_geo.data());
- free(v); free(H); free(var);
+ // receiver-sat vector ecef
+ Eigen::Vector3d receiver_sat_ecef = sat_ecef-receiver_ecef;
- return 0;
- }
+ // receiver-sat vector enu (receiver ecef as origin)
+ Eigen::Vector3d receiver_sat_enu;
+ ecef2enu(receiver_geo.data(), //geodetic position {lat,lon} (rad)
+ receiver_ecef.data(), //vector in ecef coordinate {x,y,z}
+ receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
- Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
- {
- double pos[3];
- ecef2pos(&_ecef(0), pos);
+ // elevation
+ return atan2(receiver_sat_enu(2), sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
+}
- return Eigen::Vector3d(pos);
- }
+void computeSatellitesPositions(const Observations& obs,
+ const Navigation& nav,
+ const prcopt_t& opt,
+ std::map<unsigned char,Eigen::Vector3d>& sats_pos)
+{
+ double rs[6*obs.size()],dts[2*obs.size()],var[obs.size()];
+ int svh[obs.size()];
+
+ // compute positions
+ satposs(obs.getObservations().front().time,
+ obs.data(),
+ obs.size(),
+ &nav.getNavigation(),
+ opt.sateph,
+ rs, dts, var, svh);
+
+ // store positions
+ for (int i = 0; i < obs.size(); i++)
+ {
+ if (svh[i] < 0) // ephemeris unavailable
+ sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero();
+ else
+ sats_pos[obs.getObservationByIdx(i).sat] << rs[6*i], rs[6*i+1], rs[6*i+2];
+ }
+}
}
diff --git a/src/observations.cpp b/src/observations.cpp
index 4ef5448237b6e98ba5fc17f92e9f3da2cc3a6a6a..3ac4ee43fcba98225ce52878877372cbae484243 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -12,27 +12,21 @@ Observations::Observations()
Observations::~Observations()
{
- this->obs_vector_.erase(obs_vector_.begin(), obs_vector_.end());
}
void Observations::clearObservations()
{
- this->obs_vector_.clear();
+ this->obs_.clear();
+ this->idx_2_sat_.clear();
+ this->sat_2_idx_.clear();
}
-void Observations::pushObservation(obsd_t & obs)
+void Observations::addObservation(const obsd_t & obs)
{
- this->obs_vector_.push_back(obs);
-}
-
-void Observations::reserveObservations(unsigned int n)
-{
- this->obs_vector_.reserve(n);
-}
-
-const std::vector<obsd_t> & Observations::getObservations() const
-{
- return this->obs_vector_;
+ assert(!hasSatellite(obs.sat) && "adding an observation of a satellite already stored!");
+ this->obs_.push_back(obs);
+ idx_2_sat_[obs_.size()-1] = obs.sat;
+ sat_2_idx_[obs.sat] = obs_.size()-1;
}
void Observations::print(obsd_t & _obs)
@@ -48,15 +42,36 @@ void Observations::print(obsd_t & _obs)
printArray("D: ", _obs.D, ARRAY_SIZE(_obs.D));
}
-void Observations::print(int _obs_idx)
+void Observations::printBySat(const int& _sat_number)
+{
+ print(getObservationBySat(_sat_number));
+}
+
+void Observations::printByIdx(const int& _idx)
{
- print(obs_vector_[_obs_idx]);
+ print(getObservationByIdx(_idx));
}
void Observations::print()
{
- for (auto obs: obs_vector_)
+ for (auto obs : obs_)
{
print(obs);
}
-}
\ No newline at end of file
+}
+
+void Observations::findCommonObservations(const Observations& obs_1, const Observations& obs_2,
+ Observations& common_obs_1, Observations& common_obs_2)
+{
+ // clear and reserve
+ common_obs_1.clearObservations();
+ common_obs_2.clearObservations();
+
+ // iterate 1
+ for (auto&& obs_1_ref : obs_1.getObservations())
+ if (obs_2.hasSatellite(obs_1_ref.sat))
+ {
+ common_obs_1.addObservation(obs_1_ref);
+ common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat));
+ }
+}
diff --git a/src/single_differences.cpp b/src/single_differences.cpp
index 0ce3affcc8adff3275321b89afee5a2f5eb0e846..dfb97f36b3b571dab24f16c15147817abc806448 100644
--- a/src/single_differences.cpp
+++ b/src/single_differences.cpp
@@ -5,57 +5,481 @@
* Author: jvallve
*/
-#include "single_differences.h"
+#include "gnss_utils/single_differences.h"
namespace GNSSUtils
{
-bool singleDifferences(const Observations& obs_r, const Navigation& nav_r,
+bool singleDifferences(const Observations& obs_r, Navigation& nav_r,
const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::list<int>& discarded_sats,
+ double& residual, std::set<unsigned char>& discarded_sats,
const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{
- // TODO: COMPUTE RECEIVER POSITION AT t_r
- Eigen::Vector3d pos_r(Eigen::Vector3d::Zero());
-
// COMPUTE SINGLE DIFFERENCES
- return singleDifferences(obs_r, pos_r,
+ return singleDifferences(obs_r, nav_r, computePos(obs_r,nav_r,opt).pos,
obs_k, nav_k,
d, cov_d,
residual, discarded_sats,
sd_params, opt);
}
-bool singleDifferences(const Observations& obs_r, const Eigen::Vector3d& pos_r,
+bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r,
const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::list<int>& discarded_sats,
+ double& residual, std::set<unsigned char>& discarded_sats,
const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{
- // TODO: FIND COMMON SATELLITES
+ // FIND COMMON SATELLITES OBSERVATIONS
Observations common_obs_r, common_obs_k;
+ Observations::findCommonObservations(obs_r, obs_k, common_obs_r, common_obs_k);
+ int n_common_sats = common_obs_r.getObservations().size();
+
+ // COMPUTE COMMON SATELLITES POSITION
+ std::map<unsigned char, Eigen::Vector3d> common_sats_pos_r, common_sats_pos_k;
+ computeSatellitesPositions(common_obs_r, nav_r, opt, common_sats_pos_r);
+ computeSatellitesPositions(common_obs_k, sd_params.use_old_nav ? nav_r : nav_k, opt, common_sats_pos_k);
- // TODO: COMPUTE COMMON SATELLITES POSITION
- std::map<int,Eigen::Vector3d> common_sats_pos;
+ // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
+ filterCommonSatellites(common_obs_r, common_sats_pos_r,
+ common_obs_k, common_sats_pos_k,
+ discarded_sats, x_r,
+ sd_params, opt);
// COMPUTE SINGLE DIFFERENCES
- return singleDifferences(common_obs_r, pos_r,
- common_obs_k, common_sats_pos,
+ return singleDifferences(common_obs_r, nav_r, common_sats_pos_r, x_r,
+ common_obs_k, nav_k, common_sats_pos_k,
d, cov_d,
residual, discarded_sats,
sd_params, opt);
}
-bool singleDifferences(const Observations& common_obs_r, const Eigen::Vector3d& pos_r,
- const Observations& common_obs_k, const std::map<int,Eigen::Vector3d>& common_sats_pos,
+bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r, const std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_r, const Eigen::Vector3d& x_r,
+ const Observations& common_obs_k, const Navigation& nav_k, const std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::list<int>& discarded_sats,
+ double& residual, std::set<unsigned char>& excluded_sats,
const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{
+ assert(common_obs_r.size() == common_obs_k.size());
+ assert(common_obs_r.size() == common_sats_pos_r.size());
+ assert(common_obs_k.size() == common_sats_pos_k.size());
+
+ if (!excluded_sats.empty())
+ {
+ printf("SD: WARNING! excluded_sats list is not empty. It is an output parameter of the RAIM excluded sats. Clearing the list.");
+ excluded_sats.clear();
+ }
+
+ double tr(common_obs_r.getObservations().front().time.time + common_obs_r.getObservations().front().time.sec);
+ double tk(common_obs_k.getObservations().front().time.time + common_obs_k.getObservations().front().time.sec);
+
+ int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n));
+ int n_common_sats = common_sats_pos_k.size();
+
+ if(n_common_sats < required_n_sats)
+ {
+ #if GNSS_UTILS_SD_DEBUG == 1
+ printf("SD: NOT ENOUGH COMMON SATS");
+ printf("SD: %i common sats available, %i sats are REQUIRED. [ SKIPPING SD ]",
+ n_common_sats,
+ required_n_sats);
+ #endif
+ return false;
+ }
+
+ // MULTI-FREQUENCY
+ std::map<int,std::pair<int,int>> row_2_sat_freq;
+ int row = 0;
+ for (auto obs_idx = 0; obs_idx < common_obs_k.size(); obs_idx++)
+ {
+ auto&& obs_r = common_obs_r.getObservationByIdx(obs_idx);
+ auto&& obs_k = common_obs_k.getObservationByIdx(obs_idx);
+
+ const int& sat_number = obs_k.sat;
+
+ if(sd_params.use_carrier_phase)
+ {
+ // L1/E1
+ if (std::abs(obs_r.L[0]) > 1e-12 and
+ std::abs(obs_k.L[0]) > 1e-12)
+ {
+ row_2_sat_freq[row] = std::make_pair(sat_number,0);
+ row++;
+ }
+ if (!sd_params.use_multi_freq)
+ continue;
+
+ // L2 (GPS/GLO/QZS)
+ int sys=satsys(sat_number,NULL);
+ if (NFREQ >= 2 and (sys&(SYS_GPS|SYS_GLO|SYS_QZS)) and
+ std::abs(obs_r.L[1]) > 1e-12 and
+ std::abs(obs_k.L[1]) > 1e-12)
+ {
+ row_2_sat_freq[row] = std::make_pair(sat_number,1);
+ row++;
+ }
+ // E5 (GAL)
+ if (NFREQ >= 3 and (sys&SYS_GAL) and
+ std::abs(obs_r.L[2]) > 1e-12 and
+ std::abs(obs_k.L[2]) > 1e-12)
+ {
+ row_2_sat_freq[row] = std::make_pair(sat_number,2);
+ row++;
+ }
+ }
+ else
+ // L1/E1
+ if (std::abs(obs_r.P[0]) > 1e-12 and
+ std::abs(obs_k.P[0]) > 1e-12)
+ {
+ row_2_sat_freq[row] = std::make_pair(sat_number,0);
+ row++;
+ }
+ }
+ int n_differences = row_2_sat_freq.size();
+
+ // Initial guess
+ Eigen::Vector4d d_0(d);
+
+ #if GNSS_UTILS_SD_DEBUG == 1
+ std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
+ std::cout << "d initial guess: " << d_0.transpose() << std::endl;
+ #endif
+
+ // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
+ Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences,4));
+ Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
+ Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
+ Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3,n_differences));
+ Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3,n_differences));
+ int sat_i = 0;
+
+ for (auto row_sat_freq_it : row_2_sat_freq)
+ {
+ const int& row = row_sat_freq_it.first;
+ const int& sat_number = row_sat_freq_it.second.first;
+ const int& freq = row_sat_freq_it.second.second;
+
+ auto obs_r = common_obs_r.getObservationBySat(sat_number);
+ auto obs_k = common_obs_k.getObservationBySat(sat_number);
+
+ // excluded satellite
+ if (excluded_sats.count(sat_number) != 0)
+ continue;
+
+ // Satellite's positions
+ s_r.col(row) = common_sats_pos_r.at(sat_number);
+ s_k.col(row) = common_sats_pos_k.at(sat_number);
+
+ if(sd_params.use_carrier_phase)
+ drho_m(row) = (obs_r.L[freq] * nav_r.getNavigation().lam[sat_number][freq]) -
+ (obs_k.L[freq] * nav_k.getNavigation().lam[sat_number][freq]);
+
+ else
+ drho_m(row) = obs_r.P[freq] - obs_k.P[freq];
+
+ if(!sd_params.relinearize_jacobian)
+ {
+ // Unit vectors from recerivers to satellites
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+
+ // Fill Jacobian matrix
+ A(row, 0) = u_k(0);
+ A(row, 1) = u_k(1);
+ A(row, 2) = u_k(2);
+ A(row, 3) = -1.0;
+ }
+ }
+
+ // LEAST SQUARES SOLUTION =======================================================================
+ for (int j = 0; j < sd_params.max_iterations; j++)
+ {
+ // fill A and r
+ for (int row = 0; row < A.rows(); row++)
+ {
+ // Evaluate r
+ r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+
+ // Evaluate A
+ if (sd_params.relinearize_jacobian)
+ {
+ // Unit vectors from rcvrs to sats
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+
+ // Fill Jacobian matrix
+ A(row, 0) = u_k(0);
+ A(row, 1) = u_k(1);
+ A(row, 2) = u_k(2);
+ A(row, 3) = -1.0;
+ }
+ }
+
+ // Solve
+ cov_d = (A.transpose()*A).inverse();
+ d -= cov_d*A.transpose()*r;
+
+ // wrong solution
+ if(d.array().isNaN().any())
+ {
+ std::cout << "SD: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
+ return false;
+ }
+
+ // residual
+ //residual = sqrt((r - A * d).squaredNorm() / A.rows());
+ residual = (r + A * d).norm();
+ //std::cout << "residual = " << residual << std::endl;
+ //std::cout << "SD2: r-A*d = " << (r + A * d).transpose() << std::endl;
+
+ // RAIM ====================================== (only at first iteration)
+ //TODO: RAIM WITH SATELLITES, not ROWS
+ if(j==0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
+ {
+ int worst_sat_row = -1;
+ double best_residual = 1e12;
+ Eigen::Vector4d best_d;
+ int n_removed_rows = 1;
+
+ // remove some satellites
+ while(excluded_sats.size() < sd_params.raim_n)
+ {
+ auto A_raim = A;
+ auto r_raim = r;
+
+ // solve removing each satellite
+ for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
+ {
+ // Multi-freq: some rows for the same satellite
+ n_removed_rows = 1;
+ if (sd_params.use_multi_freq)
+ while (row_removed + n_removed_rows < A_raim.rows() and row_2_sat_freq.at(row_removed+n_removed_rows) == row_2_sat_freq.at(row_removed))
+ n_removed_rows++;
+
+ // remove satellite rows
+ for (auto r = 0; r < n_removed_rows; r++)
+ {
+ A_raim.row(row_removed+r) = Eigen::Vector4d::Zero().transpose();
+ r_raim(row_removed+r) = 0; // not necessary
+ }
+
+ // solve
+ Eigen::Vector4d d_raim = d_0-(A_raim.transpose()*A_raim).inverse()*A_raim.transpose()*r_raim;
+
+ // no NaN
+ if (!d_raim.array().isNaN().any())
+ {
+ // residual
+ residual = (r - A * d_raim).norm() / A.rows(); //FIXME: evaluate with XX_raim or not? I think next line is the good one
+ //residual = sqrt((r_raim - A_raim * d_raim).squaredNorm() / A_raim.rows());
+
+ // store if best residual
+ if (residual < best_residual)
+ {
+ worst_sat_row = row_removed;
+ best_residual = residual;
+ best_d = d_raim;
+ }
+ }
+ // restore initial A and r
+ for (auto row = 0; row < n_removed_rows; row++)
+ {
+ A_raim.row(row_removed+row) = A.row(row_removed+row);
+ r_raim(row_removed+row) = r(row_removed+row);
+ }
+ row_removed += (n_removed_rows-1);
+ }
+
+ // No successful RAIM solution
+ if(worst_sat_row == -1)
+ {
+ printf("SD: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
+ return false;
+ }
+
+ // store removed sat
+ excluded_sats.insert(row_2_sat_freq[worst_sat_row].first);
+
+ // decrease n_common_sats
+ n_differences=-n_removed_rows;
+ n_common_sats--;
+
+ // Remove selected satellite from problem
+ auto A_aux = A;
+ auto r_aux = r;
+ auto drho_m_aux = drho_m;
+ auto s_r_aux = s_r;
+ auto s_k_aux = s_k;
+ A.conservativeResize(n_differences, Eigen::NoChange);
+ r.conservativeResize(n_differences);
+ drho_m.conservativeResize(n_differences);
+ s_r.conservativeResize(Eigen::NoChange, n_differences);
+ s_k.conservativeResize(Eigen::NoChange, n_differences);
+
+ int back_elements_changing = A.rows() - worst_sat_row - n_removed_rows;
+ if (back_elements_changing != 0) //if last satelite removed, conservative resize did the job
+ {
+ A .bottomRows(back_elements_changing) = A_aux .bottomRows(back_elements_changing);
+ s_r .rightCols(back_elements_changing) = s_r_aux .rightCols(back_elements_changing);
+ s_k .rightCols(back_elements_changing) = s_k_aux .rightCols(back_elements_changing);
+ r .tail(back_elements_changing) = r_aux .tail(back_elements_changing);
+ drho_m .tail(back_elements_changing) = drho_m_aux.tail(back_elements_changing);
+ }
+
+ // apply the RAIM solution
+ d = best_d;
+ cov_d = (A.transpose()*A).inverse();
+
+ #if GNSS_UTILS_SD_DEBUG == 1
+ std::cout << "SD After RAIM iteration" << std::endl;
+ std::cout << "\tExcluded sats : ";
+ for (auto dsat : excluded_sats)
+ std::cout << (int)dsat << " ";
+ std::cout << std::endl;
+ std::cout << "\tCommon sats : " << n_common_sats << std::endl;
+ std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tClock offset = " << d(3) << std::endl;
+ std::cout << "\tResidual = " << residual << std::endl;
+ #endif
+ }
+
+ #if GNSS_UTILS_SD_DEBUG == 1
+ std::cout << "SD After RAIM " << std::endl;
+ std::cout << "\tExcluded sats : ";
+ for (auto dsat : excluded_sats)
+ std::cout << (int)dsat << " ";
+ std::cout << std::endl;
+ std::cout << "\tCommon sats : " << n_common_sats << std::endl;
+ std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tClock offset = " << d(3) << std::endl;
+ std::cout << "\tResidual = " << residual << std::endl;
+ #endif
+ }
+
+ #if GNSS_UTILS_SD_DEBUG == 1
+ std::cout << "SD iteration " << j << std::endl;
+ std::cout << "\tExcluded sats : ";
+ for (auto dsat : excluded_sats)
+ std::cout << (int)dsat << " ";
+ std::cout << std::endl;
+ std::cout << "\tCommon sats : " << n_common_sats << std::endl;
+ std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tClock offset = " << d(3) << std::endl;
+ std::cout << "\tResidual = " << residual << std::endl;
+ #endif
+ }
+
+ // weight covariance with the measurement noise (proportional to time)
+ double sq_sigma_SD = (tk-tr) * sd_params.sigma_atm * sd_params.sigma_atm +
+ 2 * (sd_params.use_carrier_phase ?
+ sd_params.sigma_carrier * sd_params.sigma_carrier :
+ sd_params.sigma_code * sd_params.sigma_code);
+ cov_d *= sq_sigma_SD;
+ //residual = (r - A * d).norm() / A.rows();
+
+ // Computing error on measurement space
+ for(int row = 0; row < n_differences; row++)
+ {
+ r(row) = drho_m(row)
+ - (s_k.col(row) - x_r - d.head(3)).norm()
+ + (s_r.col(row) - x_r).norm()
+ - d(3);
+ }
+ residual = sqrt(r.squaredNorm() / r.size());
+
return true;
}
+void filterCommonSatellites(Observations& common_obs_r, std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_r,
+ Observations& common_obs_k, std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_k,
+ std::set<unsigned char>& discarded_sats, const Eigen::Vector3d& x_r,
+ const SingleDifferencesParams& sd_params, const prcopt_t& opt)
+{
+ assert(common_obs_r.size() == common_obs_k.size());
+ assert(common_obs_r.size() == common_sats_pos_r.size());
+ assert(common_obs_r.size() == common_sats_pos_k.size());
+
+ for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
+ {
+ auto&& obs_r = common_obs_r.getObservationByIdx(obs_i);
+ auto&& obs_k = common_obs_k.getObservationByIdx(obs_i);
+ assert(obs_r.sat == obs_k.sat && "common satellites observations have to be ordered equally");
+ const int& sat_number = obs_r.sat;
+
+
+ bool discard = false;
+
+ // already discarded sats
+ if (discarded_sats.count(sat_number) != 0)
+ discard = true;
+ else
+ {
+ // wrong data (satellite is not included in the discarded list)
+ if (sd_params.use_carrier_phase and (std::abs(obs_r.L[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
+ {
+ discard = true;
+ }
+ else if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
+ {
+ discard = true;
+ }
+ else
+ {
+ // bad satellite position (satellite is not included in the discarded list)
+ if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero()) or
+ common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero()))
+ {
+ discard = true;
+ }
+ else
+ {
+ // constellation
+ int sys=satsys(obs_r.sat,NULL);
+ if (!(sys & opt.navsys))
+ {
+ discarded_sats.insert(sat_number);
+ discard = true;
+ }
+ else
+ {
+ // check both elevations
+ double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]),
+ computeSatElevation(x_r, common_sats_pos_k[sat_number]));
+ if (elevation < opt.elmin)
+ {
+ discarded_sats.insert(sat_number);
+ discard = true;
+ }
+ else
+ {
+ // snr TODO: multifrequency (2nd param and 3rd idx)
+ if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
+ {
+ discarded_sats.insert(sat_number);
+ discard = true;
+ }
+ }
+ }
+ }
+ }
+ }
+
+ // remove from vectors
+ if (discard)
+ {
+ common_obs_r.removeObservationByIdx(obs_i);
+ common_obs_k.removeObservationByIdx(obs_i);
+ common_sats_pos_r.erase(sat_number);
+ common_sats_pos_k.erase(sat_number);
+ }
+ }
+}
+
}
diff --git a/src/utils.cpp b/src/utils.cpp
index 3e41a0e2461e417c8e3ce267963f681d9ca6614e..c967e3f7c26ea944279401f68777fb99b486a789 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -29,7 +29,7 @@ void print(std::string & _msg)
std::cout << _name << ": [";
for (int ii=0; ii<size; ++ii)
{
- std::cout << (unsigned)_array[ii];
+ std::cout << (int)(_array[ii]);
if (ii==size-1)
std::cout << "] \n";
else
@@ -63,4 +63,4 @@ void print(std::string & _msg)
}
}
-}
\ No newline at end of file
+}