Skip to content
Snippets Groups Projects
Commit 626cddbc authored by Pep Martí Saumell's avatar Pep Martí Saumell
Browse files

[files] wip

parent 486f3878
No related branches found
No related tags found
3 merge requests!20new tag,!19new tag,!8Resolve "File restructuration"
This commit is part of merge request !8. Comments created here will be created in the context of that merge request.
......@@ -49,11 +49,12 @@ SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src)
# driver source files
SET(SOURCES
src/gnss_utils.cpp
src/utils/gnss_utils.cpp
src/utils/utils.cpp
src/utils/transformations.cpp
src/observations.cpp
src/navigation.cpp
src/TDCP.cpp
src/utils.cpp
src/ublox_raw.cpp)
SET(RTKLIB_SRC
......@@ -88,8 +89,9 @@ SET(RTKLIB_SRC
# application header files
SET(HEADERS
include/gnss_utils/gnss_utils.h
include/gnss_utils/utils.h
include/gnss_utils/utils/gnss_utils.h
include/gnss_utils/utils/utils.h
include/gnss_utils/utils/transformations.h
include/gnss_utils/observations.h
include/gnss_utils/navigation.h
include/gnss_utils/TDCP.h
......
......@@ -11,7 +11,7 @@
#define GNSS_UTILS_TDCP_DEBUG 0
#include <set>
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/gnss_utils.h"
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
......
......@@ -7,7 +7,7 @@
#include <memory>
#include <cassert>
#include "gnss_utils/utils.h"
#include "gnss_utils/utils/utils.h"
extern "C" {
#include "rtklib.h"
......
#ifndef UBLOX_RAW_H
#define UBLOX_RAW_H
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/gnss_utils.h"
namespace GNSSUtils
{
......
......@@ -12,6 +12,7 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
#include "gnss_utils/utils/transformations.h"
extern "C" {
#include "rtklib.h"
......@@ -62,18 +63,6 @@ int estposOwn(const obsd_t* obs,
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,
......
/*
* TDCP.h
*
* Created on: April 3, 2020
* Author: Pep Martí-Saumell
*/
#ifndef INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
#define INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
// #include <vector>
// #include <iostream>
// #include <memory>
// #include <string>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Sparse>
extern "C" {
#include "rtklib.h"
}
namespace GNSSUtils
{
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);
} // namespace GNSSUtils
#endif // INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
\ No newline at end of file
File moved
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/gnss_utils.h"
#include <typeinfo>
......
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/gnss_utils.h"
#include "gnss_utils/observations.h"
using namespace GNSSUtils;
......
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/gnss_utils.h"
namespace GNSSUtils
{
......@@ -19,38 +19,35 @@ ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
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 << "computePos: error in computing positioning, message: " << msg << "\n";
}
output.time = sol.time.time;
output.sec = sol.time.sec;
output.pos = Eigen::Vector3d(sol.rr);
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[4], sol.qr[2];
//std::cout << "Compute pos: " << output.pos.transpose() << "\n";
//std::cout << "Covariance:\n" << output.pos_covar << "\n";
// 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;
output.pos_stat = pntpos(
_observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
if (output.pos_stat == 0)
{
std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
}
output.time = sol.time.time;
output.sec = sol.time.sec;
output.pos = Eigen::Vector3d(sol.rr);
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[4], sol.qr[2];
// std::cout << "Compute pos: " << output.pos.transpose() << "\n";
// std::cout << "Covariance:\n" << output.pos_covar << "\n";
// 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;
}
// ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations,
......@@ -256,150 +253,6 @@ int estposOwn(const obsd_t* obs,
return 0;
}
Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
{
Eigen::Vector3d pos;
ecef2pos(_ecef.data(), pos.data());
return pos;
}
Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos)
{
Eigen::Vector3d ecef;
pos2ecef(_pos.data(), ecef.data());
return 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 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;
}
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;
}
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));
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;
Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
}
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
{
// ecef 2 geodetic
......
#include "gnss_utils/utils/transformations.h"
using namespace GNSSUtils;
Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
{
Eigen::Vector3d pos;
ecef2pos(_ecef.data(), pos.data());
return pos;
}
Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos)
{
Eigen::Vector3d ecef;
pos2ecef(_pos.data(), ecef.data());
return 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 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;
}
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;
}
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));
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;
Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
}
\ No newline at end of file
#include "gnss_utils/utils.h"
#include "gnss_utils/utils/utils.h"
namespace GNSSUtils
{
......
#include "gtest/utils_gtest.h"
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/gnss_utils.h"
TEST(NavigationTests, Whatever)
{
......
#include "gtest/utils_gtest.h"
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/gnss_utils.h"
#include "gnss_utils/observations.h"
using namespace GNSSUtils;
......
#include "gtest/utils_gtest.h"
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/gnss_utils.h"
// Geodetic system parameters
static double kSemimajorAxis = 6378137;
......
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