Skip to content
Snippets Groups Projects
Commit 94c4fdb3 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

WIP

parent 57dcf28b
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
......@@ -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
......@@ -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
......@@ -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);
}
......
......@@ -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 #######
......
......@@ -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;
}
......
......@@ -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];
}
}
}
......@@ -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));
}
}
This diff is collapsed.
......@@ -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
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment