mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-04 17:57:03 +00:00
Code Issues Releases Wiki Activity

Refactoring least squares computation

Browse Source
This commit is contained in:
Carles Fernandez 2015-11-14 14:17:02 +01:00
parent 8abe0486e9
commit d52c3e36e3
18 changed files with 1019 additions and 1943 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
View File

@ -222,7 +222,7 @@ int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_it
if (pvt_result == true)
{
d_kml_dump->print_position_galileo(d_ls_pvt, d_flag_averaging);
d_kml_dump->print_position(d_ls_pvt, d_flag_averaging);
//ToDo: Implement Galileo RINEX and Galileo NMEA outputs
// d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
//

1
src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h
View File

@ -47,7 +47,6 @@
#include "kml_printer.h"
#include "rinex_printer.h"
#include "galileo_e1_ls_pvt.h"
#include "GPS_L1_CA.h"
#include "Galileo_E1.h"
class galileo_e1_pvt_cc;

2
src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
View File

@ -278,7 +278,7 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
if (pvt_result == true)
{
d_kml_dump->print_position_hybrid(d_ls_pvt, d_flag_averaging);
d_kml_dump->print_position(d_ls_pvt, d_flag_averaging);
//ToDo: Implement Galileo RINEX and Galileo NMEA outputs
// d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
//

4
src/algorithms/PVT/libs/CMakeLists.txt
View File

@ -19,13 +19,15 @@
add_definitions( -DGNSS_SDR_VERSION="${VERSION}" )
set(PVT_LIB_SOURCES
ls_pvt.cc
gps_l1_ca_ls_pvt.cc
galileo_e1_ls_pvt.cc
hybrid_ls_pvt.cc
kml_printer.cc
rinex_printer.cc
nmea_printer.cc
rtcm_printer.cc
rtcm_printer.cc
geojson_printer.cc
)
include_directories(

549
src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
View File

@ -36,16 +36,15 @@
using google::LogMessage;
galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file)
galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file) : Ls_Pvt()
{
// init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels;
d_ephemeris = new Galileo_Navigation_Message[nchannels];
d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file;
d_averaging_depth = 0;
d_galileo_current_time = 0;
b_valid_position = false;
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
{
@ -63,28 +62,6 @@ galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels, std::string dump_filename, b
}
}
}
d_valid_observations = 0;
d_latitude_d = 0.0;
d_longitude_d = 0.0;
d_height_m = 0.0;
d_avg_latitude_d = 0.0;
d_avg_longitude_d = 0.0;
d_avg_height_m = 0.0;
d_x_m = 0.0;
d_y_m = 0.0;
d_z_m = 0.0;
d_GDOP = 0.0;
d_PDOP = 0.0;
d_HDOP = 0.0;
d_VDOP = 0.0;
d_TDOP = 0.0;
d_flag_averaging = false;
}
void galileo_e1_ls_pvt::set_averaging_depth(int depth)
{
d_averaging_depth = depth;
}
@ -95,153 +72,6 @@ galileo_e1_ls_pvt::~galileo_e1_ls_pvt()
}
arma::vec galileo_e1_ls_pvt::rotateSatellite(double traveltime, const arma::vec & X_sat)
{
/*
* Returns rotated satellite ECEF coordinates due to Earth
* rotation during signal travel time
*
* Inputs:
* travelTime - signal travel time
* X_sat - satellite's ECEF coordinates
*
* Returns:
* X_sat_rot - rotated satellite's coordinates (ECEF)
*/
//--- Find rotation angle --------------------------------------------------
double omegatau;
omegatau = GALILEO_OMEGA_EARTH_DOT * traveltime;
//--- Build a rotation matrix ----------------------------------------------
arma::mat R3 = arma::zeros(3,3);
R3(0, 0) = cos(omegatau);
R3(0, 1) = sin(omegatau);
R3(0, 2) = 0.0;
R3(1, 0) = -sin(omegatau);
R3(1, 1) = cos(omegatau);
R3(1, 2) = 0.0;
R3(2, 0) = 0.0;
R3(2, 1) = 0.0;
R3(2, 2) = 1.0;
//--- Do the rotation ------------------------------------------------------
arma::vec X_sat_rot;
X_sat_rot = R3 * X_sat;
return X_sat_rot;
}
arma::vec galileo_e1_ls_pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w)
{
/* Computes the Least Squares Solution.
* Inputs:
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
* obs - Observations - the pseudorange measurements to each satellite
* w - weigths vector
*
* Returns:
* pos - receiver position and receiver clock error
* (in ECEF system: [X, Y, Z, dt])
*/
//=== Initialization =======================================================
int nmbOfIterations = 10; // TODO: include in config
int nmbOfSatellites;
nmbOfSatellites = satpos.n_cols; //Armadillo
arma::vec pos = "0.0 0.0 0.0 0.0";
arma::mat A;
arma::mat omc;
arma::mat az;
arma::mat el;
A = arma::zeros(nmbOfSatellites, 4);
omc = arma::zeros(nmbOfSatellites, 1);
az = arma::zeros(1, nmbOfSatellites);
el = arma::zeros(1, nmbOfSatellites);
arma::mat X = satpos;
arma::vec Rot_X;
double rho2;
double traveltime;
double trop = 0.0;
double dlambda;
double dphi;
double h;
arma::mat mat_tmp;
arma::vec x;
//=== Iteratively find receiver position ===================================
for (int iter = 0; iter < nmbOfIterations; iter++)
{
for (int i = 0; i < nmbOfSatellites; i++)
{
if (iter == 0)
{
//--- Initialize variables at the first iteration --------------
Rot_X = X.col(i); //Armadillo
trop = 0.0;
}
else
{
//--- Update equations -----------------------------------------
rho2 = (X(0, i) - pos(0)) *
(X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
(X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
(X(2, i) - pos(2));
traveltime = sqrt(rho2) / GALILEO_C_m_s;
//--- Correct satellite position (do to earth rotation) --------
Rot_X = rotateSatellite(traveltime, X.col(i)); //armadillo
//--- Find DOA and range of satellites
topocent(&d_visible_satellites_Az[i],
&d_visible_satellites_El[i],
&d_visible_satellites_Distance[i],
pos.subvec(0,2),
Rot_X - pos.subvec(0, 2));
if(traveltime < 0.1 && nmbOfSatellites > 3)
{
//--- Find receiver's height
togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
//--- Find delay due to troposphere (in meters)
tropo(&trop, sin(d_visible_satellites_El[i] * GALILEO_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
if(trop > 50.0 ) trop = 0.0;
}
}
//--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
//--- Construct the A matrix ---------------------------------------
//Armadillo
A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
A(i,3) = 1.0;
}
//--- Find position update ---------------------------------------------
x = arma::solve(w*A, w*omc); // Armadillo
//--- Apply position update --------------------------------------------
pos = pos + x;
if (arma::norm(x,2) < 1e-4)
{
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
}
}
try
{
//-- compute the Dilution Of Precision values
d_Q = arma::inv(arma::htrans(A)*A);
}
catch(std::exception& e)
{
d_Q = arma::zeros(4,4);
}
return pos;
}
bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging)
{
@ -350,6 +180,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs="<< obs;
DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W);
// Compute GST and Gregorian time
@ -379,42 +210,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
<< " [deg], Height= " << d_height_m << " [m]" << std::endl;
// ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat F = arma::zeros(3,3);
F(0,0) = -sin(GPS_TWO_PI * (d_longitude_d/360.0));
F(0,1) = -sin(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0));
F(0,2) = cos(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0));
F(1,0) = cos((GPS_TWO_PI * d_longitude_d)/360.0);
F(1,1) = -sin((GPS_TWO_PI * d_latitude_d)/360.0) * sin((GPS_TWO_PI * d_longitude_d)/360.0);
F(1,2) = cos((GPS_TWO_PI * d_latitude_d/360.0)) * sin((GPS_TWO_PI * d_longitude_d)/360.0);
F(2,0) = 0;
F(2,1) = cos((GPS_TWO_PI * d_latitude_d)/360.0);
F(2,2) = sin((GPS_TWO_PI * d_latitude_d/360.0));
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
arma::mat DOP_ENU = arma::zeros(3, 3);
try
{
DOP_ENU = arma::htrans(F) * Q_ECEF * F;
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP
d_TDOP = sqrt(d_Q(3,3)); // TDOP
}
catch(std::exception& ex)
{
d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
galileo_e1_ls_pvt::compute_DOP();
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
@ -512,340 +308,3 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
return false;
}
void galileo_e1_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{
/* Conversion of Cartesian coordinates (X,Y,Z) to geographical
coordinates (latitude, longitude, h) on a selected reference ellipsoid.
Choices of Reference Ellipsoid for Geographical Coordinates
0. International Ellipsoid 1924
1. International Ellipsoid 1967
2. World Geodetic System 1972
3. Geodetic Reference System 1980
4. World Geodetic System 1984
*/
const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
double lambda = atan2(Y, X);
double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1;
double oldh = 0.0;
double N;
int iterations = 0;
do
{
oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h) ))));
h = sqrt(X * X + Y * Y) / cos(phi) - N;
iterations = iterations + 1;
if (iterations > 100)
{
LOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
break;
}
}
while (std::abs(h - oldh) > 1.0e-12);
d_latitude_d = phi * 180.0 / GALILEO_PI;
d_longitude_d = lambda * 180.0 / GALILEO_PI;
d_height_m = h;
}
void galileo_e1_ls_pvt::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;
}
*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;
}
void galileo_e1_ls_pvt::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 = GALILEO_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
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 = arma::zeros(3,3);
F(0,0) = -sl;
F(0,1) = -sb * cl;
F(0,2) = cb * cl;
F(1,0) = cl;
F(1,1) = -sb * sl;
F(1,2) = cb * sl;
F(2,0) = 0.0;
F(2,1) = cb;
F(2,2) = 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.0;
*El = 90.0;
}
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));
}
void galileo_e1_ls_pvt::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:
sinel - sin of elevation angle of satellite
hsta_km - height of station in km
p_mb - atmospheric pressure in mb at height hp_km
t_kel - surface temperature in degrees Kelvin at height htkel_km
hum - humidity in % at height hhum_km
hp_km - height of pressure measurement in km
htkel_km - height of temperature measurement in km
hhum_km - height of humidity measurement in km
Outputs:
ddr_m - range correction (meters)
Reference
Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric
Refraction Correction Model. Paper presented at the
American Geophysical Union Annual Fall Meeting, San
Francisco, December 12-17
Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
*/
const double a_e = 6378.137; // semi-major axis of earth ellipsoid
const double b0 = 7.839257e-5;
const double tlapse = -6.5;
const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5);
double tkhum = t_kel + tlapse * (hhum_km - htkel_km);
double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15);
double e0 = 0.0611 * hum * pow(10, atkel);
double tksea = t_kel - tlapse * htkel_km;
double tkelh = tksea + tlapse * hhum_km;
double e0sea = e0 * pow((tksea / tkelh), (4 * em));
double tkelp = tksea + tlapse * hp_km;
double psea = p_mb * pow((tksea / tkelp), em);
if(sinel < 0) { sinel = 0.0; }
double tropo_delay = 0.0;
bool done = false;
double refsea = 77.624e-6 / tksea;
double htop = 1.1385e-5 / refsea;
refsea = refsea * psea;
double ref = refsea * pow(((htop - hsta_km) / htop), 4);
double a;
double b;
double rtop;
while(1)
{
rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2));
// check to see if geometry is crazy
if(rtop < 0) { rtop = 0; }
rtop = sqrt(rtop) - (a_e + hsta_km) * sinel;
a = -sinel / (htop - hsta_km);
b = -b0 * (1 - pow(sinel,2)) / (htop - hsta_km);
arma::vec rn = arma::vec(8);
rn.zeros();
for(int i = 0; i<8; i++)
{
rn(i) = pow(rtop, (i+1+1));
}
arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b /3, a * (pow(a, 2) + 3 * b),
pow(a, 4)/5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b)/3,
pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0};
if(pow(b, 2) > 1.0e-35)
{
alpha(6) = a * pow(b, 3) /2;
alpha(7) = pow(b, 4) / 9;
}
double dr = rtop;
arma::mat aux_ = alpha * rn;
dr = dr + aux_(0, 0);
tropo_delay = tropo_delay + dr * ref * 1000;
if(done == true)
{
*ddr_m = tropo_delay;
break;
}
done = true;
refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea;
htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea;
ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4);
}
}

86
src/algorithms/PVT/libs/galileo_e1_ls_pvt.h
View File

@ -32,113 +32,43 @@
#ifndef GNSS_SDR_GALILEO_E1_LS_PVT_H_
#define GNSS_SDR_GALILEO_E1_LS_PVT_H_
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <cstdio>
#include <deque>
#include <fstream>
#include <iostream>
#include <map>
#include <sstream>
#include <string>
#include <armadillo>
#include <boost/date_time/posix_time/posix_time.hpp>
#include "GPS_L1_CA.h"
#include "ls_pvt.h"
#include "galileo_navigation_message.h"
#include "gnss_synchro.h"
#include "galileo_ephemeris.h"
#include "galileo_utc_model.h"
#define PVT_MAX_CHANNELS 24
/*!
* \brief This class implements a simple PVT Least Squares solution
*/
class galileo_e1_ls_pvt
class galileo_e1_ls_pvt : public Ls_Pvt
{
private:
arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w);
arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat);
void topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx);
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
void 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);
public:
int d_nchannels; //!< Number of available channels for positioning
int d_valid_observations; //!< Number of valid pseudorange observations (valid satellites)
int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS]; //!< Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //!< Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //!< Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~galileo_e1_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging);
int d_nchannels; //!< Number of available channels for positioning
Galileo_Navigation_Message* d_ephemeris;
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
Galileo_Utc_Model galileo_utc_model;
Galileo_Iono galileo_iono;
Galileo_Almanac galileo_almanac;
double d_galileo_current_time;
boost::posix_time::ptime d_position_UTC_time;
bool b_valid_position;
double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //!< Longitude in degrees
double d_height_m; //!< Height [m]
//averaging
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
int d_averaging_depth; //!< Length of averaging window
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_height_m; //!< Averaged height [m]
double d_x_m;
double d_y_m;
double d_z_m;
// DOP estimations
arma::mat d_Q;
double d_GDOP;
double d_PDOP;
double d_HDOP;
double d_VDOP;
double d_TDOP;
bool d_flag_dump_enabled;
bool d_flag_averaging;
std::string d_dump_filename;
std::ofstream d_dump_file;
void set_averaging_depth(int depth);
galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~galileo_e1_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging);
/*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical
* coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid.
*
* \param[in] X [m] Cartesian coordinate
* \param[in] Y [m] Cartesian coordinate
* \param[in] Z [m] Cartesian coordinate
* \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates:
* 0 - International Ellipsoid 1924.
* 1 - International Ellipsoid 1967.
* 2 - World Geodetic System 1972.
* 3 - Geodetic Reference System 1980.
* 4 - World Geodetic System 1984.
*
*/
void cart2geo(double X, double Y, double Z, int elipsoid_selection);
};
#endif

126
src/algorithms/PVT/libs/geojson_printer.cc Normal file
View File

@ -0,0 +1,126 @@
/*!
* \file geojson_printer.cc
* \brief Implementation of a class that prints PVT solutions in GeoJSON format
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "geojson_printer.h"
#include <iostream>
#include <iomanip>
GeoJSON_Printer::GeoJSON_Printer () {}
GeoJSON_Printer::~GeoJSON_Printer ()
{
close_file();
}
bool GeoJSON_Printer::set_headers(std::string filename)
{
//time_t rawtime;
//struct tm * timeinfo;
//time ( &rawtime );
//timeinfo = localtime ( &rawtime );
geojson_file.open(filename.c_str());
if (geojson_file.is_open())
{
DLOG(INFO) << "GeoJSON printer writing on " << filename.c_str();
// Set iostream numeric format and precision
geojson_file.setf(geojson_file.fixed, geojson_file.floatfield);
geojson_file << std::setprecision(14);
geojson_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
<< "<altitudeMode>absolute</altitudeMode>" << std::endl
<< "<coordinates>" << std::endl;
return true;
}
else
{
return false;
}
}
bool GeoJSON_Printer::print_position(const std::shared_ptr<Ls_Pvt>& position, bool print_average_values)
{
double latitude;
double longitude;
double height;
std::shared_ptr<Ls_Pvt> position_ = position;
if (print_average_values == false)
{
latitude = position_->d_latitude_d;
longitude = position_->d_longitude_d;
height = position_->d_height_m;
}
else
{
latitude = position_->d_avg_latitude_d;
longitude = position_->d_avg_longitude_d;
height = position_->d_avg_height_m;
}
if (geojson_file.is_open())
{
geojson_file << longitude << "," << latitude << "," << height << std::endl;
return true;
}
else
{
return false;
}
}
bool GeoJSON_Printer::close_file()
{
if (geojson_file.is_open())
{
geojson_file << "</coordinates>" << std::endl
<< "</LineString>" << std::endl
<< "</Placemark>" << std::endl
<< "</Document>" << std::endl
<< "</kml>";
geojson_file.close();
return true;
}
else
{
return false;
}
}

60
src/algorithms/PVT/libs/geojson_printer.h Normal file
View File

@ -0,0 +1,60 @@
/*!
* \file geojson_printer.h
* \brief Interface of a class that prints PVT solutions in GeoJSON format
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GEOJSON_PRINTER_H_
#define GNSS_SDR_GEOJSON_PRINTER_H_
#include <iostream>
#include <fstream>
#include <memory>
#include <string>
#include "ls_pvt.h"
/*!
* \brief Prints PVT solutions in GeoJSON format file
*
* See http://geojson.org/geojson-spec.html
*/
class GeoJSON_Printer
{
private:
std::ofstream geojson_file;
public:
GeoJSON_Printer();
~GeoJSON_Printer();
bool set_headers(std::string filename);
bool print_position(const std::shared_ptr<Ls_Pvt>& position, bool print_average_values);
bool close_file();
};
#endif

553
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
View File

@ -36,35 +36,16 @@
using google::LogMessage;
DEFINE_bool(tropo, true, "Apply tropospheric correction");
gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file)
gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file) : Ls_Pvt()
{
// init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels;
d_ephemeris = new Gps_Navigation_Message[nchannels];
d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file;
d_averaging_depth = 0;
d_GPS_current_time = 0;
b_valid_position = false;
d_valid_observations = 0;
d_latitude_d = 0.0;
d_longitude_d = 0.0;
d_height_m = 0.0;
d_avg_latitude_d = 0.0;
d_avg_longitude_d = 0.0;
d_avg_height_m = 0.0;
d_x_m = 0.0;
d_y_m = 0.0;
d_z_m = 0.0;
d_GDOP = 0.0;
d_PDOP = 0.0;
d_HDOP = 0.0;
d_VDOP = 0.0;
d_TDOP = 0.0;
d_flag_averaging = false;
d_GPS_current_time = 0;
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
@ -86,11 +67,6 @@ gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels, std::string dump_filename, boo
}
void gps_l1_ca_ls_pvt::set_averaging_depth(int depth)
{
d_averaging_depth = depth;
}
gps_l1_ca_ls_pvt::~gps_l1_ca_ls_pvt()
{
@ -99,155 +75,6 @@ gps_l1_ca_ls_pvt::~gps_l1_ca_ls_pvt()
}
arma::vec gps_l1_ca_ls_pvt::rotateSatellite(double traveltime, const arma::vec & X_sat)
{
/*
* Returns rotated satellite ECEF coordinates due to Earth
* rotation during signal travel time
*
* Inputs:
* travelTime - signal travel time
* X_sat - satellite's ECEF coordinates
*
* Returns:
* X_sat_rot - rotated satellite's coordinates (ECEF)
*/
//--- Find rotation angle --------------------------------------------------
double omegatau;
omegatau = OMEGA_EARTH_DOT * traveltime;
//--- Build a rotation matrix ----------------------------------------------
arma::mat R3 = arma::zeros(3,3);
R3(0, 0) = cos(omegatau);
R3(0, 1) = sin(omegatau);
R3(0, 2) = 0.0;
R3(1, 0) = -sin(omegatau);
R3(1, 1) = cos(omegatau);
R3(1, 2) = 0.0;
R3(2, 0) = 0.0;
R3(2, 1) = 0.0;
R3(2, 2) = 1.0;
//--- Do the rotation ------------------------------------------------------
arma::vec X_sat_rot;
X_sat_rot = R3 * X_sat;
return X_sat_rot;
}
arma::vec gps_l1_ca_ls_pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w)
{
/* Computes the Least Squares Solution.
* Inputs:
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
* obs - Observations - the pseudorange measurements to each satellite
* w - weigths vector
*
* Returns:
* pos - receiver position and receiver clock error
* (in ECEF system: [X, Y, Z, dt])
*/
//=== Initialization =======================================================
int nmbOfIterations = 10; // TODO: include in config
int nmbOfSatellites;
nmbOfSatellites = satpos.n_cols; //Armadillo
arma::vec pos = "0.0 0.0 0.0 0.0";
arma::mat A;
arma::mat omc;
arma::mat az;
arma::mat el;
A = arma::zeros(nmbOfSatellites, 4);
omc = arma::zeros(nmbOfSatellites, 1);
az = arma::zeros(1, nmbOfSatellites);
el = arma::zeros(1, nmbOfSatellites);
arma::mat X = satpos;
arma::vec Rot_X;
double rho2;
double traveltime;
double trop = 0.0;
double dlambda;
double dphi;
double h;
arma::mat mat_tmp;
arma::vec x;
//=== Iteratively find receiver position ===================================
for (int iter = 0; iter < nmbOfIterations; iter++)
{
for (int i = 0; i < nmbOfSatellites; i++)
{
if (iter == 0)
{
//--- Initialize variables at the first iteration --------------
Rot_X = X.col(i); //Armadillo
trop = 0.0;
}
else
{
//--- Update equations -----------------------------------------
rho2 = (X(0, i) - pos(0)) * (X(0, i) - pos(0))
+ (X(1, i) - pos(1)) * (X(1, i) - pos(1))
+ (X(2, i) - pos(2)) * (X(2, i) - pos(2));
traveltime = sqrt(rho2) / GPS_C_m_s;
//--- Correct satellite position (do to earth rotation) --------
Rot_X = rotateSatellite(traveltime, X.col(i)); //armadillo
//--- Find satellites' DOA
topocent(&d_visible_satellites_Az[i], &d_visible_satellites_El[i],
&d_visible_satellites_Distance[i], pos.subvec(0,2), Rot_X - pos.subvec(0,2));
if(FLAGS_tropo)
{
if(traveltime < 0.1 && nmbOfSatellites > 3)
{
//--- Find receiver's height
togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
//--- Find delay due to troposphere (in meters)
tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
if(trop > 50.0 ) trop = 0.0;
}
}
}
//--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0,2),2) - pos(3) - trop); // Armadillo
//--- Construct the A matrix ---------------------------------------
//Armadillo
A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
A(i,3) = 1.0;
}
//--- Find position update ---------------------------------------------
x = arma::solve(w*A, w*omc); // Armadillo
//--- Apply position update --------------------------------------------
pos = pos + x;
if (arma::norm(x, 2) < 1e-4)
{
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
}
}
try
{
//-- compute the Dilution Of Precision values
d_Q = arma::inv(arma::htrans(A)*A);
}
catch(std::exception& e)
{
d_Q = arma::zeros(4, 4);
}
return pos;
}
bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double GPS_current_time, bool flag_averaging)
{
@ -339,7 +166,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W);
mypos = gps_l1_ca_ls_pvt::leastSquarePos(satpos, obs, W);
DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos;
gps_l1_ca_ls_pvt::cart2geo(mypos(0), mypos(1), mypos(2), 4);
//ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km)
@ -360,43 +187,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
<< " [deg], Height= " << d_height_m << " [m]";
// ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat F=arma::zeros(3,3);
F(0,0) = -sin(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,1) = -sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,2) = cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(1,0) = cos((GPS_TWO_PI*d_longitude_d)/360.0);
F(1,1) = -sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(1,2) = cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(2,0) = 0;
F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0));
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
arma::mat DOP_ENU = arma::zeros(3, 3);
try
{
DOP_ENU = arma::htrans(F)*Q_ECEF*F;
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
d_PDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1) + DOP_ENU(2, 2));// PDOP
d_HDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2, 2)); // VDOP
d_TDOP = sqrt(d_Q(3, 3)); // TDOP
}
catch(std::exception& ex)
{
d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
gps_l1_ca_ls_pvt::compute_DOP();
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
@ -494,339 +285,3 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
}
void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{
/* Conversion of Cartesian coordinates (X,Y,Z) to geographical
coordinates (latitude, longitude, h) on a selected reference ellipsoid.
Choices of Reference Ellipsoid for Geographical Coordinates
0. International Ellipsoid 1924
1. International Ellipsoid 1967
2. World Geodetic System 1972
3. Geodetic Reference System 1980
4. World Geodetic System 1984
*/
const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
double lambda = atan2(Y,X);
double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1;
double oldh = 0.0;
double N;
int iterations = 0;
do
{
oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
phi = atan(Z / ((sqrt(X*X + Y*Y) * (1.0 - (2.0 -f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h) ))));
h = sqrt(X * X + Y * Y) / cos(phi) - N;
iterations = iterations + 1;
if (iterations > 100)
{
LOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
break;
}
}
while (std::abs(h - oldh) > 1.0e-12);
d_latitude_d = phi * 180.0 / GPS_PI;
d_longitude_d = lambda * 180.0 / GPS_PI;
d_height_m = h;
}
void gps_l1_ca_ls_pvt::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/GPS_PI;
// compute square of eccentricity
double esq;
if (finv < 1.0E-20)
{
esq = 0;
}
else
{
esq = (2 - 1/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;
}
// 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;
}
*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;
}
void gps_l1_ca_ls_pvt::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
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 = arma::zeros(3,3);
F(0,0) = -sl;
F(0,1) = -sb * cl;
F(0,2) = cb * cl;
F(1,0) = cl;
F(1,1) = -sb * sl;
F(1,2) = cb * sl;
F(2,0) = 0;
F(2,1) = cb;
F(2,2) = 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));
}
void gps_l1_ca_ls_pvt::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:
sinel - sin of elevation angle of satellite
hsta_km - height of station in km
p_mb - atmospheric pressure in mb at height hp_km
t_kel - surface temperature in degrees Kelvin at height htkel_km
hum - humidity in % at height hhum_km
hp_km - height of pressure measurement in km
htkel_km - height of temperature measurement in km
hhum_km - height of humidity measurement in km
Outputs:
ddr_m - range correction (meters)
Reference
Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric
Refraction Correction Model. Paper presented at the
American Geophysical Union Annual Fall Meeting, San
Francisco, December 12-17
Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
*/
const double a_e = 6378.137; // semi-major axis of earth ellipsoid
const double b0 = 7.839257e-5;
const double tlapse = -6.5;
const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5);
double tkhum = t_kel + tlapse * (hhum_km - htkel_km);
double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15);
double e0 = 0.0611 * hum * pow(10, atkel);
double tksea = t_kel - tlapse * htkel_km;
double tkelh = tksea + tlapse * hhum_km;
double e0sea = e0 * pow((tksea / tkelh), (4 * em));
double tkelp = tksea + tlapse * hp_km;
double psea = p_mb * pow((tksea / tkelp), em);
if(sinel < 0) { sinel = 0.0; }
double tropo_delay = 0.0;
bool done = false;
double refsea = 77.624e-6 / tksea;
double htop = 1.1385e-5 / refsea;
refsea = refsea * psea;
double ref = refsea * pow(((htop - hsta_km) / htop), 4);
double a;
double b;
double rtop;
while(1)
{
rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2));
// check to see if geometry is crazy
if(rtop < 0) { rtop = 0; }
rtop = sqrt(rtop) - (a_e + hsta_km) * sinel;
a = -sinel / (htop - hsta_km);
b = -b0 * (1 - pow(sinel,2)) / (htop - hsta_km);
arma::vec rn = arma::vec(8);
rn.zeros();
for(int i = 0; i<8; i++)
{
rn(i) = pow(rtop, (i+1+1));
}
arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b /3, a * (pow(a, 2) + 3 * b),
pow(a, 4)/5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b)/3,
pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0};
if(pow(b, 2) > 1.0e-35)
{
alpha(6) = a * pow(b, 3) /2;
alpha(7) = pow(b, 4) / 9;
}
double dr = rtop;
arma::mat aux_ = alpha * rn;
dr = dr + aux_(0, 0);
tropo_delay = tropo_delay + dr * ref * 1000;
if(done == true)
{
*ddr_m = tropo_delay;
break;
}
done = true;
refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea;
htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea;
ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4);
}
}

88
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h
View File

@ -1,7 +1,7 @@
/*!
* \file gps_l1_ca_ls_pvt.h
* \brief Interface of a Least Squares Position, Velocity, and Time (PVT)
* solver, based on K.Borre's Matlab receiver.
* solver for GPS L1 C/A, based on K.Borre's Matlab receiver.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* -------------------------------------------------------------------------
*
@ -31,20 +31,13 @@
#ifndef GNSS_SDR_GPS_L1_CA_LS_PVT_H_
#define GNSS_SDR_GPS_L1_CA_LS_PVT_H_
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <cstdio>
#include <deque>
#include <fstream>
#include <iostream>
#include <map>
#include <sstream>
#include <string>
#include <armadillo>
#include <boost/date_time/posix_time/posix_time.hpp>
#include "gnss_synchro.h"
#include "ls_pvt.h"
#include "GPS_L1_CA.h"
#include "gnss_synchro.h"
#include "gps_ephemeris.h"
#include "gps_navigation_message.h"
#include "gps_utc_model.h"
@ -53,27 +46,18 @@
#include "sbas_satellite_correction.h"
#include "sbas_ephemeris.h"
#define PVT_MAX_CHANNELS 24
/*!
* \brief This class implements a simple PVT Least Squares solution
* \brief This class implements a simple PVT Least Squares solution for GPS L1 C/A signals
*/
class gps_l1_ca_ls_pvt
class gps_l1_ca_ls_pvt : public Ls_Pvt
{
private:
arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w);
arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat);
void topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx);
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
void 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);
public:
int d_nchannels; //!< Number of available channels for positioning
int d_valid_observations; //!< Number of valid pseudorange observations (valid satellites)
int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS]; //!< Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //!< Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //!< Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
gps_l1_ca_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file);
~gps_l1_ca_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double GPS_current_time, bool flag_averaging);
int d_nchannels; //!< Number of available channels for positioning
Gps_Navigation_Message* d_ephemeris;
@ -87,64 +71,12 @@ public:
std::map<int,Sbas_Ephemeris> sbas_ephemeris_map;
double d_GPS_current_time;
boost::posix_time::ptime d_position_UTC_time;
bool b_valid_position;
double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //!< Longitude in degrees
double d_height_m; //!< Height [m]
//averaging
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
int d_averaging_depth; //!< Length of averaging window
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_height_m; //!< Averaged height [m]
double d_x_m;
double d_y_m;
double d_z_m;
// DOP estimations
arma::mat d_Q;
double d_GDOP;
double d_PDOP;
double d_HDOP;
double d_VDOP;
double d_TDOP;
bool d_flag_dump_enabled;
bool d_flag_averaging;
std::string d_dump_filename;
std::ofstream d_dump_file;
void set_averaging_depth(int depth);
gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~gps_l1_ca_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double GPS_current_time, bool flag_averaging);
/*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical
* coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid.
*
* \param[in] X [m] Cartesian coordinate
* \param[in] Y [m] Cartesian coordinate
* \param[in] Z [m] Cartesian coordinate
* \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates:
* 0 - International Ellipsoid 1924.
* 1 - International Ellipsoid 1967.
* 2 - World Geodetic System 1972.
* 3 - Geodetic Reference System 1980.
* 4 - World Geodetic System 1984.
*
*/
void cart2geo(double X, double Y, double Z, int elipsoid_selection);
};
#endif

543
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@ -36,7 +36,7 @@
using google::LogMessage;
hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file)
hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file) : Ls_Pvt()
{
// init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels;
@ -44,25 +44,7 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
d_GPS_ephemeris = new Gps_Navigation_Message[nchannels];
d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file;
d_averaging_depth = 0;
d_galileo_current_time = 0;
b_valid_position = false;
d_latitude_d = 0.0;
d_longitude_d = 0.0;
d_height_m = 0.0;
d_avg_latitude_d = 0.0;
d_avg_longitude_d = 0.0;
d_avg_height_m = 0.0;
d_x_m = 0.0;
d_y_m = 0.0;
d_z_m = 0.0;
d_GDOP = 0.0;
d_PDOP = 0.0;
d_HDOP = 0.0;
d_VDOP = 0.0;
d_TDOP = 0.0;
d_flag_averaging = false;
d_valid_observations = 0;
d_valid_GPS_obs = 0;
d_valid_GAL_obs = 0;
count_valid_position = 0;
@ -86,10 +68,6 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
}
void hybrid_ls_pvt::set_averaging_depth(int depth)
{
d_averaging_depth = depth;
}
hybrid_ls_pvt::~hybrid_ls_pvt()
@ -100,152 +78,8 @@ hybrid_ls_pvt::~hybrid_ls_pvt()
}
arma::vec hybrid_ls_pvt::rotateSatellite(double traveltime, const arma::vec & X_sat)
{
/*
* Returns rotated satellite ECEF coordinates due to Earth
* rotation during signal travel time
*
* Inputs:
* travelTime - signal travel time
* X_sat - satellite's ECEF coordinates
*
* Returns:
* X_sat_rot - rotated satellite's coordinates (ECEF)
*/
//--- Find rotation angle --------------------------------------------------
double omegatau;
omegatau = OMEGA_EARTH_DOT * traveltime;
//--- Build a rotation matrix ----------------------------------------------
arma::mat R3 = arma::zeros(3,3);
R3(0, 0) = cos(omegatau);
R3(0, 1) = sin(omegatau);
R3(0, 2) = 0.0;
R3(1, 0) = -sin(omegatau);
R3(1, 1) = cos(omegatau);
R3(1, 2) = 0.0;
R3(2, 0) = 0.0;
R3(2, 1) = 0.0;
R3(2, 2) = 1;
//--- Do the rotation ------------------------------------------------------
arma::vec X_sat_rot;
X_sat_rot = R3 * X_sat;
return X_sat_rot;
}
arma::vec hybrid_ls_pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w)
{
/* Computes the Least Squares Solution.
* Inputs:
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
* obs - Observations - the pseudorange measurements to each satellite
* w - weigths vector
*
* Returns:
* pos - receiver position and receiver clock error
* (in ECEF system: [X, Y, Z, dt])
*/
//=== Initialization =======================================================
int nmbOfIterations = 10; // TODO: include in config
int nmbOfSatellites;
nmbOfSatellites = satpos.n_cols; //Armadillo
arma::vec pos = "0.0 0.0 0.0 0.0";
arma::mat A;
arma::mat omc;
arma::mat az;
arma::mat el;
A = arma::zeros(nmbOfSatellites, 4);
omc = arma::zeros(nmbOfSatellites, 1);
az = arma::zeros(1, nmbOfSatellites);
el = arma::zeros(1, nmbOfSatellites);
arma::mat X = satpos;
arma::vec Rot_X;
double rho2;
double traveltime;
double trop = 0.0;
double dlambda;
double dphi;
double h;
arma::mat mat_tmp;
arma::vec x;
//=== Iteratively find receiver position ===================================
for (int iter = 0; iter < nmbOfIterations; iter++)
{
for (int i = 0; i < nmbOfSatellites; i++)
{
if (iter == 0)
{
//--- Initialize variables at the first iteration --------------
Rot_X = X.col(i); //Armadillo
trop = 0.0;
}
else
{
//--- Update equations -----------------------------------------
rho2 = (X(0, i) - pos(0)) *
(X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
(X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
(X(2, i) - pos(2));
traveltime = sqrt(rho2) / GALILEO_C_m_s;
//--- Correct satellite position (do to earth rotation) --------
Rot_X = rotateSatellite(traveltime, X.col(i)); //armadillo
//--- Find DOA and range of satellites
topocent(&d_visible_satellites_Az[i],
&d_visible_satellites_El[i],
&d_visible_satellites_Distance[i],
pos.subvec(0,2),
Rot_X - pos.subvec(0, 2));
if(traveltime < 0.1 && nmbOfSatellites > 3)
{
//--- Find receiver's height
togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
//--- Find delay due to troposphere (in meters)
tropo(&trop, sin(d_visible_satellites_El[i] * GALILEO_PI/180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
if(trop > 50.0 ) trop = 0.0;
}
}
//--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
//--- Construct the A matrix ---------------------------------------
//Armadillo
A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
A(i,3) = 1.0;
}
//--- Find position update ---------------------------------------------
x = arma::solve(w*A, w*omc); // Armadillo
//--- Apply position update --------------------------------------------
pos = pos + x;
if (arma::norm(x,2) < 1e-4)
{
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
}
}
try
{
//-- compute the Dilution Of Precision values
d_Q = arma::inv(arma::htrans(A)*A);
}
catch(std::exception& e)
{
d_Q = arma::zeros(4,4);
}
return pos;
}
bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double hybrid_current_time, bool flag_averaging)
@ -456,42 +290,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
// ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat F = arma::zeros(3,3);
F(0,0) = -sin(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,1) = -sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,2) = cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(1,0) = cos((GPS_TWO_PI*d_longitude_d)/360.0);
F(1,1) = -sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(1,2) = cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(2,0) = 0;
F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0));
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
arma::mat DOP_ENU = arma::zeros(3, 3);
try
{
DOP_ENU = arma::htrans(F)*Q_ECEF*F;
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP
d_TDOP = sqrt(d_Q(3,3)); // TDOP
}
catch(std::exception& ex)
{
d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
compute_DOP();
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
@ -589,341 +388,3 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
return false;
}
void hybrid_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{
/* Conversion of Cartesian coordinates (X,Y,Z) to geographical
coordinates (latitude, longitude, h) on a selected reference ellipsoid.
Choices of Reference Ellipsoid for Geographical Coordinates
0. International Ellipsoid 1924
1. International Ellipsoid 1967
2. World Geodetic System 1972
3. Geodetic Reference System 1980
4. World Geodetic System 1984
*/
const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
double lambda = atan2(Y, X);
double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1;
double oldh = 0.0;
double N;
int iterations = 0;
do
{
oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h) ))));
h = sqrt(X * X + Y * Y) / cos(phi) - N;
iterations = iterations + 1;
if (iterations > 100)
{
LOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
break;
}
}
while (std::abs(h - oldh) > 1.0e-12);
d_latitude_d = phi * 180.0 / GPS_PI;
d_longitude_d = lambda * 180.0 / GPS_PI;
d_height_m = h;
}
void hybrid_ls_pvt::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;
}
// 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;
}
*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;
}
*h = r - a*(1.0 - 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;
}
void hybrid_ls_pvt::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
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 = arma::zeros(3,3);
F(0,0) = -sl;
F(0,1) = -sb*cl;
F(0,2) = cb*cl;
F(1,0) = cl;
F(1,1) = -sb*sl;
F(1,2) = cb*sl;
F(2,0) = 0;
F(2,1) = cb;
F(2,2) = 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));
}
void hybrid_ls_pvt::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:
sinel - sin of elevation angle of satellite
hsta_km - height of station in km
p_mb - atmospheric pressure in mb at height hp_km
t_kel - surface temperature in degrees Kelvin at height htkel_km
hum - humidity in % at height hhum_km
hp_km - height of pressure measurement in km
htkel_km - height of temperature measurement in km
hhum_km - height of humidity measurement in km
Outputs:
ddr_m - range correction (meters)
Reference
Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric
Refraction Correction Model. Paper presented at the
American Geophysical Union Annual Fall Meeting, San
Francisco, December 12-17
Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
*/
const double a_e = 6378.137; // semi-major axis of earth ellipsoid
const double b0 = 7.839257e-5;
const double tlapse = -6.5;
const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5);
double tkhum = t_kel + tlapse * (hhum_km - htkel_km);
double atkel = 7.5*(tkhum - 273.15) / (237.3 + tkhum - 273.15);
double e0 = 0.0611 * hum * pow(10, atkel);
double tksea = t_kel - tlapse * htkel_km;
double tkelh = tksea + tlapse * hhum_km;
double e0sea = e0 * pow((tksea / tkelh), (4 * em));
double tkelp = tksea + tlapse * hp_km;
double psea = p_mb * pow((tksea / tkelp), em);
if(sinel < 0) { sinel = 0.0; }
double tropo_delay = 0.0;
bool done = false;
double refsea = 77.624e-6 / tksea;
double htop = 1.1385e-5 / refsea;
refsea = refsea * psea;
double ref = refsea * pow(((htop - hsta_km) / htop), 4);
double a;
double b;
double rtop;
while(1)
{
rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2));
// check to see if geometry is crazy
if(rtop < 0) { rtop = 0; }
rtop = sqrt(rtop) - (a_e + hsta_km) * sinel;
a = -sinel / (htop - hsta_km);
b = -b0 * (1 - pow(sinel,2)) / (htop - hsta_km);
arma::vec rn = arma::vec(8);
rn.zeros();
for(int i = 0; i<8; i++)
{
rn(i) = pow(rtop, (i+1+1));
}
arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b /3, a * (pow(a, 2) + 3 * b),
pow(a, 4)/5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b)/3,
pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0};
if(pow(b, 2) > 1.0e-35)
{
alpha(6) = a * pow(b, 3) /2;
alpha(7) = pow(b, 4) / 9;
}
double dr = rtop;
arma::mat aux_ = alpha * rn;
dr = dr + aux_(0, 0);
tropo_delay = tropo_delay + dr * ref * 1000;
if(done == true)
{
*ddr_m = tropo_delay;
break;
}
done = true;
refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea;
htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea;
ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4);
}
}

81
src/algorithms/PVT/libs/hybrid_ls_pvt.h
View File

@ -32,20 +32,11 @@
#ifndef GNSS_SDR_HYBRID_LS_PVT_H_
#define GNSS_SDR_HYBRID_LS_PVT_H_
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <cstdio>
#include <deque>
#include <fstream>
#include <iostream>
#include <map>
#include <memory>
#include <sstream>
#include <string>
#include <armadillo>
#include <boost/date_time/posix_time/posix_time.hpp>
#include "GPS_L1_CA.h"
#include "ls_pvt.h"
#include "galileo_navigation_message.h"
#include "gps_navigation_message.h"
#include "gnss_synchro.h"
@ -54,29 +45,20 @@
#include "gps_ephemeris.h"
#include "gps_utc_model.h"
#define PVT_MAX_CHANNELS 24
/*!
* \brief This class implements a simple PVT Least Squares solution
*/
class hybrid_ls_pvt
class hybrid_ls_pvt : public Ls_Pvt
{
private:
arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w);
arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat);
void topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx);
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
void 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);
public:
hybrid_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~hybrid_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double hybrid_current_time, bool flag_averaging);
int d_nchannels; //!< Number of available channels for positioning
int d_valid_observations; //!< Number of valid pseudorange observations (valid satellites)
int d_valid_GPS_obs; //!< Number of valid GPS pseudorange observations (valid GPS satellites) -- used for hybrid configuration
int d_valid_GAL_obs; //!< Number of valid GALILEO pseudorange observations (valid GALILEO satellites) -- used for hybrid configuration
int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS]; //!< Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //!< Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //!< Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
Galileo_Navigation_Message* d_Gal_ephemeris;
Gps_Navigation_Message* d_GPS_ephemeris;
@ -91,63 +73,14 @@ public:
Gps_Iono gps_iono;
double d_galileo_current_time;
boost::posix_time::ptime d_position_UTC_time;
bool b_valid_position;
int count_valid_position;
double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //!< Longitude in degrees
double d_height_m; //!< Height [m]
//averaging
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
int d_averaging_depth; //!< Length of averaging window
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_height_m; //!< Averaged height [m]
double d_x_m;
double d_y_m;
double d_z_m;
// DOP estimations
arma::mat d_Q;
double d_GDOP;
double d_PDOP;
double d_HDOP;
double d_VDOP;
double d_TDOP;
bool d_flag_dump_enabled;
bool d_flag_averaging;
std::string d_dump_filename;
std::ofstream d_dump_file;
void set_averaging_depth(int depth);
hybrid_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~hybrid_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double hybrid_current_time, bool flag_averaging);
/*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical
* coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid.
*
* \param[in] X [m] Cartesian coordinate
* \param[in] Y [m] Cartesian coordinate
* \param[in] Z [m] Cartesian coordinate
* \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates:
* 0 - International Ellipsoid 1924.
* 1 - International Ellipsoid 1967.
* 2 - World Geodetic System 1972.
* 3 - Geodetic Reference System 1980.
* 4 - World Geodetic System 1984.
*
*/
void cart2geo(double X, double Y, double Z, int elipsoid_selection);
};
#endif

64
src/algorithms/PVT/libs/kml_printer.cc
View File

@ -82,13 +82,13 @@ bool Kml_Printer::set_headers(std::string filename)
bool Kml_Printer::print_position(const std::shared_ptr<gps_l1_ca_ls_pvt>& position, bool print_average_values)
bool Kml_Printer::print_position(const std::shared_ptr<Ls_Pvt>& position, bool print_average_values)
{
double latitude;
double longitude;
double height;
std::shared_ptr<gps_l1_ca_ls_pvt> position_ = position;
std::shared_ptr<Ls_Pvt> position_ = position;
if (print_average_values == false)
{
@ -114,66 +114,6 @@ bool Kml_Printer::print_position(const std::shared_ptr<gps_l1_ca_ls_pvt>& positi
}
}
//ToDo: make the class ls_pvt generic and heritate the particular gps/gal/glo ls_pvt in order to
// reuse kml_printer functions
bool Kml_Printer::print_position_galileo(const std::shared_ptr<galileo_e1_ls_pvt>& position, bool print_average_values)
{
double latitude;
double longitude;
double height;
std::shared_ptr<galileo_e1_ls_pvt> position_ = position;
if (print_average_values == false)
{
latitude = position_->d_latitude_d;
longitude = position_->d_longitude_d;
height = position_->d_height_m;
}
else
{
latitude = position_->d_avg_latitude_d;
longitude = position_->d_avg_longitude_d;
height = position_->d_avg_height_m;
}
if (kml_file.is_open())
{
kml_file << longitude << "," << latitude << "," << height << std::endl;
return true;
}
else
{
return false;
}
}
bool Kml_Printer::print_position_hybrid(const std::shared_ptr<hybrid_ls_pvt>& position, bool print_average_values)
{
double latitude;
double longitude;
double height;
if (print_average_values == false)
{
latitude = position->d_latitude_d;
longitude = position->d_longitude_d;
height = position->d_height_m;
}
else
{
latitude = position->d_avg_latitude_d;
longitude = position->d_avg_longitude_d;
height = position->d_avg_height_m;
}
if (kml_file.is_open())
{
kml_file << longitude << "," << latitude << "," << height << std::endl;
return true;
}
else
{
return false;
}
}
bool Kml_Printer::close_file()
{

12
src/algorithms/PVT/libs/kml_printer.h
View File

@ -37,9 +37,7 @@
#include <fstream>
#include <memory>
#include <string>
#include "gps_l1_ca_ls_pvt.h"
#include "galileo_e1_ls_pvt.h"
#include "hybrid_ls_pvt.h"
#include "ls_pvt.h"
/*!
* \brief Prints PVT information to OGC KML format file (can be viewed with Google Earth)
@ -51,13 +49,11 @@ class Kml_Printer
private:
std::ofstream kml_file;
public:
bool set_headers(std::string filename);
bool print_position(const std::shared_ptr<gps_l1_ca_ls_pvt>& position, bool print_average_values);
bool print_position_galileo(const std::shared_ptr<galileo_e1_ls_pvt>& position, bool print_average_values);
bool print_position_hybrid(const std::shared_ptr<hybrid_ls_pvt>& position, bool print_average_values);
bool close_file();
Kml_Printer();
~Kml_Printer();
bool set_headers(std::string filename);
bool print_position(const std::shared_ptr<Ls_Pvt>& position, bool print_average_values);
bool close_file();
};
#endif

607
src/algorithms/PVT/libs/ls_pvt.cc Normal file
View File

@ -0,0 +1,607 @@
/*!
* \file ls_pvt.h
* \brief Implementation of a base class for Least Squares PVT solutions
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "ls_pvt.h"
#include <exception>
#include "GPS_L1_CA.h"
#include <gflags/gflags.h>
#include <glog/logging.h>
using google::LogMessage;
DEFINE_bool(tropo, true, "Apply tropospheric correction");
Ls_Pvt::Ls_Pvt()
{
d_valid_observations = 0;
d_latitude_d = 0.0;
d_longitude_d = 0.0;
d_height_m = 0.0;
d_avg_latitude_d = 0.0;
d_avg_longitude_d = 0.0;
d_avg_height_m = 0.0;
d_x_m = 0.0;
d_y_m = 0.0;
d_z_m = 0.0;
d_GDOP = 0.0;
d_PDOP = 0.0;
d_HDOP = 0.0;
d_VDOP = 0.0;
d_TDOP = 0.0;
d_flag_averaging = false;
b_valid_position = false;
d_averaging_depth = 0;
}
arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w)
{
/* Computes the Least Squares Solution.
* Inputs:
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
* obs - Observations - the pseudorange measurements to each satellite
* w - weigths vector
*
* Returns:
* pos - receiver position and receiver clock error
* (in ECEF system: [X, Y, Z, dt])
*/
//=== Initialization =======================================================
int nmbOfIterations = 10; // TODO: include in config
int nmbOfSatellites;
nmbOfSatellites = satpos.n_cols; //Armadillo
arma::vec pos = "0.0 0.0 0.0 0.0";
arma::mat A;
arma::mat omc;
arma::mat az;
arma::mat el;
A = arma::zeros(nmbOfSatellites, 4);
omc = arma::zeros(nmbOfSatellites, 1);
az = arma::zeros(1, nmbOfSatellites);
el = arma::zeros(1, nmbOfSatellites);
arma::mat X = satpos;
arma::vec Rot_X;
double rho2;
double traveltime;
double trop = 0.0;
double dlambda;
double dphi;
double h;
arma::mat mat_tmp;
arma::vec x;
//=== Iteratively find receiver position ===================================
for (int iter = 0; iter < nmbOfIterations; iter++)
{
for (int i = 0; i < nmbOfSatellites; i++)
{
if (iter == 0)
{
//--- Initialize variables at the first iteration --------------
Rot_X = X.col(i); //Armadillo
trop = 0.0;
}
else
{
//--- Update equations -----------------------------------------
rho2 = (X(0, i) - pos(0)) *
(X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
(X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
(X(2, i) - pos(2));
traveltime = sqrt(rho2) / GPS_C_m_s;
//--- Correct satellite position (do to earth rotation) --------
Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
//--- Find DOA and range of satellites
Ls_Pvt::topocent(&d_visible_satellites_Az[i],
&d_visible_satellites_El[i],
&d_visible_satellites_Distance[i],
pos.subvec(0,2),
Rot_X - pos.subvec(0, 2));
if(traveltime < 0.1 && nmbOfSatellites > 3)
{
//--- Find receiver's height
Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
//--- Find delay due to troposphere (in meters)
Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
if(trop > 50.0 ) trop = 0.0;
}
}
//--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
//--- Construct the A matrix ---------------------------------------
//Armadillo
A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
A(i,3) = 1.0;
}
//--- Find position update ---------------------------------------------
x = arma::solve(w*A, w*omc); // Armadillo
//--- Apply position update --------------------------------------------
pos = pos + x;
if (arma::norm(x,2) < 1e-4)
{
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
}
}
try
{
//-- compute the Dilution Of Precision values
d_Q = arma::inv(arma::htrans(A)*A);
}
catch(std::exception& e)
{
d_Q = arma::zeros(4,4);
}
return pos;
}
arma::vec Ls_Pvt::rotateSatellite(double const traveltime, const arma::vec & X_sat)
{
/*
* Returns rotated satellite ECEF coordinates due to Earth
* rotation during signal travel time
*
* Inputs:
* travelTime - signal travel time
* X_sat - satellite's ECEF coordinates
*
* Returns:
* X_sat_rot - rotated satellite's coordinates (ECEF)
*/
//--- Find rotation angle --------------------------------------------------
double omegatau;
omegatau = OMEGA_EARTH_DOT * traveltime;
//--- Build a rotation matrix ----------------------------------------------
arma::mat R3 = arma::zeros(3,3);
R3(0, 0) = cos(omegatau);
R3(0, 1) = sin(omegatau);
R3(0, 2) = 0.0;
R3(1, 0) = -sin(omegatau);
R3(1, 1) = cos(omegatau);
R3(1, 2) = 0.0;
R3(2, 0) = 0.0;
R3(2, 1) = 0.0;
R3(2, 2) = 1;
//--- Do the rotation ------------------------------------------------------
arma::vec X_sat_rot;
X_sat_rot = R3 * X_sat;
return X_sat_rot;
}
void Ls_Pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{
/* Conversion of Cartesian coordinates (X,Y,Z) to geographical
coordinates (latitude, longitude, h) on a selected reference ellipsoid.
Choices of Reference Ellipsoid for Geographical Coordinates
0. International Ellipsoid 1924
1. International Ellipsoid 1967
2. World Geodetic System 1972
3. Geodetic Reference System 1980
4. World Geodetic System 1984
*/
const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
double lambda = atan2(Y, X);
double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1;
double oldh = 0.0;
double N;
int iterations = 0;
do
{
oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h) ))));
h = sqrt(X * X + Y * Y) / cos(phi) - N;
iterations = iterations + 1;
if (iterations > 100)
{
LOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
break;
}
}
while (std::abs(h - oldh) > 1.0e-12);
d_latitude_d = phi * 180.0 / GPS_PI;
d_longitude_d = lambda * 180.0 / GPS_PI;
d_height_m = h;
}
void Ls_Pvt::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;
}
*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;
}
void Ls_Pvt::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:
sinel - sin of elevation angle of satellite
hsta_km - height of station in km
p_mb - atmospheric pressure in mb at height hp_km
t_kel - surface temperature in degrees Kelvin at height htkel_km
hum - humidity in % at height hhum_km
hp_km - height of pressure measurement in km
htkel_km - height of temperature measurement in km
hhum_km - height of humidity measurement in km
Outputs:
ddr_m - range correction (meters)
Reference
Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric
Refraction Correction Model. Paper presented at the
American Geophysical Union Annual Fall Meeting, San
Francisco, December 12-17
Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
*/
const double a_e = 6378.137; // semi-major axis of earth ellipsoid
const double b0 = 7.839257e-5;
const double tlapse = -6.5;
const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5);
double tkhum = t_kel + tlapse * (hhum_km - htkel_km);
double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15);
double e0 = 0.0611 * hum * pow(10, atkel);
double tksea = t_kel - tlapse * htkel_km;
double tkelh = tksea + tlapse * hhum_km;
double e0sea = e0 * pow((tksea / tkelh), (4 * em));
double tkelp = tksea + tlapse * hp_km;
double psea = p_mb * pow((tksea / tkelp), em);
if(sinel < 0) { sinel = 0.0; }
double tropo_delay = 0.0;
bool done = false;
double refsea = 77.624e-6 / tksea;
double htop = 1.1385e-5 / refsea;
refsea = refsea * psea;
double ref = refsea * pow(((htop - hsta_km) / htop), 4);
double a;
double b;
double rtop;
while(1)
{
rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2));
// check to see if geometry is crazy
if(rtop < 0) { rtop = 0; }
rtop = sqrt(rtop) - (a_e + hsta_km) * sinel;
a = -sinel / (htop - hsta_km);
b = -b0 * (1 - pow(sinel,2)) / (htop - hsta_km);
arma::vec rn = arma::vec(8);
rn.zeros();
for(int i = 0; i<8; i++)
{
rn(i) = pow(rtop, (i+1+1));
}
arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b /3, a * (pow(a, 2) + 3 * b),
pow(a, 4)/5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b)/3,
pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0};
if(pow(b, 2) > 1.0e-35)
{
alpha(6) = a * pow(b, 3) /2;
alpha(7) = pow(b, 4) / 9;
}
double dr = rtop;
arma::mat aux_ = alpha * rn;
dr = dr + aux_(0, 0);
tropo_delay = tropo_delay + dr * ref * 1000;
if(done == true)
{
*ddr_m = tropo_delay;
break;
}
done = true;
refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea;
htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea;
ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4);
}
}
void Ls_Pvt::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
Ls_Pvt::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 = arma::zeros(3,3);
F(0,0) = -sl;
F(0,1) = -sb * cl;
F(0,2) = cb * cl;
F(1,0) = cl;
F(1,1) = -sb * sl;
F(1,2) = cb * sl;
F(2,0) = 0;
F(2,1) = cb;
F(2,2) = 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));
}
int Ls_Pvt::compute_DOP()
{
// ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat F = arma::zeros(3,3);
F(0,0) = -sin(GPS_TWO_PI * (d_longitude_d/360.0));
F(0,1) = -sin(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0));
F(0,2) = cos(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0));
F(1,0) = cos((GPS_TWO_PI * d_longitude_d)/360.0);
F(1,1) = -sin((GPS_TWO_PI * d_latitude_d)/360.0) * sin((GPS_TWO_PI * d_longitude_d)/360.0);
F(1,2) = cos((GPS_TWO_PI * d_latitude_d/360.0)) * sin((GPS_TWO_PI * d_longitude_d)/360.0);
F(2,0) = 0;
F(2,1) = cos((GPS_TWO_PI * d_latitude_d)/360.0);
F(2,2) = sin((GPS_TWO_PI * d_latitude_d/360.0));
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
arma::mat DOP_ENU = arma::zeros(3, 3);
try
{
DOP_ENU = arma::htrans(F) * Q_ECEF * F;
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
d_PDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1) + DOP_ENU(2, 2));// PDOP
d_HDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2, 2)); // VDOP
d_TDOP = sqrt(d_Q(3, 3)); // TDOP
}
catch(std::exception& ex)
{
d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
return 0;
}
int Ls_Pvt::set_averaging_depth(int depth)
{
d_averaging_depth = depth;
return 0;
}

176
src/algorithms/PVT/libs/ls_pvt.h Normal file
View File

@ -0,0 +1,176 @@
/*!
* \file ls_pvt.h
* \brief Interface of a base class for Least Squares PVT solutions
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_LS_PVT_H_
#define GNSS_SDR_LS_PVT_H_
#include <deque>
#include <armadillo>
#include <boost/date_time/posix_time/posix_time.hpp>
#define PVT_MAX_CHANNELS 24
/*!
* \brief Base class for the Least Squares PVT solution
*
*/
class Ls_Pvt
{
public:
Ls_Pvt();
arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w);
arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat);
int d_valid_observations; //!< Number of valid pseudorange observations (valid satellites)
int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS]; //!< Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //!< Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //!< Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
boost::posix_time::ptime d_position_UTC_time;
bool b_valid_position;
double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //!< Longitude in degrees
double d_height_m; //!< Height [m]
//averaging
int d_averaging_depth; //!< Length of averaging window
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_height_m; //!< Averaged height [m]
double d_x_m;
double d_y_m;
double d_z_m;
// DOP estimations
arma::mat d_Q;
double d_GDOP;
double d_PDOP;
double d_HDOP;
double d_VDOP;
double d_TDOP;
int compute_DOP(); //!< Compute Dilution Of Precision
bool d_flag_averaging;
int set_averaging_depth(int depth);
/*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical
* coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid.
*
* \param[in] X [m] Cartesian coordinate
* \param[in] Y [m] Cartesian coordinate
* \param[in] Z [m] Cartesian coordinate
* \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates:
* 0 - International Ellipsoid 1924.
* 1 - International Ellipsoid 1967.
* 2 - World Geodetic System 1972.
* 3 - Geodetic Reference System 1980.
* 4 - World Geodetic System 1984.
*
*/
void 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
*/
void 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
*/
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
/*!
* \brief Tropospheric correction
*
* \param[in] sinel - sin of elevation angle of satellite
* \param[in] hsta_km - height of station in km
* \param[in] p_mb - atmospheric pressure in mb at height hp_km
* \param[in] t_kel - surface temperature in degrees Kelvin at height htkel_km
* \param[in] hum - humidity in % at height hhum_km
* \param[in] hp_km - height of pressure measurement in km
* \param[in] htkel_km - height of temperature measurement in km
* \param[in] hhum_km - height of humidity measurement in km
*
* \param[out] ddr_m - range correction (meters)
*
*
* Reference:
* Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric
* Refraction Correction Model. Paper presented at the
* American Geophysical Union Annual Fall Meeting, San
* Francisco, December 12-17
*
* Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
*/
void 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);
};
#endif

2
src/algorithms/PVT/libs/nmea_printer.cc
View File

@ -132,7 +132,7 @@ void Nmea_Printer::close_serial ()
}
bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr<gps_l1_ca_ls_pvt>& pvt_data, bool print_average_values)
bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr<Ls_Pvt>& pvt_data, bool print_average_values)
{
std::string GPRMC;
std::string GPGGA;

6
src/algorithms/PVT/libs/nmea_printer.h
View File

@ -40,7 +40,7 @@
#include <iostream>
#include <fstream>
#include <string>
#include "gps_l1_ca_ls_pvt.h"
#include "ls_pvt.h"
/*!
@ -60,7 +60,7 @@ public:
/*!
* \brief Print NMEA PVT and satellite info to the initialized device
*/
bool Print_Nmea_Line(const std::shared_ptr<gps_l1_ca_ls_pvt>& position, bool print_average_values);
bool Print_Nmea_Line(const std::shared_ptr<Ls_Pvt>& position, bool print_average_values);
/*!
* \brief Default destructor.
@ -72,7 +72,7 @@ private:
std::ofstream nmea_file_descriptor; // Output file stream for NMEA log file
std::string nmea_devname;
int nmea_dev_descriptor; // NMEA serial device descriptor (i.e. COM port)
std::shared_ptr<gps_l1_ca_ls_pvt> d_PVT_data;
std::shared_ptr<Ls_Pvt> d_PVT_data;
int init_serial(std::string serial_device); //serial port control
void close_serial();
std::string get_GPGGA(); // fix data