[hamradio-commits] [gnss-sdr] 31/126: Refactoring least squares computation
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Sat Dec 26 18:37:58 UTC 2015
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch next
in repository gnss-sdr.
commit d52c3e36e3d08afbe760a0fd3b3b63801d35724f
Author: Carles Fernandez <carles.fernandez at gmail.com>
Date: Sat Nov 14 14:17:02 2015 +0100
Refactoring least squares computation
---
.../PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc | 2 +-
.../PVT/gnuradio_blocks/galileo_e1_pvt_cc.h | 1 -
.../PVT/gnuradio_blocks/hybrid_pvt_cc.cc | 2 +-
src/algorithms/PVT/libs/CMakeLists.txt | 4 +-
src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc | 549 +------------------
src/algorithms/PVT/libs/galileo_e1_ls_pvt.h | 86 +--
src/algorithms/PVT/libs/geojson_printer.cc | 126 +++++
.../PVT/libs/{kml_printer.h => geojson_printer.h} | 31 +-
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc | 553 +------------------
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h | 88 +--
src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 543 +-----------------
src/algorithms/PVT/libs/hybrid_ls_pvt.h | 81 +--
src/algorithms/PVT/libs/kml_printer.cc | 64 +--
src/algorithms/PVT/libs/kml_printer.h | 12 +-
src/algorithms/PVT/libs/ls_pvt.cc | 607 +++++++++++++++++++++
.../PVT/libs/{galileo_e1_ls_pvt.h => ls_pvt.h} | 126 +++--
src/algorithms/PVT/libs/nmea_printer.cc | 2 +-
src/algorithms/PVT/libs/nmea_printer.h | 6 +-
18 files changed, 876 insertions(+), 2007 deletions(-)
diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
index e991f5d..bd13684 100644
--- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
+++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
@@ -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);
//
diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h
index 2dfd1d9..02b910f 100644
--- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h
+++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h
@@ -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;
diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
index 3502429..601919e 100644
--- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
+++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
@@ -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);
//
diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt
index 2be1c45..47b2b16 100644
--- a/src/algorithms/PVT/libs/CMakeLists.txt
+++ b/src/algorithms/PVT/libs/CMakeLists.txt
@@ -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(
diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
index cf36f38..9dde4c9 100644
--- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
@@ -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);
- }
-}
diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.h b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.h
index 1119a22..2220847 100644
--- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.h
+++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.h
@@ -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();
- Galileo_Navigation_Message* d_ephemeris;
+ 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
diff --git a/src/algorithms/PVT/libs/geojson_printer.cc b/src/algorithms/PVT/libs/geojson_printer.cc
new file mode 100644
index 0000000..2d9f764
--- /dev/null
+++ b/src/algorithms/PVT/libs/geojson_printer.cc
@@ -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;
+ }
+}
+
+
diff --git a/src/algorithms/PVT/libs/kml_printer.h b/src/algorithms/PVT/libs/geojson_printer.h
similarity index 58%
copy from src/algorithms/PVT/libs/kml_printer.h
copy to src/algorithms/PVT/libs/geojson_printer.h
index a34426f..aabc994 100644
--- a/src/algorithms/PVT/libs/kml_printer.h
+++ b/src/algorithms/PVT/libs/geojson_printer.h
@@ -1,7 +1,7 @@
/*!
- * \file kml_printer.h
- * \brief Interface of a class that prints PVT information to a kml file
- * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ * \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
*
*
* -------------------------------------------------------------------------
@@ -30,34 +30,31 @@
*/
-#ifndef GNSS_SDR_KML_PRINTER_H_
-#define GNSS_SDR_KML_PRINTER_H_
+#ifndef GNSS_SDR_GEOJSON_PRINTER_H_
+#define GNSS_SDR_GEOJSON_PRINTER_H_
#include <iostream>
#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)
+ * \brief Prints PVT solutions in GeoJSON format file
*
- * See http://www.opengeospatial.org/standards/kml
+ * See http://geojson.org/geojson-spec.html
*/
-class Kml_Printer
+class GeoJSON_Printer
{
private:
- std::ofstream kml_file;
+ std::ofstream geojson_file;
public:
+ GeoJSON_Printer();
+ ~GeoJSON_Printer();
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 print_position(const std::shared_ptr<Ls_Pvt>& position, bool print_average_values);
bool close_file();
- Kml_Printer();
- ~Kml_Printer();
};
#endif
diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
index 6f56c08..e0e83be 100644
--- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
@@ -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;
// ############# 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);
- }
-}
diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h
index c7e3bab..ea9b4d4 100644
--- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h
+++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h
@@ -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
diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
index 22e6625..41d2801 100644
--- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
@@ -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);
- }
-}
diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h
index a989532..243d75c 100644
--- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h
+++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.h
@@ -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;
+ int count_valid_position;
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
diff --git a/src/algorithms/PVT/libs/kml_printer.cc b/src/algorithms/PVT/libs/kml_printer.cc
index 3ba2373..c74ba24 100644
--- a/src/algorithms/PVT/libs/kml_printer.cc
+++ b/src/algorithms/PVT/libs/kml_printer.cc
@@ -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()
{
diff --git a/src/algorithms/PVT/libs/kml_printer.h b/src/algorithms/PVT/libs/kml_printer.h
index a34426f..6f90d73 100644
--- a/src/algorithms/PVT/libs/kml_printer.h
+++ b/src/algorithms/PVT/libs/kml_printer.h
@@ -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
diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc
new file mode 100644
index 0000000..b78d198
--- /dev/null
+++ b/src/algorithms/PVT/libs/ls_pvt.cc
@@ -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;
+}
diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.h b/src/algorithms/PVT/libs/ls_pvt.h
similarity index 58%
copy from src/algorithms/PVT/libs/galileo_e1_ls_pvt.h
copy to src/algorithms/PVT/libs/ls_pvt.h
index 1119a22..eb923f2 100644
--- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.h
+++ b/src/algorithms/PVT/libs/ls_pvt.h
@@ -1,8 +1,8 @@
/*!
- * \file galileo_e1_ls_pvt.h
- * \brief Interface of a Least Squares Position, Velocity, and Time (PVT)
- * solver, based on K.Borre's Matlab receiver.
- * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ * \file ls_pvt.h
+ * \brief Interface of a base class for Least Squares PVT solutions
+ * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
+ *
*
* -------------------------------------------------------------------------
*
@@ -29,42 +29,28 @@
* -------------------------------------------------------------------------
*/
-#ifndef GNSS_SDR_GALILEO_E1_LS_PVT_H_
-#define GNSS_SDR_GALILEO_E1_LS_PVT_H_
+#ifndef GNSS_SDR_LS_PVT_H_
+#define GNSS_SDR_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 "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
+ * \brief Base class for the Least Squares PVT solution
+ *
*/
-class galileo_e1_ls_pvt
+class Ls_Pvt
{
-private:
+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);
- 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
@@ -72,14 +58,6 @@ public:
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_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;
@@ -89,10 +67,11 @@ public:
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;
- 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]
@@ -108,20 +87,12 @@ public:
double d_HDOP;
double d_VDOP;
double d_TDOP;
+ int compute_DOP(); //!< Compute Dilution Of Precision
- 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);
+ int 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
@@ -139,6 +110,67 @@ public:
*
*/
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
diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc
index 1274a9e..ceda216 100644
--- a/src/algorithms/PVT/libs/nmea_printer.cc
+++ b/src/algorithms/PVT/libs/nmea_printer.cc
@@ -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;
diff --git a/src/algorithms/PVT/libs/nmea_printer.h b/src/algorithms/PVT/libs/nmea_printer.h
index f7c5a3f..b613d45 100644
--- a/src/algorithms/PVT/libs/nmea_printer.h
+++ b/src/algorithms/PVT/libs/nmea_printer.h
@@ -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
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list