From f48bc6c2b667a299e64e566fff5ae9595cbbc5e7 Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Mon, 30 Mar 2020 13:04:04 +0200
Subject: [PATCH] [utils] wip
---
.gitignore | 3 +-
.vscode/c_cpp_properties.json | 16 +
deps/RTKLIB | 2 +-
include/gnss_utils/gnss_utils.h | 134 ++++--
src/gnss_utils.cpp | 823 +++++++++++++++++---------------
test/gtest_navigation.cpp | 13 +
test/gtest_observations.cpp | 14 +
7 files changed, 561 insertions(+), 444 deletions(-)
create mode 100644 .vscode/c_cpp_properties.json
create mode 100644 test/gtest_navigation.cpp
create mode 100644 test/gtest_observations.cpp
diff --git a/.gitignore b/.gitignore
index 0948008..5a52268 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
bin/
build/
-lib/
\ No newline at end of file
+lib/
+.clang-format
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..afd1a20
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,16 @@
+{
+ "configurations": [
+ {
+ "name": "Linux",
+ "includePath": [
+ "${workspaceFolder}/**",
+ "${workspaceFolder}/include"
+ ],
+ "defines": [],
+ "compilerPath": "/usr/bin/clang",
+ "cStandard": "c11",
+ "intelliSenseMode": "clang-x64"
+ }
+ ],
+ "version": 4
+}
\ No newline at end of file
diff --git a/deps/RTKLIB b/deps/RTKLIB
index 4b8f505..eac44a4 160000
--- a/deps/RTKLIB
+++ b/deps/RTKLIB
@@ -1 +1 @@
-Subproject commit 4b8f505328faeb827fe7e0eb5adac1dddaf39cf4
+Subproject commit eac44a45c19ac56c0c031135e9818788b5993005
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index ec75472..ce4304f 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -1,7 +1,6 @@
#ifndef GNSS_UTILS_H
#define GNSS_UTILS_H
-
#include <vector>
#include <iostream>
#include <memory>
@@ -14,62 +13,91 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
-extern "C"
-{
- #include "rtklib.h"
+extern "C" {
+#include "rtklib.h"
}
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
- };
-
- ComputePosOutput computePos(const Observations & _observations,
- Navigation & _navigation,
- const prcopt_t & _prcopt);
-
- // ComputePosOutput computePosOwn(const Observations & _observations,
- // Navigation & _navigation,
- // const prcopt_t & _prcopt);
-
- // int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
- // const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
- // char *msg);
-
- 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);
-
- Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef);
- Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _latlon);
- Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef);
- Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu);
-
- void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
- void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_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<int,Eigen::Vector3d>& sats_pos);
+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
+};
+
+ComputePosOutput computePos(const Observations& _observations, Navigation& _navigation, const prcopt_t& _prcopt);
+
+// ComputePosOutput computePosOwn(const Observations & _observations,
+// Navigation & _navigation,
+// const prcopt_t & _prcopt);
+
+// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
+// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
+// char *msg);
+
+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);
+
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef);
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon);
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef);
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu);
+
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF);
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_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<int, Eigen::Vector3d>& sats_pos);
+
+template <typename T>
+bool equalArray(const T* array1, const T* array2, const int& size1, const int& size2)
+{
+ if (size1 != size2)
+ return false;
+
+ for (int ii = 0; ii < size1; ++ii)
+ {
+ if (array1[ii] != array2[ii])
+ return false;
+ }
+
+ return true;
}
+bool equalTime(const gtime_t& time1, const gtime_t& time2);
+bool equalObservations(const obsd_t& obs1, const obsd_t& obs2);
+
+} // namespace GNSSUtils
#endif
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 743ef28..802d0b3 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -2,439 +2,484 @@
namespace GNSSUtils
{
-
- ComputePosOutput computePos(const GNSSUtils::Observations & _observations,
- GNSSUtils::Navigation & _navigation,
- const prcopt_t & _prcopt)
- {
+ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
+ GNSSUtils::Navigation& _navigation,
+ const prcopt_t& _prcopt)
+{
+ // Remove duplicated satellites
+ _navigation.uniqueNavigation();
+
+ // Define error msg
+ char msg[128] = "";
- // Remove duplicated satellites
- _navigation.uniqueNavigation();
-
- // Define error msg
- char msg[128] = "";
-
- GNSSUtils::ComputePosOutput output;
- sol_t sol;
- sol = {{0}};
-
- output.pos_stat = pntpos(_observations.data(), _observations.size(),
- &(_navigation.getNavigation()),
- &_prcopt, &sol, NULL, NULL, msg);
-
- if (output.pos_stat == 0)
- {
- std::cout << "Bad computing: " << msg << "\n";
- }
-
- output.time = sol.time.time;
- output.sec = sol.time.sec;
- output.pos = Eigen::Vector3d(sol.rr);
- std::cout << "Compute pos: " << output.pos.transpose() << "\n";
- output.vel = Eigen::Vector3d(&sol.rr[3]);
- output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
- sol.qr[3], sol.qr[1], sol.qr[4],
- sol.qr[5], sol.qr[3], sol.qr[2];
-
- // XXX: segmentation fault here.
- // if (sol.dtr != NULL)
- // {
- // 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;
- output.age = sol.age;
- output.ratio = sol.ratio;
- output.lat_lon = ecefToLatLonAlt(output.pos);
-
- return output;
+ GNSSUtils::ComputePosOutput output;
+ sol_t sol;
+ sol = { { 0 } };
+
+ output.pos_stat = pntpos(
+ _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
+
+ if (output.pos_stat == 0)
+ {
+ std::cout << "Bad computing: " << msg << "\n";
}
- // ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations,
- // GNSSUtils::Navigation & _navigation,
- // const prcopt_t & _prcopt)
- // {
-
- // // Remove duplicated satellites
- // uniqnav(&(_navigation.getNavigation()));
-
- // // Define error msg
- // char msg[128] = "";
-
- // GNSSUtils::ComputePosOutput output;
- // sol_t sol;
- // sol = {{0}};
-
- // output.pos_stat = pntposOwn(_observations.data(), _observations.size(),
- // &(_navigation.getNavigation()),
- // &_prcopt, &sol, NULL, NULL, msg);
-
- // if (output.pos_stat == 0)
- // {
- // std::cout << "Bad computing: " << msg << "\n";
- // }
-
- // output.time = sol.time.time;
- // output.sec = sol.time.sec;
- // output.pos = Eigen::Vector3d(sol.rr);
- // // std::cout << "Compute pos: " << output.pos.transpose() << "\n";
- // output.vel = Eigen::Vector3d(&sol.rr[3]);
- // output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
- // sol.qr[3], sol.qr[1], sol.qr[4],
- // sol.qr[5], sol.qr[3], sol.qr[2];
-
- // // XXX: segmentation fault here.
- // // if (sol.dtr != NULL)
- // // {
- // // 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;
- // output.age = sol.age;
- // output.ratio = sol.ratio;
- // output.lat_lon = ecefToLatLonAlt(output.pos);
-
- // return output;
- // }
-
- /* single-point positioning COPIED FROM RTKLIB ------------------------------
- * compute receiver position, velocity, clock bias by single-point positioning
- * with pseudorange and doppler observables
- * args : obsd_t *obs I observation data
- * int n I number of observation data
- * nav_t *nav I navigation data
- * prcopt_t *opt I processing options
- * sol_t *sol IO solution
- * double *azel IO azimuth/elevation angle (rad) (NULL: no output)
- * ssat_t *ssat IO satellite status (NULL: no output)
- * char *msg O error message for error exit
- * return : status(1:ok,0:error)
- * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
- * receiver bias are negligible (only involving glonass-gps time offset
- * and receiver bias)
- *-----------------------------------------------------------------------------*/
- // int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
- // const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
- // char *msg)
+ output.time = sol.time.time;
+ output.sec = sol.time.sec;
+ output.pos = Eigen::Vector3d(sol.rr);
+ std::cout << "Compute pos: " << output.pos.transpose() << "\n";
+ output.vel = Eigen::Vector3d(&sol.rr[3]);
+ output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], sol.qr[3], sol.qr[1], sol.qr[4], sol.qr[5], sol.qr[3], sol.qr[2];
+
+ // XXX: segmentation fault here.
+ // if (sol.dtr != NULL)
// {
- // prcopt_t opt_=*opt;
- // double *rs,*dts,*var,*azel_,*resp;
- // int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
-
- // sol->stat=SOLQ_NONE;
-
- // if (n<=0) {strcpy(msg,"no observation data"); return 0;}
-
- // trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
-
- // sol->time=obs[0].time; msg[0]='\0';
-
- // rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
-
- // if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
- // #if 0
- // opt_.sateph =EPHOPT_BRDC;
- // #endif
- // opt_.ionoopt=IONOOPT_BRDC;
- // opt_.tropopt=TROPOPT_SAAS;
- // }
- // /* satellite positons, velocities and clocks */
- // satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
-
- // /* estimate receiver position with pseudorange */
- // stat=estposOwn(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
-
- // /* raim fde */
- // if (!stat&&n>=6&&opt->posopt[4]) {
- // stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
- // }
- // /* estimate receiver velocity with doppler */
- // //if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
-
- // if (azel) {
- // for (i=0;i<n*2;i++) azel[i]=azel_[i];
- // }
- // if (ssat) {
- // for (i=0;i<MAXSAT;i++) {
- // ssat[i].vs=0;
- // ssat[i].azel[0]=ssat[i].azel[1]=0.0;
- // ssat[i].resp[0]=ssat[i].resc[0]=0.0;
- // ssat[i].snr[0]=0;
- // }
- // for (i=0;i<n;i++) {
- // ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
- // ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
- // ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
- // if (!vsat[i]) continue;
- // ssat[obs[i].sat-1].vs=1;
- // ssat[obs[i].sat-1].resp[0]=resp[i];
- // }
- // }
- // free(rs); free(dts); free(var); free(azel_); free(resp);
- // return stat;
+ // 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;
+ output.age = sol.age;
+ output.ratio = sol.ratio;
+ output.lat_lon = ecefToLatLonAlt(output.pos);
+
+ return output;
+}
- 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 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);
-
- return 0;
- }
+// ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations,
+// GNSSUtils::Navigation & _navigation,
+// const prcopt_t & _prcopt)
+// {
+
+// // Remove duplicated satellites
+// uniqnav(&(_navigation.getNavigation()));
+
+// // Define error msg
+// char msg[128] = "";
+
+// GNSSUtils::ComputePosOutput output;
+// sol_t sol;
+// sol = {{0}};
+
+// output.pos_stat = pntposOwn(_observations.data(), _observations.size(),
+// &(_navigation.getNavigation()),
+// &_prcopt, &sol, NULL, NULL, msg);
+
+// if (output.pos_stat == 0)
+// {
+// std::cout << "Bad computing: " << msg << "\n";
+// }
+
+// output.time = sol.time.time;
+// output.sec = sol.time.sec;
+// output.pos = Eigen::Vector3d(sol.rr);
+// // std::cout << "Compute pos: " << output.pos.transpose() << "\n";
+// output.vel = Eigen::Vector3d(&sol.rr[3]);
+// output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
+// sol.qr[3], sol.qr[1], sol.qr[4],
+// sol.qr[5], sol.qr[3], sol.qr[2];
+
+// // XXX: segmentation fault here.
+// // if (sol.dtr != NULL)
+// // {
+// // 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;
+// output.age = sol.age;
+// output.ratio = sol.ratio;
+// output.lat_lon = ecefToLatLonAlt(output.pos);
+
+// return output;
+// }
+
+/* single-point positioning COPIED FROM RTKLIB ------------------------------
+ * compute receiver position, velocity, clock bias by single-point positioning
+ * with pseudorange and doppler observables
+ * args : obsd_t *obs I observation data
+ * int n I number of observation data
+ * nav_t *nav I navigation data
+ * prcopt_t *opt I processing options
+ * sol_t *sol IO solution
+ * double *azel IO azimuth/elevation angle (rad) (NULL: no output)
+ * ssat_t *ssat IO satellite status (NULL: no output)
+ * char *msg O error message for error exit
+ * return : status(1:ok,0:error)
+ * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
+ * receiver bias are negligible (only involving glonass-gps time offset
+ * and receiver bias)
+ *-----------------------------------------------------------------------------*/
+// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
+// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
+// char *msg)
+// {
+// prcopt_t opt_=*opt;
+// double *rs,*dts,*var,*azel_,*resp;
+// int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
+
+// sol->stat=SOLQ_NONE;
+
+// if (n<=0) {strcpy(msg,"no observation data"); return 0;}
+
+// trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
+
+// sol->time=obs[0].time; msg[0]='\0';
+
+// rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
+
+// if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
+// #if 0
+// opt_.sateph =EPHOPT_BRDC;
+// #endif
+// opt_.ionoopt=IONOOPT_BRDC;
+// opt_.tropopt=TROPOPT_SAAS;
+// }
+// /* satellite positons, velocities and clocks */
+// satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
+
+// /* estimate receiver position with pseudorange */
+// stat=estposOwn(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+
+// /* raim fde */
+// if (!stat&&n>=6&&opt->posopt[4]) {
+// stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+// }
+// /* estimate receiver velocity with doppler */
+// //if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
+
+// if (azel) {
+// for (i=0;i<n*2;i++) azel[i]=azel_[i];
+// }
+// if (ssat) {
+// for (i=0;i<MAXSAT;i++) {
+// ssat[i].vs=0;
+// ssat[i].azel[0]=ssat[i].azel[1]=0.0;
+// ssat[i].resp[0]=ssat[i].resc[0]=0.0;
+// ssat[i].snr[0]=0;
+// }
+// for (i=0;i<n;i++) {
+// ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
+// ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
+// ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
+// if (!vsat[i]) continue;
+// ssat[obs[i].sat-1].vs=1;
+// ssat[obs[i].sat-1].resp[0]=resp[i];
+// }
+// }
+// free(rs); free(dts); free(var); free(azel_); free(resp);
+// return stat;
+// }
+
+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 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);
+
+ return 0;
+}
-Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef)
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
{
- Eigen::Vector3d pos;
- ecef2pos(_ecef.data(), pos.data());
+ Eigen::Vector3d pos;
+ ecef2pos(_ecef.data(), pos.data());
- return pos;
+ return pos;
}
-Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _pos)
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos)
{
- Eigen::Vector3d ecef;
- pos2ecef(_pos.data(), ecef.data());
+ Eigen::Vector3d ecef;
+ pos2ecef(_pos.data(), ecef.data());
- return ecef;
+ return ecef;
}
-
-Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef)
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef)
{
- Eigen::Matrix3d cov_enu;
-
- /* RTKLIB transform covariance to local tangental coordinate --------------------------
- * transform ecef covariance to local tangental coordinate
- * args : double *pos I geodetic position {lat,lon} (rad)
- * double *P I covariance in ecef coordinate
- * double *Q O covariance in local tangental coordinate
- * return : none
- *-----------------------------------------------------------------------------*/
- //extern void covenu(const double *pos, const double *P, double *Q);
- covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data());
-
- return cov_enu;
+ Eigen::Matrix3d cov_enu;
+
+ /* RTKLIB transform covariance to local tangental coordinate --------------------------
+ * transform ecef covariance to local tangental coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *P I covariance in ecef coordinate
+ * double *Q O covariance in local tangental coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ // extern void covenu(const double *pos, const double *P, double *Q);
+ covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data());
+
+ return cov_enu;
}
-Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu)
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu)
{
- Eigen::Matrix3d cov_ecef;
-
- /* RTKLIB transform local enu coordinate covariance to xyz-ecef -----------------------
- * transform local enu covariance to xyz-ecef coordinate
- * args : double *pos I geodetic position {lat,lon} (rad)
- * double *Q I covariance in local enu coordinate
- * double *P O covariance in xyz-ecef coordinate
- * return : none
- *-----------------------------------------------------------------------------*/
- //extern void covecef(const double *pos, const double *Q, double *P)
- covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data());
-
- return cov_ecef;
+ Eigen::Matrix3d cov_ecef;
+
+ /* RTKLIB transform local enu coordinate covariance to xyz-ecef -----------------------
+ * transform local enu covariance to xyz-ecef coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *Q I covariance in local enu coordinate
+ * double *P O covariance in xyz-ecef coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ // extern void covecef(const double *pos, const double *Q, double *P)
+ covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data());
+
+ return cov_ecef;
}
-void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF)
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF)
{
// Convert ECEF coordinates to geodetic coordinates.
// J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
// to geodetic coordinates," IEEE Transactions on Aerospace and
// Electronic Systems, vol. 30, pp. 957-961, 1994.
-// double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
-// double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
-// double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
-// double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq;
-// double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
-// double S = cbrt(1 + C + sqrt(C * C + 2 * C));
-// double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
-// double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
-// double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
-// + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
-// - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * r * r);
-// double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2));
-// double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
-//
-// double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
-// double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
-//
-//
-// double sLat = sin(latitude);
-// double cLat = cos(latitude);
-// double sLon = sin(longitude);
-// double cLon = cos(longitude);
-//
-// R_ENU_ECEF(0,0) = -sLon;
-// R_ENU_ECEF(0,1) = cLon;
-// R_ENU_ECEF(0,2) = 0.0;
-//
-// R_ENU_ECEF(1,0) = -sLat*cLon;
-// R_ENU_ECEF(1,1) = -sLat * sLon;
-// R_ENU_ECEF(1,2) = cLat;
-//
-// R_ENU_ECEF(2,0) = cLat * cLon;
-// R_ENU_ECEF(2,1) = cLat * sLon;
-// R_ENU_ECEF(2,2) = sLat;
-//
-// t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
-
- Eigen::Vector3d ENU_lat_lon_alt = GNSSUtils::ecefToLatLonAlt(_t_ECEF_ENU);
-
- double sLat = sin(ENU_lat_lon_alt(0));
- double cLat = cos(ENU_lat_lon_alt(0));
- double sLon = sin(ENU_lat_lon_alt(1));
- double cLon = cos(ENU_lat_lon_alt(1));
-
- R_ENU_ECEF(0,0) = -sLon;
- R_ENU_ECEF(0,1) = cLon;
- R_ENU_ECEF(0,2) = 0.0;
-
- R_ENU_ECEF(1,0) = -sLat*cLon;
- R_ENU_ECEF(1,1) = -sLat * sLon;
- R_ENU_ECEF(1,2) = cLat;
-
- R_ENU_ECEF(2,0) = cLat * cLon;
- R_ENU_ECEF(2,1) = cLat * sLon;
- R_ENU_ECEF(2,2) = sLat;
-
- t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU;
+ // double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
+ // double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+ // double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
+ // double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared
+ // * Esq; double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); double S =
+ // cbrt(1 + C + sqrt(C * C + 2 * C)); double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); double Q = sqrt(1 + 2 *
+ // kFirstEccentricitySquared * kFirstEccentricitySquared * P); double r_0 = -(P * kFirstEccentricitySquared * r) /
+ // (1 + Q)
+ // + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
+ // - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P *
+ // r * r);
+ // double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2)
+ // * _t_ECEF_ENU(2)); double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
+ //
+ // double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
+ // double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
+ //
+ //
+ // double sLat = sin(latitude);
+ // double cLat = cos(latitude);
+ // double sLon = sin(longitude);
+ // double cLon = cos(longitude);
+ //
+ // R_ENU_ECEF(0,0) = -sLon;
+ // R_ENU_ECEF(0,1) = cLon;
+ // R_ENU_ECEF(0,2) = 0.0;
+ //
+ // R_ENU_ECEF(1,0) = -sLat*cLon;
+ // R_ENU_ECEF(1,1) = -sLat * sLon;
+ // R_ENU_ECEF(1,2) = cLat;
+ //
+ // R_ENU_ECEF(2,0) = cLat * cLon;
+ // R_ENU_ECEF(2,1) = cLat * sLon;
+ // R_ENU_ECEF(2,2) = sLat;
+ //
+ // t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
+
+ Eigen::Vector3d ENU_lat_lon_alt = GNSSUtils::ecefToLatLonAlt(_t_ECEF_ENU);
+
+ double sLat = sin(ENU_lat_lon_alt(0));
+ double cLat = cos(ENU_lat_lon_alt(0));
+ double sLon = sin(ENU_lat_lon_alt(1));
+ double cLon = cos(ENU_lat_lon_alt(1));
+
+ R_ENU_ECEF(0, 0) = -sLon;
+ R_ENU_ECEF(0, 1) = cLon;
+ R_ENU_ECEF(0, 2) = 0.0;
+
+ R_ENU_ECEF(1, 0) = -sLat * cLon;
+ R_ENU_ECEF(1, 1) = -sLat * sLon;
+ R_ENU_ECEF(1, 2) = cLat;
+
+ R_ENU_ECEF(2, 0) = cLat * cLon;
+ R_ENU_ECEF(2, 1) = cLat * sLon;
+ R_ENU_ECEF(2, 2) = sLat;
+
+ t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU;
}
-void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF)
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF)
{
- double sLat = sin(_ENU_latlonalt(0));
- double cLat = cos(_ENU_latlonalt(0));
- double sLon = sin(_ENU_latlonalt(1));
- double cLon = cos(_ENU_latlonalt(1));
+ double sLat = sin(_ENU_latlonalt(0));
+ double cLat = cos(_ENU_latlonalt(0));
+ double sLon = sin(_ENU_latlonalt(1));
+ double cLon = cos(_ENU_latlonalt(1));
- R_ENU_ECEF(0,0) = -sLon;
- R_ENU_ECEF(0,1) = cLon;
- R_ENU_ECEF(0,2) = 0.0;
+ R_ENU_ECEF(0, 0) = -sLon;
+ R_ENU_ECEF(0, 1) = cLon;
+ R_ENU_ECEF(0, 2) = 0.0;
- R_ENU_ECEF(1,0) = -sLat*cLon;
- R_ENU_ECEF(1,1) = -sLat * sLon;
- R_ENU_ECEF(1,2) = cLat;
+ R_ENU_ECEF(1, 0) = -sLat * cLon;
+ R_ENU_ECEF(1, 1) = -sLat * sLon;
+ R_ENU_ECEF(1, 2) = cLat;
- R_ENU_ECEF(2,0) = cLat * cLon;
- R_ENU_ECEF(2,1) = cLat * sLon;
- R_ENU_ECEF(2,2) = sLat;
+ R_ENU_ECEF(2, 0) = cLat * cLon;
+ R_ENU_ECEF(2, 1) = cLat * sLon;
+ R_ENU_ECEF(2, 2) = sLat;
- Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
+ Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
- t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
+ t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
}
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());
+ // ecef 2 geodetic
+ Eigen::Vector3d receiver_geo;
+ ecef2pos(receiver_ecef.data(), receiver_geo.data());
+
+ // receiver-sat vector ecef
+ Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;
+
+ // receiver-sat vector enu (receiver ecef as origin)
+ Eigen::Vector3d receiver_sat_enu;
+ ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
+ receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z}
+ receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
+
+ // elevation
+ return atan2(receiver_sat_enu(2),
+ sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
+}
+
+void computeSatellitesPositions(const Observations& obs,
+ const Navigation& nav,
+ const prcopt_t& opt,
+ std::map<int, Eigen::Vector3d>& sats_pos)
+{
+ double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
+ int svh[obs.size()];
- // receiver-sat vector ecef
- Eigen::Vector3d receiver_sat_ecef = sat_ecef-receiver_ecef;
+ // std::cout << "computing sats position from sats: ";
+ // for (auto&& obs_ref : obs.getObservations())
+ // std::cout << (int)obs_ref.sat << " ";
+ // std::cout << std::endl;
- // receiver-sat vector enu (receiver ecef as origin)
- Eigen::Vector3d receiver_sat_enu;
- ecef2enu(receiver_geo.data(), //geodetic position {lat,lon} (rad)
- receiver_sat_ecef.data(), //vector in ecef coordinate {x,y,z}
- receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
+ // compute positions
+ satposs(
+ obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), opt.sateph, rs, dts, var, svh);
- // elevation
- return atan2(receiver_sat_enu(2), sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
+ // store positions
+ // std::cout << "filling sats positions: \n";
+ 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];
+ // std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " <<
+ // sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
+ }
}
-void computeSatellitesPositions(const Observations& obs,
- const Navigation& nav,
- const prcopt_t& opt,
- std::map<int,Eigen::Vector3d>& sats_pos)
+bool equalTime(const gtime_t& time1, const gtime_t& time2)
{
- double rs[6*obs.size()],dts[2*obs.size()],var[obs.size()];
- int svh[obs.size()];
-
- //std::cout << "computing sats position from sats: ";
- //for (auto&& obs_ref : obs.getObservations())
- // std::cout << (int)obs_ref.sat << " ";
- //std::cout << std::endl;
-
- // compute positions
- satposs(obs.getObservations().front().time,
- obs.data(),
- obs.size(),
- &nav.getNavigation(),
- opt.sateph,
- rs, dts, var, svh);
-
- // store positions
- //std::cout << "filling sats positions: \n";
- 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];
- //std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " << sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
- }
+ return (difftime(time1.time, time2.time) == 0.0 && time1.sec == time2.sec);
}
+
+bool equalObservations(const obsd_t& obs1, const obsd_t& obs2)
+{
+ if (!equalTime(obs1.time, obs2.time))
+ return false;
+ if (!equalTime(obs1.eventime, obs2.eventime))
+ return false;
+ if (obs1.timevalid != obs2.timevalid)
+ return false;
+ if (obs1.sat != obs2.sat)
+ return false;
+ if (obs1.rcv != obs2.sat)
+ return false;
+ if (memcmp(obs1.SNR, obs2.SNR, sizeof(obs1.SNR)) != 0)
+ return false;
+ if (memcmp(obs1.LLI, obs2.LLI, sizeof(obs1.LLI)) != 0)
+ return false;
+ if (memcmp(obs1.code, obs2.code, sizeof(obs1.code)) != 0)
+ return false;
+ if (memcmp(obs1.qualL, obs2.qualL, sizeof(obs1.qualL)) != 0)
+ return false;
+ if (memcmp(obs1.qualP, obs2.qualP, sizeof(obs1.qualP)) != 0)
+ return false;
+ if (obs1.freq != obs2.freq)
+ return false;
+ if (!equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0])))
+ return false;
+ if (!equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0])))
+ return false;
+ if (!equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0])))
+ return false;
+
+ return true;
}
+
+} // namespace GNSSUtils
diff --git a/test/gtest_navigation.cpp b/test/gtest_navigation.cpp
new file mode 100644
index 0000000..4e4e1db
--- /dev/null
+++ b/test/gtest_navigation.cpp
@@ -0,0 +1,13 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/gnss_utils.h"
+
+TEST(NavigationTests, Whatever)
+{
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
new file mode 100644
index 0000000..80db0f1
--- /dev/null
+++ b/test/gtest_observations.cpp
@@ -0,0 +1,14 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/gnss_utils.h"
+
+TEST(ObservationsTest, LoadFromRinex)
+{
+
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
--
GitLab