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

SD working as eurecat, RAIM not tested

parent e7fb40bb
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
#ifndef GNSS_UTILS_H
#define GNSS_UTILS_H
#include <vector>
#include <iostream>
#include <memory>
......
......@@ -5,6 +5,7 @@
#include <map>
#include <iostream>
#include <memory>
#include <cassert>
#include "gnss_utils/utils.h"
......@@ -64,26 +65,12 @@ class Observations
private:
// 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<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_;
......@@ -96,21 +83,25 @@ inline std::vector<obsd_t>& Observations::getObservations()
inline obsd_t& Observations::getObservationBySat(const unsigned char& sat_number)
{
assert(sat_2_idx_.count(sat_number) != 0);
return obs_.at(sat_2_idx_.at(sat_number));
}
inline const obsd_t& Observations::getObservationBySat(const unsigned char& sat_number) const
{
assert(sat_2_idx_.count(sat_number) != 0);
return obs_.at(sat_2_idx_.at(sat_number));
}
inline obsd_t& Observations::getObservationByIdx(const int& idx)
{
assert(obs_.size() > idx);
return obs_.at(idx);
}
inline const obsd_t& Observations::getObservationByIdx(const int& idx) const
{
assert(obs_.size() > idx);
return obs_.at(idx);
}
......
......@@ -8,16 +8,12 @@
#ifndef INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_
#define INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_
#define GNSS_UTILS_SD_DEBUG 1
#define GNSS_UTILS_SD_DEBUG 0
#include <set>
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
extern "C"
{
#include "rtklib.h"
}
namespace GNSSUtils
{
......@@ -53,7 +49,7 @@ namespace GNSSUtils
const Observations& common_obs_k, const Navigation& nav_k, const std::map<int,Eigen::Vector3d>& common_sats_pos_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
double& residual, std::set<int>& discarded_sats,
const SingleDifferencesParams& sd_opt, const prcopt_t& opt);
const SingleDifferencesParams& sd_opt);
void filterCommonSatellites(Observations& common_obs_r, std::map<int,Eigen::Vector3d>& common_sats_pos_r,
Observations& common_obs_k, std::map<int,Eigen::Vector3d>& common_sats_pos_k,
......
......@@ -275,8 +275,8 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve
// 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}
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)));
......@@ -290,6 +290,11 @@ void computeSatellitesPositions(const Observations& obs,
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(),
......@@ -299,13 +304,14 @@ void computeSatellitesPositions(const Observations& obs,
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;
}
}
}
......@@ -6,6 +6,22 @@ using namespace GNSSUtils;
Navigation::Navigation()
{
nav_.eph =NULL;
nav_.geph=NULL;
nav_.seph=NULL;
nav_.peph=NULL;
nav_.pclk=NULL;
nav_.alm =NULL;
nav_.tec =NULL;
nav_.n =nav_.nmax =0;
nav_.ng=nav_.ngmax=0;
nav_.ns=nav_.nsmax=0;
nav_.ne=nav_.nemax=0;
nav_.nc=nav_.ncmax=0;
nav_.na=nav_.namax=0;
nav_.nt=nav_.ntmax=0;
// allocateEphemeris(0);
// allocateGLONASSEphemeris(0);
// allocateSBASEphemeris(0);
......@@ -191,6 +207,8 @@ bool Navigation::addAlmanac(const alm_t& alm)
//{
// std::cout << "Navigation::deleteAlmanac...\n";
// free(nav_.alm);
// nav_.na = 0;
// nav_.namax = 0;
// std::cout << "deleted!\n";
//}
......
......@@ -23,10 +23,19 @@ void Observations::clearObservations()
void Observations::addObservation(const obsd_t & obs)
{
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
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;
idx_2_sat_.push_back(obs.sat);
sat_2_idx_[obs.sat] = obs_.size()-1;
//std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
void Observations::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
......@@ -57,6 +66,54 @@ void Observations::loadFromRinex(const std::string& rnx_file, gtime_t t_start, g
free(obs.data);
}
void Observations::removeObservationByIdx(const int& _idx)
{
//std::cout << "removing observation of idx " << _idx << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(obs_.size() > _idx);
assert(idx_2_sat_.size() > _idx);
assert(sat_2_idx_.count(idx_2_sat_.at(_idx)) != 0);
// remove obs from sat_2_idx map
sat_2_idx_.erase(idx_2_sat_.at(_idx));
// decrease the idx of the observations after the removed one
for (auto&& sat_idx : sat_2_idx_)
if (sat_idx.second > _idx)
sat_idx.second--;
// remove obs from obs and idx_2_sat vectors
obs_.erase(obs_.begin() + _idx);
idx_2_sat_.erase(idx_2_sat_.begin() + _idx);
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
void Observations::removeObservationBySat(const int& _sat)
{
//std::cout << "removing observation of sat " << _sat << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(sat_2_idx_.count(_sat) != 0);
assert(hasSatellite(_sat));
assert(obs_.size() > sat_2_idx_.at(_sat));
assert(idx_2_sat_.size() > sat_2_idx_.at(_sat));
int idx = sat_2_idx_.at(_sat);
// remove obs from sat_2_idx map
sat_2_idx_.erase(_sat);
// decrease the idx of the observations after the removed one
for (auto&& sat_idx : sat_2_idx_)
if (sat_idx.second > idx)
sat_idx.second--;
// remove obs from obs and idx_2_sat vectors
obs_.erase(obs_.begin() + idx);
idx_2_sat_.erase(idx_2_sat_.begin() + idx);
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
void Observations::print(obsd_t & _obs)
{
std::string msg = "Observation of satellite #" + std::to_string(_obs.sat);
......@@ -95,6 +152,15 @@ void Observations::findCommonObservations(const Observations& obs_1, const Obser
common_obs_1.clearObservations();
common_obs_2.clearObservations();
//std::cout << "obs 1 sats: ";
//for (auto&& obs_1_ref : obs_1.getObservations())
// std::cout << (int)obs_1_ref.sat << " ";
//std::cout << std::endl;
//std::cout << "obs 2 sats: ";
//for (auto&& obs_2_ref : obs_2.getObservations())
// std::cout << (int)obs_2_ref.sat << " ";
//std::cout << std::endl;
// iterate 1
for (auto&& obs_1_ref : obs_1.getObservations())
if (obs_2.hasSatellite(obs_1_ref.sat))
......@@ -102,4 +168,10 @@ void Observations::findCommonObservations(const Observations& obs_1, const Obser
common_obs_1.addObservation(obs_1_ref);
common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat));
}
//std::cout << "common sats: ";
//for (auto&& obs_2_ref : common_obs_1.getObservations())
// std::cout << (int)obs_2_ref.sat << " ";
//std::cout << std::endl;
}
......@@ -51,25 +51,19 @@ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const
common_obs_k, nav_k, common_sats_pos_k,
d, cov_d,
residual, discarded_sats,
sd_params, opt);
sd_params);
}
bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r, const std::map<int,Eigen::Vector3d>& common_sats_pos_r, const Eigen::Vector3d& x_r,
const Observations& common_obs_k, const Navigation& nav_k, const std::map<int,Eigen::Vector3d>& common_sats_pos_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
double& residual, std::set<int>& discarded_sats,
const SingleDifferencesParams& sd_params, const prcopt_t& opt)
const SingleDifferencesParams& sd_params)
{
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 (!discarded_sats.empty())
// {
// printf("SD: WARNING! excluded_sats list is not empty. It is an output parameter of the RAIM excluded sats. Clearing the list.");
// discarded_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);
......@@ -119,7 +113,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
row++;
}
// E5 (GAL)
if (NFREQ >= 3 and (sys&SYS_GAL) and
else 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)
{
......@@ -144,6 +138,10 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
#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;
std::cout << "common sats: ";
for (auto row_sat_freq_it : row_2_sat_freq)
std::cout << row_sat_freq_it.second.first << " ";
std::cout << std::endl;
#endif
// FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
......@@ -165,18 +163,35 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
// excluded satellite
if (discarded_sats.count(sat_number) != 0)
{
#if GNSS_UTILS_SD_DEBUG == 1
std::cout << "\tdiscarded sat" << std::endl;
#endif
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]);
drho_m(row) = (obs_k.L[freq] * nav_k.getNavigation().lam[sat_number-1][freq]) -
(obs_r.L[freq] * nav_r.getNavigation().lam[sat_number-1][freq]);
else
drho_m(row) = obs_r.P[freq] - obs_k.P[freq];
drho_m(row) = obs_k.P[freq] - obs_r.P[freq];
#if GNSS_UTILS_SD_DEBUG == 1
std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " << nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
//std::cout << "\tpositions:\n\t\tt_r: " << s_r.col(row).transpose() << "\n\t\tt_k: " << s_k.col(row).transpose() << std::endl;
//std::cout << "\tobs_r.L: " << obs_r.L[freq] << std::endl;
//std::cout << "\tobs_k.L: " << obs_k.L[freq] << std::endl;
//std::cout << "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
//std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " << nav_k.getNavigation().lam[sat_number-1][freq] << std::endl;
//std::cout << "\trho_r: " << obs_r.L[freq] * nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
//std::cout << "\trho_k: " << obs_k.L[freq] * nav_k.getNavigation().lam[sat_number-1][freq] << std::endl;
//std::cout << "\tdrho_m: " << drho_m(row) << std::endl;
#endif
if(!sd_params.relinearize_jacobian)
{
......@@ -231,6 +246,19 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
//std::cout << "residual = " << residual << std::endl;
//std::cout << "SD2: r-A*d = " << (r + A * d).transpose() << std::endl;
#if GNSS_UTILS_SD_DEBUG == 1
std::cout << "Displacement vector = %s" << d.head<3>().transpose() << std::endl;
printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d.head<3>().norm());
printf("SD: residual = %13.10f\n", residual);
printf("SD: row = %i\n", r.rows());
std::cout << "SD: drho = " << r.transpose() << std::endl;
std::cout << "SD: drho_m = " << drho_m.transpose() << std::endl;
std::cout << "SD: H = \n" << A << std::endl;
printf("SD: dT = %8.3f secs\n", d(3));
#endif
// RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
int input_discarded_sats = discarded_sats.size();
if(j==0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
......@@ -400,10 +428,16 @@ void filterCommonSatellites(Observations& common_obs_r, std::map<int,Eigen::Vect
std::set<int>& discarded_sats, const Eigen::Vector3d& x_r,
const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{
assert(&common_obs_r != &common_obs_k);
assert(&common_sats_pos_r != &common_sats_pos_k);
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());
//std::cout << "filterCommonSatellites: initial size: " << common_obs_k.size() << std::endl;
std::set<int> remove_sats;
for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
{
auto&& obs_r = common_obs_r.getObservationByIdx(obs_i);
......@@ -411,73 +445,86 @@ void filterCommonSatellites(Observations& common_obs_r, std::map<int,Eigen::Vect
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_sats.insert(sat_number);
continue;
}
// 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))
{
// std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.L[0] << " " << obs_k.L[0] << std::endl;
remove_sats.insert(sat_number);
continue;
}
if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
{
// std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.P[0] << " " << obs_k.P[0] << std::endl;
remove_sats.insert(sat_number);
continue;
}
// bad satellite position (satellite is not included in the discarded list)
if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or
common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3))
{
// std::cout << "Discarding sat " << sat_number << ": bad satellite position: \n\t" << common_sats_pos_r[sat_number].transpose() << "\n\t" << common_sats_pos_k[sat_number].transpose() << std::endl;
remove_sats.insert(sat_number);
continue;
}
// constellation
int sys=satsys(obs_r.sat,NULL);
if (!(sys & opt.navsys))
{
// std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " << opt.navsys << std::endl;
discarded_sats.insert(sat_number);
remove_sats.insert(sat_number);
continue;
}
// 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)
{
// std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << opt.elmin << std::endl;
discarded_sats.insert(sat_number);
remove_sats.insert(sat_number);
continue;
}
// remove from vectors
if (discard)
// snr TODO: multifrequency (2nd param and 3rd idx)
if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
{
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);
// std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
discarded_sats.insert(sat_number);
remove_sats.insert(sat_number);
continue;
}
}
// remove sats
//std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
for (auto sat : remove_sats)
{
assert(common_obs_r.hasSatellite(sat));
assert(common_obs_k.hasSatellite(sat));
assert(common_sats_pos_r.count(sat));
assert(common_sats_pos_k.count(sat));
common_obs_r.removeObservationBySat(sat);
common_obs_k.removeObservationBySat(sat);
common_sats_pos_r.erase(sat);
common_sats_pos_k.erase(sat);
}
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());
//std::cout << "final size: " << common_obs_k.size() << std::endl;
}
}
......
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