mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-01-07 07:50:32 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
Carles Fernandez
commit
3970f6b315
@ -31,6 +31,7 @@
|
|||||||
|
|
||||||
#include "ls_pvt.h"
|
#include "ls_pvt.h"
|
||||||
#include "GPS_L1_CA.h"
|
#include "GPS_L1_CA.h"
|
||||||
|
#include "geofunctions.h"
|
||||||
#include <glog/logging.h>
|
#include <glog/logging.h>
|
||||||
#include <exception>
|
#include <exception>
|
||||||
#include <stdexcept>
|
#include <stdexcept>
|
||||||
@ -235,12 +236,12 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
|
|||||||
double* azim = 0;
|
double* azim = 0;
|
||||||
double* elev = 0;
|
double* elev = 0;
|
||||||
double* dist = 0;
|
double* dist = 0;
|
||||||
Ls_Pvt::topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
|
topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
|
||||||
|
|
||||||
if (traveltime < 0.1 && nmbOfSatellites > 3)
|
if (traveltime < 0.1 && nmbOfSatellites > 3)
|
||||||
{
|
{
|
||||||
//--- Find receiver's height
|
//--- Find receiver's height
|
||||||
Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
|
togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
|
||||||
// Add troposphere correction if the receiver is below the troposphere
|
// Add troposphere correction if the receiver is below the troposphere
|
||||||
if (h > 15000)
|
if (h > 15000)
|
||||||
{
|
{
|
||||||
|
|||||||
@ -31,6 +31,7 @@
|
|||||||
|
|
||||||
#include "pvt_solution.h"
|
#include "pvt_solution.h"
|
||||||
#include "GPS_L1_CA.h"
|
#include "GPS_L1_CA.h"
|
||||||
|
#include "geofunctions.h"
|
||||||
#include <glog/logging.h>
|
#include <glog/logging.h>
|
||||||
#include <exception>
|
#include <exception>
|
||||||
|
|
||||||
@ -133,125 +134,6 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
|
|
||||||
{
|
|
||||||
/* Subroutine to calculate geodetic coordinates latitude, longitude,
|
|
||||||
height given Cartesian coordinates X,Y,Z, and reference ellipsoid
|
|
||||||
values semi-major axis (a) and the inverse of flattening (finv).
|
|
||||||
|
|
||||||
The output units of angular quantities will be in decimal degrees
|
|
||||||
(15.5 degrees not 15 deg 30 min). The output units of h will be the
|
|
||||||
same as the units of X,Y,Z,a.
|
|
||||||
|
|
||||||
Inputs:
|
|
||||||
a - semi-major axis of the reference ellipsoid
|
|
||||||
finv - inverse of flattening of the reference ellipsoid
|
|
||||||
X,Y,Z - Cartesian coordinates
|
|
||||||
|
|
||||||
Outputs:
|
|
||||||
dphi - latitude
|
|
||||||
dlambda - longitude
|
|
||||||
h - height above reference ellipsoid
|
|
||||||
|
|
||||||
Based in a Matlab function by Kai Borre
|
|
||||||
*/
|
|
||||||
|
|
||||||
*h = 0;
|
|
||||||
double tolsq = 1.e-10; // tolerance to accept convergence
|
|
||||||
int maxit = 10; // max number of iterations
|
|
||||||
double rtd = 180.0 / GPS_PI;
|
|
||||||
|
|
||||||
// compute square of eccentricity
|
|
||||||
double esq;
|
|
||||||
if (finv < 1.0E-20)
|
|
||||||
{
|
|
||||||
esq = 0.0;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
esq = (2.0 - 1.0 / finv) / finv;
|
|
||||||
}
|
|
||||||
|
|
||||||
// first guess
|
|
||||||
double P = sqrt(X * X + Y * Y); // P is distance from spin axis
|
|
||||||
|
|
||||||
//direct calculation of longitude
|
|
||||||
if (P > 1.0E-20)
|
|
||||||
{
|
|
||||||
*dlambda = atan2(Y, X) * rtd;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
*dlambda = 0.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// correct longitude bound
|
|
||||||
if (*dlambda < 0)
|
|
||||||
{
|
|
||||||
*dlambda = *dlambda + 360.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0)
|
|
||||||
|
|
||||||
double sinphi;
|
|
||||||
if (r > 1.0E-20)
|
|
||||||
{
|
|
||||||
sinphi = Z / r;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
sinphi = 0.0;
|
|
||||||
}
|
|
||||||
*dphi = asin(sinphi);
|
|
||||||
|
|
||||||
// initial value of height = distance from origin minus
|
|
||||||
// approximate distance from origin to surface of ellipsoid
|
|
||||||
if (r < 1.0E-20)
|
|
||||||
{
|
|
||||||
*h = 0;
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
*h = r - a * (1 - sinphi * sinphi / finv);
|
|
||||||
|
|
||||||
// iterate
|
|
||||||
double cosphi;
|
|
||||||
double N_phi;
|
|
||||||
double dP;
|
|
||||||
double dZ;
|
|
||||||
double oneesq = 1.0 - esq;
|
|
||||||
|
|
||||||
for (int i = 0; i < maxit; i++)
|
|
||||||
{
|
|
||||||
sinphi = sin(*dphi);
|
|
||||||
cosphi = cos(*dphi);
|
|
||||||
|
|
||||||
// compute radius of curvature in prime vertical direction
|
|
||||||
N_phi = a / sqrt(1 - esq * sinphi * sinphi);
|
|
||||||
|
|
||||||
// compute residuals in P and Z
|
|
||||||
dP = P - (N_phi + (*h)) * cosphi;
|
|
||||||
dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
|
|
||||||
|
|
||||||
// update height and latitude
|
|
||||||
*h = *h + (sinphi * dZ + cosphi * dP);
|
|
||||||
*dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
|
|
||||||
|
|
||||||
// test for convergence
|
|
||||||
if ((dP * dP + dZ * dZ) < tolsq)
|
|
||||||
{
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
if (i == (maxit - 1))
|
|
||||||
{
|
|
||||||
LOG(WARNING) << "The computation of geodetic coordinates did not converge";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
*dphi = (*dphi) * rtd;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km)
|
int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km)
|
||||||
{
|
{
|
||||||
/* Inputs:
|
/* Inputs:
|
||||||
@ -359,73 +241,6 @@ int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx)
|
|
||||||
{
|
|
||||||
/* Transformation of vector dx into topocentric coordinate
|
|
||||||
system with origin at x
|
|
||||||
Inputs:
|
|
||||||
x - vector origin coordinates (in ECEF system [X; Y; Z;])
|
|
||||||
dx - vector ([dX; dY; dZ;]).
|
|
||||||
|
|
||||||
Outputs:
|
|
||||||
D - vector length. Units like the input
|
|
||||||
Az - azimuth from north positive clockwise, degrees
|
|
||||||
El - elevation angle, degrees
|
|
||||||
|
|
||||||
Based on a Matlab function by Kai Borre
|
|
||||||
*/
|
|
||||||
|
|
||||||
double lambda;
|
|
||||||
double phi;
|
|
||||||
double h;
|
|
||||||
double dtr = GPS_PI / 180.0;
|
|
||||||
double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
|
|
||||||
double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
|
|
||||||
|
|
||||||
// Transform x into geodetic coordinates
|
|
||||||
Pvt_Solution::togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
|
|
||||||
|
|
||||||
double cl = cos(lambda * dtr);
|
|
||||||
double sl = sin(lambda * dtr);
|
|
||||||
double cb = cos(phi * dtr);
|
|
||||||
double sb = sin(phi * dtr);
|
|
||||||
|
|
||||||
arma::mat F = {{-sl, -sb * cl, cb * cl},
|
|
||||||
{cl, -sb * sl, cb * sl},
|
|
||||||
{0, 0, cb, sb}};
|
|
||||||
|
|
||||||
arma::vec local_vector;
|
|
||||||
|
|
||||||
local_vector = arma::htrans(F) * dx;
|
|
||||||
|
|
||||||
double E = local_vector(0);
|
|
||||||
double N = local_vector(1);
|
|
||||||
double U = local_vector(2);
|
|
||||||
|
|
||||||
double hor_dis;
|
|
||||||
hor_dis = sqrt(E * E + N * N);
|
|
||||||
|
|
||||||
if (hor_dis < 1.0E-20)
|
|
||||||
{
|
|
||||||
*Az = 0;
|
|
||||||
*El = 90;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
*Az = atan2(E, N) / dtr;
|
|
||||||
*El = atan2(U, hor_dis) / dtr;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (*Az < 0)
|
|
||||||
{
|
|
||||||
*Az = *Az + 360.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
*D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void Pvt_Solution::set_averaging_depth(int depth)
|
void Pvt_Solution::set_averaging_depth(int depth)
|
||||||
{
|
{
|
||||||
d_averaging_depth = depth;
|
d_averaging_depth = depth;
|
||||||
@ -519,26 +334,31 @@ double Pvt_Solution::get_height() const
|
|||||||
return d_height_m;
|
return d_height_m;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
double Pvt_Solution::get_speed_over_ground() const
|
double Pvt_Solution::get_speed_over_ground() const
|
||||||
{
|
{
|
||||||
return d_speed_over_ground_m_s;
|
return d_speed_over_ground_m_s;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Pvt_Solution::set_speed_over_ground(double speed_m_s)
|
void Pvt_Solution::set_speed_over_ground(double speed_m_s)
|
||||||
{
|
{
|
||||||
d_speed_over_ground_m_s = speed_m_s;
|
d_speed_over_ground_m_s = speed_m_s;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Pvt_Solution::set_course_over_ground(double cog_deg)
|
void Pvt_Solution::set_course_over_ground(double cog_deg)
|
||||||
{
|
{
|
||||||
d_course_over_ground_d = cog_deg;
|
d_course_over_ground_d = cog_deg;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
double Pvt_Solution::get_course_over_ground() const
|
double Pvt_Solution::get_course_over_ground() const
|
||||||
{
|
{
|
||||||
return d_course_over_ground_d;
|
return d_course_over_ground_d;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
double Pvt_Solution::get_avg_latitude() const
|
double Pvt_Solution::get_avg_latitude() const
|
||||||
{
|
{
|
||||||
return d_avg_latitude_d;
|
return d_avg_latitude_d;
|
||||||
|
|||||||
@ -129,41 +129,6 @@ public:
|
|||||||
*/
|
*/
|
||||||
int cart2geo(double X, double Y, double Z, int elipsoid_selection);
|
int cart2geo(double X, double Y, double Z, int elipsoid_selection);
|
||||||
|
|
||||||
/*!
|
|
||||||
* \brief Transformation of vector dx into topocentric coordinate system with origin at x
|
|
||||||
*
|
|
||||||
* \param[in] x Vector origin coordinates (in ECEF system [X; Y; Z;])
|
|
||||||
* \param[in] dx Vector ([dX; dY; dZ;]).
|
|
||||||
*
|
|
||||||
* \param[out] D Vector length. Units like the input
|
|
||||||
* \param[out] Az Azimuth from north positive clockwise, degrees
|
|
||||||
* \param[out] El Elevation angle, degrees
|
|
||||||
*
|
|
||||||
* Based on a Matlab function by Kai Borre
|
|
||||||
*/
|
|
||||||
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx);
|
|
||||||
|
|
||||||
/*!
|
|
||||||
* \brief Subroutine to calculate geodetic coordinates latitude, longitude,
|
|
||||||
* height given Cartesian coordinates X,Y,Z, and reference ellipsoid
|
|
||||||
* values semi-major axis (a) and the inverse of flattening (finv).
|
|
||||||
*
|
|
||||||
* The output units of angular quantities will be in decimal degrees
|
|
||||||
* (15.5 degrees not 15 deg 30 min). The output units of h will be the
|
|
||||||
* same as the units of X,Y,Z,a.
|
|
||||||
*
|
|
||||||
* \param[in] a - semi-major axis of the reference ellipsoid
|
|
||||||
* \param[in] finv - inverse of flattening of the reference ellipsoid
|
|
||||||
* \param[in] X,Y,Z - Cartesian coordinates
|
|
||||||
*
|
|
||||||
* \param[out] dphi - latitude
|
|
||||||
* \param[out] dlambda - longitude
|
|
||||||
* \param[out] h - height above reference ellipsoid
|
|
||||||
*
|
|
||||||
* Based in a Matlab function by Kai Borre
|
|
||||||
*/
|
|
||||||
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
|
|
||||||
|
|
||||||
/*!
|
/*!
|
||||||
* \brief Tropospheric correction
|
* \brief Tropospheric correction
|
||||||
*
|
*
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user