mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 12:10:34 +00:00
Code Issues Releases Wiki Activity

Refactoring PVT solution library and adding a GeoJSON format printer

Browse Source
This commit is contained in:
Carles Fernandez 2015-11-14 20:41:28 +01:00
parent 4aac371bbf
commit f68a1fe9bc
20 changed files with 829 additions and 764 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -77,6 +77,13 @@ galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr<g
d_kml_dump = std::make_shared<Kml_Printer>();
d_kml_dump->set_headers(kml_dump_filename);
//initialize geojson_printer
std::string geojson_dump_filename;
geojson_dump_filename = d_dump_filename;
geojson_dump_filename.append(".geojson");
d_geojson_printer = std::make_shared<GeoJSON_Printer>();
d_geojson_printer->set_headers(geojson_dump_filename);
//initialize nmea_printer
d_nmea_printer = std::make_shared<Nmea_Printer>(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname);
d_dump_filename.append("_raw.dat");
@ -223,9 +230,9 @@ int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_it
if (pvt_result == true)
{
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);
//
d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging);
d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
if (!b_rinex_header_writen)
{
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;

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

@ -46,6 +46,7 @@
#include "nmea_printer.h"
#include "kml_printer.h"
#include "rinex_printer.h"
#include "geojson_printer.h"
#include "galileo_e1_ls_pvt.h"
#include "Galileo_E1.h"
@ -108,6 +109,8 @@ private:
long unsigned int d_last_sample_nav_output;
std::shared_ptr<Kml_Printer> d_kml_dump;
std::shared_ptr<Nmea_Printer> d_nmea_printer;
std::shared_ptr<GeoJSON_Printer> d_geojson_printer;
double d_rx_time;
std::shared_ptr<galileo_e1_ls_pvt> d_ls_pvt;
bool pseudoranges_pairCompare_min(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b);

14
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
View File

@ -87,8 +87,15 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels,
std::string kml_dump_filename;
kml_dump_filename = d_dump_filename;
kml_dump_filename.append(".kml");
d_kml_dump = std::make_shared<Kml_Printer>();
d_kml_dump->set_headers(kml_dump_filename);
d_kml_printer = std::make_shared<Kml_Printer>();
d_kml_printer->set_headers(kml_dump_filename);
//initialize geojson_printer
std::string geojson_dump_filename;
geojson_dump_filename = d_dump_filename;
geojson_dump_filename.append(".geojson");
d_geojson_printer = std::make_shared<GeoJSON_Printer>();
d_geojson_printer->set_headers(geojson_dump_filename);
//initialize nmea_printer
d_nmea_printer = std::make_shared<Nmea_Printer>(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname);
@ -261,7 +268,8 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging);
if (pvt_result == true)
{
d_kml_dump->print_position(d_ls_pvt, d_flag_averaging);
d_kml_printer->print_position(d_ls_pvt, d_flag_averaging);
d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging);
d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
if (!b_rinex_header_writen)

4
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h
View File

@ -45,6 +45,7 @@
#include "nmea_printer.h"
#include "kml_printer.h"
#include "rinex_printer.h"
#include "geojson_printer.h"
#include "gps_l1_ca_ls_pvt.h"
#include "GPS_L1_CA.h"
@ -107,8 +108,9 @@ private:
int d_display_rate_ms;
long unsigned int d_sample_counter;
long unsigned int d_last_sample_nav_output;
std::shared_ptr<Kml_Printer> d_kml_dump;
std::shared_ptr<Kml_Printer> d_kml_printer;
std::shared_ptr<Nmea_Printer> d_nmea_printer;
std::shared_ptr<GeoJSON_Printer> d_geojson_printer;
double d_rx_time;
std::shared_ptr<gps_l1_ca_ls_pvt> d_ls_pvt;

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

@ -81,6 +81,13 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_q
d_kml_dump = std::make_shared<Kml_Printer>();
d_kml_dump->set_headers(kml_dump_filename);
//initialize geojson_printer
std::string geojson_dump_filename;
geojson_dump_filename = d_dump_filename;
geojson_dump_filename.append(".geojson");
d_geojson_printer = std::make_shared<GeoJSON_Printer>();
d_geojson_printer->set_headers(geojson_dump_filename);
//initialize nmea_printer
d_nmea_printer = std::make_shared<Nmea_Printer>(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname);
@ -279,9 +286,9 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
if (pvt_result == true)
{
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);
//
d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging);
d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
if (!b_rinex_header_writen) // & we have utc data in nav message!
{
std::map<int, Galileo_Ephemeris>::iterator galileo_ephemeris_iter;

3
src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h
View File

@ -49,7 +49,9 @@
#include "gps_iono.h"
#include "nmea_printer.h"
#include "kml_printer.h"
#include "geojson_printer.h"
#include "rinex_printer.h"
#include "nmea_printer.h"
#include "hybrid_ls_pvt.h"
#include "GPS_L1_CA.h"
#include "Galileo_E1.h"
@ -113,6 +115,7 @@ private:
long unsigned int d_last_sample_nav_output;
std::shared_ptr<Kml_Printer> d_kml_dump;
std::shared_ptr<Nmea_Printer> d_nmea_printer;
std::shared_ptr<GeoJSON_Printer> d_geojson_printer;
double d_rx_time;
double d_TOW_at_curr_symbol_constellation;
std::shared_ptr<hybrid_ls_pvt> d_ls_pvt;

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

@ -19,6 +19,7 @@
add_definitions( -DGNSS_SDR_VERSION="${VERSION}" )
set(PVT_LIB_SOURCES
pvt_solution.cc
ls_pvt.cc
gps_l1_ca_ls_pvt.cc
galileo_e1_ls_pvt.cc

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

@ -251,60 +251,12 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
}
// MOVING AVERAGE PVT
if (flag_averaging == true)
{
if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth)
{
// Pop oldest value
d_hist_longitude_d.pop_back();
d_hist_latitude_d.pop_back();
d_hist_height_m.pop_back();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = 0;
d_avg_longitude_d = 0;
d_avg_height_m = 0;
for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++)
{
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
}
d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth);
d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
d_avg_height_m = d_avg_height_m / static_cast<double>(d_averaging_depth);
b_valid_position = true;
return true; //indicates that the returned position is valid
}
else
{
// int current_depth=d_hist_longitude_d.size();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = d_latitude_d;
d_avg_longitude_d = d_longitude_d;
d_avg_height_m = d_height_m;
b_valid_position = false;
return false; //indicates that the returned position is not valid yet
}
}
else
{
b_valid_position = true;
return true; //indicates that the returned position is valid
}
galileo_e1_ls_pvt::pos_averaging(flag_averaging);
}
else
{
b_valid_position = false;
return false;
}
return false;
return b_valid_position;
}

33
src/algorithms/PVT/libs/geojson_printer.cc
View File

@ -48,14 +48,24 @@ GeoJSON_Printer::~GeoJSON_Printer ()
bool GeoJSON_Printer::set_headers(std::string filename)
{
geojson_file.open(filename.c_str());
filename_ = filename;
first_pos = true;
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);
// Writing the header
geojson_file << "{" << std::endl;
geojson_file << " \"type\": \"Feature\"," << std::endl;
geojson_file << " \"properties\": {" << std::endl;
geojson_file << " \"name\": \"Locations generated by GNSS-SDR\" " << std::endl;
geojson_file << " }," << std::endl;
geojson_file << " \"geometry\": {" << std::endl;
geojson_file << " \"type\": \"MultiPoint\"," << std::endl;
geojson_file << " \"coordinates\": [" << std::endl;
@ -70,13 +80,13 @@ bool GeoJSON_Printer::set_headers(std::string filename)
bool GeoJSON_Printer::print_position(const std::shared_ptr<Ls_Pvt>& position, bool print_average_values)
bool GeoJSON_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values)
{
double latitude;
double longitude;
double height;
std::shared_ptr<Ls_Pvt> position_ = position;
std::shared_ptr<Pvt_Solution> position_ = position;
if (print_average_values == false)
{
@ -93,7 +103,16 @@ bool GeoJSON_Printer::print_position(const std::shared_ptr<Ls_Pvt>& position, bo
if (geojson_file.is_open())
{
geojson_file << " [" << longitude << ", " << latitude << ", " << height << "]," << std::endl;
if (first_pos == true)
{
geojson_file << " [" << longitude << ", " << latitude << ", " << height << "]";
first_pos = false;
}
else
{
geojson_file << "," << std::endl;
geojson_file << " [" << longitude << ", " << latitude << ", " << height << "]";
}
return true;
}
else
@ -108,10 +127,18 @@ bool GeoJSON_Printer::close_file()
{
if (geojson_file.is_open())
{
geojson_file << std::endl;
geojson_file << " ]" << std::endl;
geojson_file << " }" << std::endl;
geojson_file << "}" << std::endl;
geojson_file.close();
// if nothing is written, erase the file
if (first_pos == true)
{
if(remove(filename_.c_str()) != 0) LOG(INFO) << "Error deleting temporary file";
}
return true;
}
else

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

@ -37,7 +37,7 @@
#include <fstream>
#include <memory>
#include <string>
#include "ls_pvt.h"
#include "pvt_solution.h"
/*!
@ -49,11 +49,13 @@ class GeoJSON_Printer
{
private:
std::ofstream geojson_file;
bool first_pos;
std::string filename_;
public:
GeoJSON_Printer();
~GeoJSON_Printer();
bool set_headers(std::string filename);
bool print_position(const std::shared_ptr<Ls_Pvt>& position, bool print_average_values);
bool print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values);
bool close_file();
};

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

@ -228,60 +228,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
}
// MOVING AVERAGE PVT
if (flag_averaging == true)
{
if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth)
{
// Pop oldest value
d_hist_longitude_d.pop_back();
d_hist_latitude_d.pop_back();
d_hist_height_m.pop_back();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = 0;
d_avg_longitude_d = 0;
d_avg_height_m = 0;
for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++)
{
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
}
d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth);
d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
d_avg_height_m = d_avg_height_m / static_cast<double>(d_averaging_depth);
b_valid_position = true;
return true; //indicates that the returned position is valid
}
else
{
//int current_depth=d_hist_longitude_d.size();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = d_latitude_d;
d_avg_longitude_d = d_longitude_d;
d_avg_height_m = d_height_m;
b_valid_position = false;
return false; //indicates that the returned position is not valid yet
}
}
else
{
b_valid_position = true;
return true; //indicates that the returned position is valid
}
gps_l1_ca_ls_pvt::pos_averaging(flag_averaging);
}
else
{
b_valid_position = false;
return false;
}
return b_valid_position;
}

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

@ -331,60 +331,12 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
}
// MOVING AVERAGE PVT
if (flag_averaging == true)
{
if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth)
{
// Pop oldest value
d_hist_longitude_d.pop_back();
d_hist_latitude_d.pop_back();
d_hist_height_m.pop_back();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = 0;
d_avg_longitude_d = 0;
d_avg_height_m = 0;
for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++)
{
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
}
d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth);
d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
d_avg_height_m = d_avg_height_m / static_cast<double>(d_averaging_depth);
b_valid_position = true;
return true; //indicates that the returned position is valid
}
else
{
// int current_depth=d_hist_longitude_d.size();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = d_latitude_d;
d_avg_longitude_d = d_longitude_d;
d_avg_height_m = d_height_m;
b_valid_position = false;
return false; //indicates that the returned position is not valid yet
}
}
else
{
b_valid_position = true;
return true; //indicates that the returned position is valid
}
hybrid_ls_pvt::pos_averaging(flag_averaging);
}
else
{
b_valid_position = false;
return false;
}
return false;
return b_valid_position;
}

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

@ -82,13 +82,13 @@ bool Kml_Printer::set_headers(std::string filename)
bool Kml_Printer::print_position(const std::shared_ptr<Ls_Pvt>& position, bool print_average_values)
bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values)
{
double latitude;
double longitude;
double height;
std::shared_ptr<Ls_Pvt> position_ = position;
std::shared_ptr<Pvt_Solution> position_ = position;
if (print_average_values == false)
{

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

@ -37,7 +37,7 @@
#include <fstream>
#include <memory>
#include <string>
#include "ls_pvt.h"
#include "pvt_solution.h"
/*!
* \brief Prints PVT information to OGC KML format file (can be viewed with Google Earth)
@ -52,7 +52,7 @@ public:
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 print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values);
bool close_file();
};

453
src/algorithms/PVT/libs/ls_pvt.cc
View File

@ -1,5 +1,5 @@
/*!
* \file ls_pvt.h
* \file ls_pvt.cc
* \brief Implementation of a base class for Least Squares PVT solutions
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
*
@ -38,30 +38,15 @@
using google::LogMessage;
DEFINE_bool(tropo, true, "Apply tropospheric correction");
//DEFINE_bool(tropo, true, "Apply tropospheric correction");
Ls_Pvt::Ls_Pvt()
Ls_Pvt::Ls_Pvt() : Pvt_Solution()
{
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)
@ -173,435 +158,3 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
}
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;
}

127
src/algorithms/PVT/libs/ls_pvt.h
View File

@ -33,144 +33,21 @@
#define GNSS_SDR_LS_PVT_H_
#include <deque>
#include <armadillo>
#include <boost/date_time/posix_time/posix_time.hpp>
#define PVT_MAX_CHANNELS 24
#include "pvt_solution.h"
/*!
* \brief Base class for the Least Squares PVT solution
*
*/
class Ls_Pvt
class Ls_Pvt : public Pvt_Solution
{
public:
Ls_Pvt();
arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w);
arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat);
int d_valid_observations; //!< Number of valid pseudorange observations (valid satellites)
int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS]; //!< Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //!< Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //!< Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
boost::posix_time::ptime d_position_UTC_time;
bool b_valid_position;
double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //!< Longitude in degrees
double d_height_m; //!< Height [m]
//averaging
int d_averaging_depth; //!< Length of averaging window
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_height_m; //!< Averaged height [m]
double d_x_m;
double d_y_m;
double d_z_m;
// DOP estimations
arma::mat d_Q;
double d_GDOP;
double d_PDOP;
double d_HDOP;
double d_VDOP;
double d_TDOP;
int compute_DOP(); //!< Compute Dilution Of Precision
bool d_flag_averaging;
int set_averaging_depth(int depth);
/*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical
* coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid.
*
* \param[in] X [m] Cartesian coordinate
* \param[in] Y [m] Cartesian coordinate
* \param[in] Z [m] Cartesian coordinate
* \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates:
* 0 - International Ellipsoid 1924.
* 1 - International Ellipsoid 1967.
* 2 - World Geodetic System 1972.
* 3 - Geodetic Reference System 1980.
* 4 - World Geodetic System 1984.
*
*/
void cart2geo(double X, double Y, double Z, int elipsoid_selection);
/*!
* \brief Transformation of vector dx into topocentric coordinate system with origin at x
*
* \param[in] x Vector origin coordinates (in ECEF system [X; Y; Z;])
* \param[in] dx Vector ([dX; dY; dZ;]).
*
* \param[out] D Vector length. Units like the input
* \param[out] Az Azimuth from north positive clockwise, degrees
* \param[out] El Elevation angle, degrees
*
* Based on a Matlab function by Kai Borre
*/
void topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx);
/*!
* \brief Subroutine to calculate geodetic coordinates latitude, longitude,
* height given Cartesian coordinates X,Y,Z, and reference ellipsoid
* values semi-major axis (a) and the inverse of flattening (finv).
*
* The output units of angular quantities will be in decimal degrees
* (15.5 degrees not 15 deg 30 min). The output units of h will be the
* same as the units of X,Y,Z,a.
*
* \param[in] a - semi-major axis of the reference ellipsoid
* \param[in] finv - inverse of flattening of the reference ellipsoid
* \param[in] X,Y,Z - Cartesian coordinates
*:
* \param[out] dphi - latitude
* \param[out] dlambda - longitude
* \param[out] h - height above reference ellipsoid
*
* Based in a Matlab function by Kai Borre
*/
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
/*!
* \brief Tropospheric correction
*
* \param[in] sinel - sin of elevation angle of satellite
* \param[in] hsta_km - height of station in km
* \param[in] p_mb - atmospheric pressure in mb at height hp_km
* \param[in] t_kel - surface temperature in degrees Kelvin at height htkel_km
* \param[in] hum - humidity in % at height hhum_km
* \param[in] hp_km - height of pressure measurement in km
* \param[in] htkel_km - height of temperature measurement in km
* \param[in] hhum_km - height of humidity measurement in km
*
* \param[out] ddr_m - range correction (meters)
*
*
* Reference:
* Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric
* Refraction Correction Model. Paper presented at the
* American Geophysical Union Annual Fall Meeting, San
* Francisco, December 12-17
*
* Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
*/
void tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km);
};
#endif

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

@ -39,7 +39,7 @@
#include <boost/date_time/posix_time/posix_time.hpp>
#include <glog/logging.h>
#include <gflags/gflags.h>
#include "GPS_L1_CA.h"
using google::LogMessage;
@ -132,7 +132,7 @@ void Nmea_Printer::close_serial ()
}
bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr<Ls_Pvt>& pvt_data, bool print_average_values)
bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr<Pvt_Solution>& pvt_data, bool print_average_values)
{
std::string GPRMC;
std::string GPGGA;

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

@ -40,7 +40,7 @@
#include <iostream>
#include <fstream>
#include <string>
#include "ls_pvt.h"
#include "pvt_solution.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<Ls_Pvt>& position, bool print_average_values);
bool Print_Nmea_Line(const std::shared_ptr<Pvt_Solution>& 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<Ls_Pvt> d_PVT_data;
std::shared_ptr<Pvt_Solution> d_PVT_data;
int init_serial(std::string serial_device); //serial port control
void close_serial();
std::string get_GPGGA(); // fix data

548
src/algorithms/PVT/libs/pvt_solution.cc Normal file
View File

@ -0,0 +1,548 @@
/*!
* \file pvt_solution.cc
* \brief Implementation of a base class for a PVT solution
* \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 "pvt_solution.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");
Pvt_Solution::Pvt_Solution()
{
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_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;
d_valid_observations = 0;
}
arma::vec Pvt_Solution::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;
}
int Pvt_Solution::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;
return 0;
}
int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
{
/* Subroutine to calculate geodetic coordinates latitude, longitude,
height given Cartesian coordinates X,Y,Z, and reference ellipsoid
values semi-major axis (a) and the inverse of flattening (finv).
The output units of angular quantities will be in decimal degrees
(15.5 degrees not 15 deg 30 min). The output units of h will be the
same as the units of X,Y,Z,a.
Inputs:
a - semi-major axis of the reference ellipsoid
finv - inverse of flattening of the reference ellipsoid
X,Y,Z - Cartesian coordinates
Outputs:
dphi - latitude
dlambda - longitude
h - height above reference ellipsoid
Based in a Matlab function by Kai Borre
*/
*h = 0;
double tolsq = 1.e-10; // tolerance to accept convergence
int maxit = 10; // max number of iterations
double rtd = 180.0 / GPS_PI;
// compute square of eccentricity
double esq;
if (finv < 1.0E-20)
{
esq = 0.0;
}
else
{
esq = (2.0 - 1.0 / finv) / finv;
}
// first guess
double P = sqrt(X * X + Y * Y); // P is distance from spin axis
//direct calculation of longitude
if (P > 1.0E-20)
{
*dlambda = atan2(Y, X) * rtd;
}
else
{
*dlambda = 0.0;
}
// correct longitude bound
if (*dlambda < 0)
{
*dlambda = *dlambda + 360.0;
}
double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0)
double sinphi;
if (r > 1.0E-20)
{
sinphi = Z/r;
}
else
{
sinphi = 0.0;
}
*dphi = asin(sinphi);
// initial value of height = distance from origin minus
// approximate distance from origin to surface of ellipsoid
if (r < 1.0E-20)
{
*h = 0;
return 1;
}
*h = r - a * (1 - sinphi * sinphi/finv);
// iterate
double cosphi;
double N_phi;
double dP;
double dZ;
double oneesq = 1.0 - esq;
for (int i = 0; i < maxit; i++)
{
sinphi = sin(*dphi);
cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq * sinphi * sinphi);
// compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
// update height and latitude
*h = *h + (sinphi * dZ + cosphi * dP);
*dphi = *dphi + (cosphi * dZ - sinphi * dP)/(N_phi + (*h));
// test for convergence
if ((dP * dP + dZ * dZ) < tolsq)
{
break;
}
if (i == (maxit - 1))
{
LOG(WARNING) << "The computation of geodetic coordinates did not converge";
}
}
*dphi = (*dphi) * rtd;
return 0;
}
int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km)
{
/* 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);
}
return 0;
}
int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx)
{
/* Transformation of vector dx into topocentric coordinate
system with origin at x
Inputs:
x - vector origin coordinates (in ECEF system [X; Y; Z;])
dx - vector ([dX; dY; dZ;]).
Outputs:
D - vector length. Units like the input
Az - azimuth from north positive clockwise, degrees
El - elevation angle, degrees
Based on a Matlab function by Kai Borre
*/
double lambda;
double phi;
double h;
double dtr = GPS_PI / 180.0;
double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
// Transform x into geodetic coordinates
Pvt_Solution::togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
arma::mat F = 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));
return 0;
}
int Pvt_Solution::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 Pvt_Solution::set_averaging_depth(int depth)
{
d_averaging_depth = depth;
return 0;
}
int Pvt_Solution::pos_averaging(bool flag_averaring)
{
// MOVING AVERAGE PVT
bool avg = flag_averaring;
if (avg == true)
{
if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth)
{
// Pop oldest value
d_hist_longitude_d.pop_back();
d_hist_latitude_d.pop_back();
d_hist_height_m.pop_back();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = 0;
d_avg_longitude_d = 0;
d_avg_height_m = 0;
for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++)
{
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
}
d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth);
d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
d_avg_height_m = d_avg_height_m / static_cast<double>(d_averaging_depth);
b_valid_position = true;
}
else
{
//int current_depth=d_hist_longitude_d.size();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = d_latitude_d;
d_avg_longitude_d = d_longitude_d;
d_avg_height_m = d_height_m;
b_valid_position = false;
}
}
else
{
b_valid_position = true;
}
return 0;
}

170
src/algorithms/PVT/libs/pvt_solution.h Normal file
View File

@ -0,0 +1,170 @@
/*!
* \file pvt_solution.h
* \brief Interface of a base class for a PVT solution
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_PVT_SOLUTION_H_
#define GNSS_SDR_PVT_SOLUTION_H_
#include <deque>
#include <armadillo>
#include <boost/date_time/posix_time/posix_time.hpp>
#define PVT_MAX_CHANNELS 24
/*!
* \brief Base class for a PVT solution
*
*/
class Pvt_Solution
{
public:
Pvt_Solution();
double d_latitude_d;
double d_longitude_d;
double d_height_m;
boost::posix_time::ptime d_position_UTC_time;
bool b_valid_position;
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
//averaging
int d_averaging_depth; //!< Length of averaging window
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_height_m; //!< Averaged height [m]
int pos_averaging(bool flag_averaging);
// DOP estimations
arma::mat d_Q;
double d_GDOP;
double d_PDOP;
double d_HDOP;
double d_VDOP;
double d_TDOP;
int compute_DOP(); //!< Compute Dilution Of Precision
bool d_flag_averaging;
int set_averaging_depth(int depth);
arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat);
/*!
* \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.
*
*/
int cart2geo(double X, double Y, double Z, int elipsoid_selection);
/*!
* \brief Transformation of vector dx into topocentric coordinate system with origin at x
*
* \param[in] x Vector origin coordinates (in ECEF system [X; Y; Z;])
* \param[in] dx Vector ([dX; dY; dZ;]).
*
* \param[out] D Vector length. Units like the input
* \param[out] Az Azimuth from north positive clockwise, degrees
* \param[out] El Elevation angle, degrees
*
* Based on a Matlab function by Kai Borre
*/
int topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx);
/*!
* \brief Subroutine to calculate geodetic coordinates latitude, longitude,
* height given Cartesian coordinates X,Y,Z, and reference ellipsoid
* values semi-major axis (a) and the inverse of flattening (finv).
*
* The output units of angular quantities will be in decimal degrees
* (15.5 degrees not 15 deg 30 min). The output units of h will be the
* same as the units of X,Y,Z,a.
*
* \param[in] a - semi-major axis of the reference ellipsoid
* \param[in] finv - inverse of flattening of the reference ellipsoid
* \param[in] X,Y,Z - Cartesian coordinates
*
* \param[out] dphi - latitude
* \param[out] dlambda - longitude
* \param[out] h - height above reference ellipsoid
*
* Based in a Matlab function by Kai Borre
*/
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
/*!
* \brief Tropospheric correction
*
* \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
*/
int 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