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

Merge branch '15-tdcp-batch-implementation' into 'devel' 

Resolve "TDCP batch implementation"

Closes #15

See merge request mobile_robotics/gauss_project/gnss_utils!17
parents 37a4c4a8 5b7b8bb0
No related branches found
No related tags found
3 merge requests!20new tag,!19new tag,!17Resolve "TDCP batch implementation"
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 805 additions and 349 deletions
include/gnss_utils/gnss_utils.h
+ 2
− 2
View file @ 74dd8ebb
......@@ -118,7 +118,7 @@ struct Options
int tropopt; // troposphere option: TROPOPT_OFF(0):correction off, TROPOPT_SAAS(1):Saastamoinen model, TROPOPT_SBAS(2):SBAS model, TROPOPT_EST(3):troposphere option: ZTD estimation, TROPOPT_ESTG(4):ZTD+grad estimation, TROPOPT_ZTD(5):ZTD correction,6:ZTD+grad correction
int sbascorr; // SBAS correction options (can be added): SBSOPT_LCORR(1): long term correction, SBSOPT_FCORR(2): fast correction, SBSOPT_ICORR(4): ionosphere correction, SBSOPT_RANGE(8): ranging
int raim; // RAIM removed sats
double elmin; // min elevation (degrees)
double elmin; // min elevation (rad)
double maxgdop; // maxgdop: reject threshold of gdop
bool GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used
TdcpOptions tdcp; // TDCP options
......@@ -173,7 +173,7 @@ class Range;
// Typedefs
typedef std::map<int,Satellite> Satellites;
typedef std::map<int,Range> Ranges;
typedef std::map<int,Eigen::Vector2d> Azels;
typedef std::map<int,Eigen::Vector2d> Azels; // Azimuth and elevation (rad)
// pointer typedefs
typedef std::shared_ptr<Observations> ObservationsPtr;
......
include/gnss_utils/observations.h
+ 4
− 9
View file @ 74dd8ebb
......@@ -42,15 +42,10 @@ public:
std::set<int> filterByCode();
std::set<int> filterByCarrierPhase();
std::set<int> filterByConstellations(const int& navsys);
std::set<int> filterByElevationSnr(const Eigen::Vector3d& x_r,
const Satellites& sats,
const snrmask_t& snrmask,
const double& elmin,
const bool& multi_freq);
std::set<int> filterByElevationSnr(const Azels& azels,
const snrmask_t& snrmask,
const double& elmin,
const bool& multi_freq);
std::set<int> filterByElevationSnr(const Azels& azels,
const snrmask_t& snrmask,
const double& elmin,
const bool& multi_freq);
std::set<int> filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
......
include/gnss_utils/range.h
+ 6
− 0
View file @ 74dd8ebb
......@@ -18,6 +18,7 @@ namespace GnssUtils
class Range
{
public:
int sys = SYS_GPS;
int sat = 0;
double P = -1;
double P_var = 1;
......@@ -28,6 +29,9 @@ class Range
double L = -1;
double L_corrected = -1;
double L_var = 1;
double L2 = -1;
double L2_corrected = -1;
double L2_var = 1;
public:
Range();
......@@ -39,6 +43,8 @@ class Range
const Azels& azel,
const Eigen::Vector3d& latlonalt,
const Options& opt);
static std::set<int> findCommonSatellites(const Ranges& ranges_1, const Ranges& ranges_2);
};
} /* namespace GnssUtils */
......
include/gnss_utils/snapshot.h
+ 37
− 10
View file @ 74dd8ebb
......@@ -27,16 +27,15 @@ public:
// Public methods
void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
double getGPST() const;
ObservationsPtr getObservations() const;
// Navigation
NavigationPtr getNavigation() const;
const Satellites& getSatellites() const;
Satellites& getSatellites();
void setObservations(ObservationsPtr obs);
void setNavigation(NavigationPtr nav);
void computeSatellites(const int& eph_opt); // see rtklib.h L396);
bool satellitesComputed() const;
// Observations
ObservationsPtr getObservations() const;
void setObservations(ObservationsPtr obs);
std::set<int> filterObservations(const std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
......@@ -49,14 +48,21 @@ public:
const bool& check_carrier_phase,
const Options& opt);
// Pseudo-ranges
// Satellites
const Satellites& getSatellites() const;
Satellites& getSatellites();
void computeSatellites(const int& eph_opt); // see rtklib.h L396);
bool satellitesComputed() const;
// Ranges
Ranges& getRanges();
const Ranges& getRanges() const;
bool rangesComputed() const;
void computeRanges(const Azels& azel,
const Eigen::Vector3d& latlonalt,
const Options& opt);
void computeRanges(const Eigen::Vector3d& x_ecef,
const Options& opt);
void print();
private:
......@@ -81,6 +87,11 @@ private:
namespace GnssUtils
{
inline double Snapshot::getGPST() const
{
return obs_->getObservations()[0].time.time + obs_->getObservations()[0].time.sec;
}
inline GnssUtils::ObservationsPtr Snapshot::getObservations() const
{
return obs_;
......@@ -122,7 +133,15 @@ inline std::set<int> Snapshot::filterObservations(const std::set<int>& disc
const bool& check_carrier_phase,
const Options& opt)
{
return obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt);
std::set<int> filtered_sats = obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt);
for (auto sat : filtered_sats)
{
sats_.erase(sat);
ranges_.erase(sat);
}
return filtered_sats;
}
inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarded_sats,
......@@ -131,7 +150,15 @@ inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarde
const bool& check_carrier_phase,
const Options& opt)
{
return obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt);
std::set<int> filtered_sats = obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt);
for (auto sat : filtered_sats)
{
sats_.erase(sat);
ranges_.erase(sat);
}
return filtered_sats;
}
inline const Ranges& Snapshot::getRanges() const
......
include/gnss_utils/tdcp.h
+ 44
− 31
View file @ 74dd8ebb
......@@ -14,40 +14,53 @@ struct TdcpBatchParams
int raim_n;
double raim_min_residual;
bool relinearize_jacobian;
bool old_nav;
int max_iterations;
};
bool Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
const std::set<int>& discarded_sats,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt);
bool Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
const std::set<int>& discarded_sats,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt);
bool Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const std::set<int> common_sats,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int> raim_discarded_sats,
const TdcpBatchParams& tdcp_params);
struct TdcpOutput
{
bool success = false;
std::string msg = "";
std::set<int> raim_discarded_sats;
std::set<int> used_sats;
Eigen::Vector4d d = Eigen::Vector4d::Zero();
Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero();
double dt = 0;
double residual = 0;
};
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt);
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt);
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const std::set<int>& common_sats,
const Eigen::Vector4d& d_0,
const TdcpBatchParams& tdcp_params);
TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::MatrixXd& A,
Eigen::VectorXd& r,
Eigen::VectorXd& drho_m,
Eigen::MatrixXd& s_k,
Eigen::MatrixXd& s_r,
const Eigen::Vector4d& d_0,
std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params);
} // namespace GnssUtils
#endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */
include/gnss_utils/utils/satellite.h
+ 6
− 2
View file @ 74dd8ebb
......@@ -19,12 +19,16 @@ namespace GnssUtils
class Observations;
class Navigation;
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
Satellites computeSatellites(const Observations& obs,
const Navigation& nav,
const int& eph_opt); // see rtklib.h L396
Eigen::Vector2d computeAzel(const Eigen::Vector3d& sat_ecef, const Eigen::Vector3d& receiver_ecef);
Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef);
Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef);
struct Satellite
{
int sys;
......
src/observations.cpp
+ 16
− 68
View file @ 74dd8ebb
......@@ -304,18 +304,19 @@ std::set<int> Observations::filterByConstellations(const int& navsys)
return remove_sats;
}
std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vector2d>& azels,
std::set<int> Observations::filterByElevationSnr(const Azels& azels,
const snrmask_t& snrmask,
const double& elmin,
const bool &multi_freq)
{
std::set<int> remove_sats;
for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
for (auto&& sat_idx_pair : sat_2_idx_)
{
auto&& obs_sat = getObservationByIdx(obs_i);
const int& sat_number = obs_sat.sat;
const int& sat_number = sat_idx_pair.first;
assert(azels.count(sat_number) && "azel missing for a satellite of this observation");
const double& elevation(azels.at(sat_number)(1));
auto&& obs_sat = getObservationByIdx(sat_idx_pair.second);
// check elevation
if (elevation < elmin)
......@@ -354,27 +355,6 @@ std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vecto
return remove_sats;
}
std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r,
const Satellites& sats,
const snrmask_t& snrmask,
const double& elmin,
const bool &multi_freq)
{
std::set<int> remove_sats;
std::map<int,Eigen::Vector2d> azels;
for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
{
auto&& obs_sat = getObservationByIdx(obs_i);
const int& sat_number = obs_sat.sat;
double elevation = computeSatElevation(x_r, sats.at(obs_sat.sat).pos);
azels.emplace(obs_sat.sat,Eigen::Vector2d(0.0,elevation));
}
return filterByElevationSnr(azels, snrmask, elmin, multi_freq);
}
std::set<int> Observations::filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
......@@ -403,51 +383,19 @@ std::set<int> Observations::filter(const Satellites& sats,
const double& elmin,
const bool& multi_freq)
{
//std::cout << "filter: initial size: " << obs_.size() << std::endl;
// Ephemeris
std::set<int> remove_sats = filterByEphemeris(sats);
// Discarded sats
std::set<int> remove_sats_discarded = filterBySatellites(discarded_sats);
remove_sats.insert(remove_sats_discarded.begin(), remove_sats_discarded.end());
assert(!x_r.isApprox(Eigen::Vector3d::Zero(), 1e-3));
// Code
if (check_code)
{
std::set<int> remove_sats_code = filterByCode();
remove_sats.insert(remove_sats_code.begin(), remove_sats_code.end());
}
Azels azels = computeAzels(sats, x_r);
// Carrier phase
if (check_carrier_phase)
{
std::set<int> remove_sats_carrier = filterByCarrierPhase();
remove_sats.insert(remove_sats_carrier.begin(), remove_sats_carrier.end());
}
// satellite flag and ephemeris variance
for (auto sat_pair : sats)
if (remove_sats.count(sat_pair.first) == 0)
if (satexclude(sat_pair.first,sat_pair.second.var, sat_pair.second.svh, NULL))
{
//std::cout << "Discarding sat " << sat_pair.first << ": unhealthy or huge variance svh = " << sat_pair.second.svh << std::endl;
removeObservationBySat(sat_pair.first);
remove_sats.insert(sat_pair.first);
}
// Constellations
std::set<int> remove_sats_constellations = filterByConstellations(navsys);
remove_sats.insert(remove_sats_constellations.begin(), remove_sats_constellations.end());
// Elevation and SNR
if (!x_r.isApprox(Eigen::Vector3d::Zero(), 1e-3))
{
std::set<int> remove_sats_elevation = filterByElevationSnr(x_r, sats, snrmask, elmin, multi_freq);
remove_sats.insert(remove_sats_elevation.begin(), remove_sats_elevation.end());
}
return remove_sats;
// std::cout << "final size: " << obs_.size() << std::endl;
return filter(sats,
discarded_sats,
azels,
check_code,
check_carrier_phase,
navsys,
snrmask,
elmin,
multi_freq);
}
std::set<int> Observations::filter(const Satellites& sats,
......
src/range.cpp
+ 49
− 0
View file @ 74dd8ebb
......@@ -103,9 +103,21 @@ Ranges Range::computeRanges(ObservationsPtr obs,
ranges[sat].P_var = varerr(&prcopt,azel_i[1],prcopt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp;
/* ------------------- Carrier phase ------------------- */
// L1
ranges[sat].L = obs_i.L[0]*nav->getNavigation().lam[sat-1][0];
ranges[sat].L_corrected = ranges[sat].L;
int sys = satsys(sat, NULL);
// L2 (GPS/GLO/QZS)
if (sys & (SYS_GPS | SYS_GLO | SYS_QZS))
ranges[sat].L2 = obs_i.L[1]*nav->getNavigation().lam[sat-1][1];
// E5 (GAL)
else if (sys & SYS_GAL)
ranges[sat].L2 = obs_i.L[2]*nav->getNavigation().lam[sat-1][2];
else
ranges[sat].L2 = 0;
ranges[sat].L2_corrected = ranges[sat].L2;
/* ionospheric corrections */
if (opt.tdcp.corr_iono)
{
......@@ -115,19 +127,29 @@ Ranges Range::computeRanges(ObservationsPtr obs,
//ranges[sat].L_corrected = obs_i.L[0]*nav->getNavigation().lam[sat-1][0];
}
else
{
ranges[sat].L_corrected -= ranges[sat].iono_corr;
ranges[sat].L2_corrected -= ranges[sat].iono_corr; //FIXME: different?
}
}
/* tropospheric corrections */
if (opt.tdcp.corr_tropo)
{
ranges[sat].L_corrected += ranges[sat].tropo_corr;
ranges[sat].L2_corrected += ranges[sat].tropo_corr; //FIXME: different?
}
/* sat clock corrections */
if (opt.tdcp.corr_clock)
{
ranges[sat].L_corrected += ranges[sat].sat_clock_corr;
ranges[sat].L2_corrected += ranges[sat].sat_clock_corr; //FIXME: different?
}
/* carrier phase variance */
ranges[sat].L_var = opt.tdcp.sigma_carrier * opt.tdcp.sigma_carrier;
ranges[sat].L2_var = opt.tdcp.sigma_carrier * opt.tdcp.sigma_carrier;
//std::cout << std::endl
// << "\t\tprange: " << pranges[sat].prange << std::endl
......@@ -141,4 +163,31 @@ Ranges Range::computeRanges(ObservationsPtr obs,
return ranges;
}
std::set<int> Range::findCommonSatellites(const Ranges& ranges_1, const Ranges& ranges_2)
{
std::set<int> common_sats;
// std::cout << "ranges_1: ";
// for (auto&& range_pair : ranges_1)
// std::cout << range_pair.first << " ";
// std::cout << std::endl;
// std::cout << "ranges_2: ";
// for (auto&& range_pair : ranges_2)
// std::cout << range_pair.first << " ";
// std::cout << std::endl;
for (auto&& range_pair : ranges_1)
if (ranges_2.count(range_pair.first))
common_sats.insert(range_pair.first);
// std::cout << "common sats: ";
// for (auto sat : common_sats)
// std::cout << sat << " ";
// std::cout << std::endl;
return common_sats;
}
} /* namespace GnssUtils */
src/snapshot.cpp
+ 10
− 0
View file @ 74dd8ebb
#include "gnss_utils/snapshot.h"
#include "gnss_utils/utils/satellite.h"
#include "gnss_utils/utils/transformations.h"
using namespace GnssUtils;
......@@ -41,3 +42,12 @@ void Snapshot::computeRanges(const Azels& azel,
ranges_ = Range::computeRanges(obs_, nav_, sats_, azel, latlonalt, opt);
}
void Snapshot::computeRanges(const Eigen::Vector3d& x_ecef,
const Options& opt)
{
ranges_ = Range::computeRanges(obs_, nav_, sats_,
computeAzels(sats_, x_ecef),
ecefToLatLonAlt(x_ecef),
opt);
}
src/tdcp.cpp
+ 198
− 184
View file @ 74dd8ebb
This diff is collapsed.
src/utils/satellite.cpp
+ 43
− 11
View file @ 74dd8ebb
......@@ -14,24 +14,56 @@
namespace GnssUtils
{
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
Eigen::Vector2d computeAzel(const Eigen::Vector3d& sat_ecef, const Eigen::Vector3d& receiver_ecef)
{
// ecef 2 geodetic
// receiver position (geo)
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 unit vector (ecef)
Eigen::Vector3d e_ecef = (sat_ecef - receiver_ecef).normalized();
// receiver-sat unit vector (enu)
Eigen::Vector3d e_enu;
ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
e_ecef.data(), // vector in ecef coordinate {x,y,z}
e_enu.data()); // vector in local tangental coordinate {e,n,u}
// 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}
Eigen::Vector2d azel;
// azimuth
azel(0) = atan2(e_enu(0), e_enu(1));
// elevation
return atan2(receiver_sat_enu(2),
sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
azel(1) = asin(e_enu(2));
return azel;
}
Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef)
{
return computeAzel(sat.pos, receiver_ecef);
/*// compute receiver-to-satellilte unit vector (ecef)
Eigen::Vector3d e;
geodist(sat.pos.data(), receiver_ecef.data(), e.data());
// compute receiver position in geodetic
Eigen::Vector3d receiver_geo;
ecef2pos(receiver_ecef.data(), receiver_geo.data());
// compute azimut and elevation
Eigen::Vector2d azel;
satazel(receiver_geo.data(), e.data(), azel.data());
return azel;*/
}
Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef)
{
Azels azels;
for (auto sat : sats)
azels.emplace(sat.first, computeAzel(sat.second, receiver_ecef));
return azels;
}
Satellites computeSatellites(const Observations& obs,
......
test/CMakeLists.txt
+ 10
− 6
View file @ 74dd8ebb
......@@ -12,14 +12,18 @@ include_directories(${GTEST_INCLUDE_DIRS})
# #
##############################################################
# Transformations test
gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp)
target_link_libraries(gtest_transformations ${PROJECT_NAME})
# Navigation test
gnss_utils_add_gtest(gtest_navigation gtest_navigation.cpp)
target_link_libraries(gtest_navigation libgtest ${PROJECT_NAME})
# Observations test
gnss_utils_add_gtest(gtest_observations gtest_observations.cpp)
target_link_libraries(gtest_observations libgtest ${PROJECT_NAME})
# Navigation test
gnss_utils_add_gtest(gtest_navigation gtest_navigation.cpp)
target_link_libraries(gtest_navigation libgtest ${PROJECT_NAME})
\ No newline at end of file
# TDCP test
gnss_utils_add_gtest(gtest_tdcp gtest_tdcp.cpp)
target_link_libraries(gtest_tdcp ${PROJECT_NAME})
# Transformations test
gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp)
target_link_libraries(gtest_transformations ${PROJECT_NAME})
\ No newline at end of file
test/gtest_tdcp.cpp 0 → 100644
+ 330
− 0
View file @ 74dd8ebb
#include "gtest/utils_gtest.h"
#include "gnss_utils/tdcp.h"
#include "gnss_utils/snapshot.h"
#include "gnss_utils/utils/satellite.h"
#include "gnss_utils/utils/transformations.h"
using namespace GnssUtils;
using namespace Eigen;
Vector3d computeRandomReceiverLatLonAlt()
{
Vector3d receiver_LLA = Vector3d::Random(); // in [-1, 1]
receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2]
receiver_LLA(1) *= M_PI; // in [-PI, PI]
receiver_LLA(2) += 1; // ([0, 2])
receiver_LLA(2) *= 5e2; // in [0, 1e3]
return receiver_LLA;
}
void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
Vector3d& sat_ENU,
Vector3d& sat_ECEF,
Vector2d& sat_azel,
double& range)
{
Vector3d t_ECEF_ENU, t_ENU_ECEF;
Matrix3d R_ENU_ECEF;
t_ECEF_ENU = latLonAltToEcef(receiver_latlonalt);
computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
// random elevation and azimuth
sat_azel = Vector2d::Random(); // in [-1, 1]
sat_azel(0) *= M_PI; // in [-pi, pi]
sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2]
range = VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500]
// ENU
sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range,
cos(sat_azel(0)) * cos(sat_azel(1)) * range,
sin(sat_azel(1)) * range;
// ECEF
sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
// Range
range = (sat_ECEF - t_ECEF_ENU).norm();
}
TEST(Tdcp, Tdcp)
{
int N_sats = 20;
TdcpBatchParams tdcp_params;
tdcp_params.max_iterations = 10;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 0;
tdcp_params.raim_min_residual = 0;
tdcp_params.relinearize_jacobian = true;
tdcp_params.tdcp.multi_freq = false;
Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
Matrix3d R_ENU_ECEF;
Vector2d azel, azel2;
Vector4d d_gt;
Matrix4d cov_d;
double range, residual;
// Snapshots
auto snapshot_r = std::make_shared<Snapshot>();
auto snapshot_k = std::make_shared<Snapshot>();
// Observations (just needed for the GPST)
snapshot_r->setObservations(std::make_shared<Observations>());
snapshot_k->setObservations(std::make_shared<Observations>());
obsd_t obs_r;
obs_r.time.sec = 0;
obs_r.time.time = 0;
snapshot_r->getObservations()->addObservation(obs_r);
obsd_t obs_k;
obs_k.time.sec = 0;
obs_k.time.time = 1;
snapshot_k->getObservations()->addObservation(obs_k);
// Random receiver position
for (auto i = 0; i<100; i++)
{
// clear satellites and ranges
snapshot_r->getSatellites().clear();
snapshot_k->getSatellites().clear();
snapshot_r->getRanges().clear();
snapshot_k->getRanges().clear();
// random setup
x_r_LLA = computeRandomReceiverLatLonAlt();
x_r_ECEF = latLonAltToEcef(x_r_LLA);
d_ECEF = Vector3d::Random() * 10;
x_k_ECEF = x_r_ECEF + d_ECEF;
x_k_LLA = ecefToLatLonAlt(x_k_ECEF);
double clock_r = Vector2d::Random()(0) * 1e-6;
double clock_k = Vector2d::Random()(0) * 1e-6;
// groundtruth
d_gt.head<3>() = d_ECEF;
d_gt(3) = (clock_k - clock_r) * CLIGHT;
std::cout << "Iteration " << i << ":\n";
std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
std::cout << "\tclock_r " << clock_r << ":\n";
std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
std::cout << "\tclock_k " << clock_k << ":\n";
std::cout << "\td_gt " << d_gt.transpose() << ":\n";
std::set<int> common_sats;
std::set<int> raim_discarded_sats;
// random visible satellites
for (auto j = 0; j<N_sats; j++)
{
common_sats.insert(j);
// Satellite r (random)
computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
snapshot_r->getSatellites().emplace(j, sat_r);
// Range r
Range range_r;
range_r.sat = j;
range_r.L_corrected = range + CLIGHT * clock_r;
range_r.L_var = 1;
range_r.L2_corrected = range_r.L_corrected;
range_r.L2_var = range_r.L_var;
snapshot_r->getRanges().emplace(j, range_r);
std::cout << "\tsat: " << j << "\n";
std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
// Satellite k (random)
computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
snapshot_k->getSatellites().emplace(j, sat_k);
// Range k
Range range_k;
range_k.sat = j;
range_k.L_corrected = range + CLIGHT * clock_k;
range_k.L_var = 1;
range_k.L2_corrected = range_k.L_corrected;
range_k.L2_var = range_k.L_var;
snapshot_k->getRanges().emplace(j, range_k);
std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
}
// TDCP
auto tdcp_out = Tdcp(snapshot_r,
snapshot_k,
x_r_ECEF,
common_sats,
Vector4d::Zero(),
tdcp_params);
// CHECKS
std::cout << "CHECKS iteration " << i << std::endl;
ASSERT_TRUE(tdcp_out.success);
ASSERT_LE(tdcp_out.residual, 1e-9);
ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
}
}
TEST(Tdcp, Tdcp_raim)
{
int N_sats = 20;
TdcpBatchParams tdcp_params;
tdcp_params.max_iterations = 10;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 2;
tdcp_params.raim_min_residual = 0;
tdcp_params.relinearize_jacobian = true;
tdcp_params.tdcp.multi_freq = false;
Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
Matrix3d R_ENU_ECEF;
Vector2d azel, azel2;
Vector4d d, d_gt;
Matrix4d cov_d;
double range, residual;
// Snapshots
auto snapshot_r = std::make_shared<Snapshot>();
auto snapshot_k = std::make_shared<Snapshot>();
// Observations (just needed for the GPST)
snapshot_r->setObservations(std::make_shared<Observations>());
snapshot_k->setObservations(std::make_shared<Observations>());
obsd_t obs_r;
obs_r.time.sec = 0;
obs_r.time.time = 0;
snapshot_r->getObservations()->addObservation(obs_r);
obsd_t obs_k;
obs_k.time.sec = 0;
obs_k.time.time = 1;
snapshot_k->getObservations()->addObservation(obs_k);
// Random receiver position
for (auto i = 0; i<100; i++)
{
// clear satellites and ranges
snapshot_r->getSatellites().clear();
snapshot_k->getSatellites().clear();
snapshot_r->getRanges().clear();
snapshot_k->getRanges().clear();
// random setup
x_r_LLA = computeRandomReceiverLatLonAlt();
x_r_ECEF = latLonAltToEcef(x_r_LLA);
d_ECEF = Vector3d::Random() * 10;
x_k_ECEF = x_r_ECEF + d_ECEF;
x_k_LLA = ecefToLatLonAlt(x_k_ECEF);
double clock_r = Vector2d::Random()(0) * 1e-6;
double clock_k = Vector2d::Random()(0) * 1e-6;
// groundtruth
d_gt.head<3>() = d_ECEF;
d_gt(3) = (clock_k - clock_r) * CLIGHT;
std::cout << "Iteration " << i << ":\n";
std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
std::cout << "\tclock_r " << clock_r << ":\n";
std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
std::cout << "\tclock_k " << clock_k << ":\n";
std::cout << "\td_gt " << d_gt.transpose() << ":\n";
std::set<int> common_sats;
std::set<int> raim_discarded_sats;
// random visible satellites
for (auto j = 0; j<N_sats; j++)
{
common_sats.insert(j);
// Satellite r (random)
computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
snapshot_r->getSatellites().emplace(j, sat_r);
// Range r
Range range_r;
range_r.sat = j;
range_r.L_corrected = range + CLIGHT * clock_r;
range_r.L_var = 1;
range_r.L2_corrected = range_r.L_corrected;
range_r.L2_var = range_r.L_var;
snapshot_r->getRanges().emplace(j, range_r);
std::cout << "\tsat: " << j << "\n";
std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
// Satellite k (random)
computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
snapshot_k->getSatellites().emplace(j, sat_k);
// Range k
Range range_k;
range_k.sat = j;
range_k.L_corrected = range + CLIGHT * clock_k;
range_k.L_var = 1;
range_k.L2_corrected = range_k.L_corrected;
range_k.L2_var = range_k.L_var;
snapshot_k->getRanges().emplace(j, range_k);
std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
}
// Distort 2 ranges -> to be detected by RAIM
int wrong_sat1 = i % N_sats;
snapshot_k->getRanges().at(wrong_sat1).L_corrected += 20;
int wrong_sat2 = (i + N_sats / 2) % N_sats;
snapshot_r->getRanges().at(wrong_sat2).L_corrected -= 10;
// TDCP
auto tdcp_out = Tdcp(snapshot_r,
snapshot_k,
x_r_ECEF,
common_sats,
Vector4d::Zero(),
tdcp_params);
// CHECKS
std::cout << "CHECKS iteration " << i << std::endl;
ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
ASSERT_TRUE(tdcp_out.success);
ASSERT_LE(tdcp_out.residual, 1e-9);
ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
test/gtest_transformations.cpp
+ 50
− 26
View file @ 74dd8ebb
......@@ -10,6 +10,46 @@ static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
using namespace GnssUtils;
Eigen::Vector3d computeRandomReceiverLatLonAlt()
{
Eigen::Vector3d receiver_LLA = Eigen::Vector3d::Random(); // in [-1, 1]
receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2]
receiver_LLA(1) *= M_PI; // in [-PI, PI]
receiver_LLA(2) += 1; // ([0, 2])
receiver_LLA(2) *= 5e2; // in [0, 1e3]
return receiver_LLA;
}
void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt,
Eigen::Vector3d& sat_ENU,
Eigen::Vector3d& sat_ECEF,
Eigen::Vector2d& sat_azel,
double range)
{
Eigen::Vector3d t_ECEF_ENU, t_ENU_ECEF;
Eigen::Matrix3d R_ENU_ECEF;
t_ECEF_ENU = latLonAltToEcef(receiver_latlonalt);
computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
// random elevation and azimuth
sat_azel = Eigen::Vector2d::Random(); // in [-1, 1]
sat_azel(0) *= M_PI; // in [-pi, pi]
sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2]
range = Eigen::VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500]
// ENU
sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range,
cos(sat_azel(0)) * cos(sat_azel(1)) * range,
sin(sat_azel(1)) * range;
// ECEF
sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
}
TEST(TransformationsTest, ecefToLatLonAlt)
{
Eigen::Vector3d ecef, latlonalt;
......@@ -78,10 +118,7 @@ TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt)
// latlon -> ecef -> latlon
for (auto i = 0; i<100; i++)
{
latlonalt0 = Eigen::Vector3d::Random();
latlonalt0(0) *= M_PI / 2;
latlonalt0(1) *= M_PI;
latlonalt0(2) *= 1e3;
latlonalt0 = computeRandomReceiverLatLonAlt();
ecef0 = latLonAltToEcef(latlonalt0);
latlonalt1 = ecefToLatLonAlt(ecef0);
......@@ -98,10 +135,7 @@ TEST(TransformationsTest, computeEnuEcef)
// random
for (auto i = 0; i<100; i++)
{
t_ENU_latlonalt = Eigen::Vector3d::Random();
t_ENU_latlonalt(0) *= M_PI / 2;
t_ENU_latlonalt(1) *= M_PI;
t_ENU_latlonalt(2) *= 1e3;
t_ENU_latlonalt = computeRandomReceiverLatLonAlt();
t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
......@@ -159,37 +193,27 @@ TEST(TransformationsTest, computeEnuEcef)
}
}
TEST(TransformationsTest, computeSatElevation)
TEST(TransformationsTest, computeAzel)
{
Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt,rec_sat_ENU, sat_ECEF;
Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt, sat_ENU, sat_ECEF;
Eigen::Matrix3d R_ENU_ECEF;
Eigen::Vector2d azel, azel2;
double range;
// random receiver position
for (auto i = 0; i<100; i++)
{
t_ENU_latlonalt = Eigen::Vector3d::Random();
t_ENU_latlonalt(0) *= M_PI / 2;
t_ENU_latlonalt(1) *= M_PI;
t_ENU_latlonalt(2) *= 1e3;
t_ENU_latlonalt = computeRandomReceiverLatLonAlt();
t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
// random elevation and heading
for (auto j = 0; j<100; j++)
{
Eigen::Vector2d rand2 = Eigen::Vector2d::Random();
double heading = rand2(0) * M_PI;
double elevation = rand2(0) * M_PI / 2;
rec_sat_ENU << cos(heading) * 1000, sin(heading) * 1000, tan(elevation) * 1000;
sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * rec_sat_ENU;
computeRandomVisibleSatellite(t_ENU_latlonalt, sat_ENU, sat_ECEF, azel, range);
double elevation2 = computeSatElevation(t_ECEF_ENU, sat_ECEF);
azel2 = computeAzel(sat_ECEF, t_ECEF_ENU);
ASSERT_NEAR(elevation, elevation2,1e-6);
ASSERT_MATRIX_APPROX(azel,azel2,1e-6);
}
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment