mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-25 22:43:14 +00:00
Code Issues Releases Wiki Activity

Remove old, unused code

Browse Source
This commit is contained in:
Carles Fernandez 2020-07-21 13:12:57 +02:00
parent d3654456e9
commit 399903e491
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
5 changed files with 10 additions and 991 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -10,34 +10,30 @@
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS ${CMAKE_SOURCE_DIR}/docs/protobuf/monitor_pvt.proto)
set(PVT_LIB_SOURCES
pvt_conf.cc
pvt_solution.cc
ls_pvt.cc
hybrid_ls_pvt.cc
kml_printer.cc
geojson_printer.cc
gpx_printer.cc
rinex_printer.cc
kml_printer.cc
nmea_printer.cc
rinex_printer.cc
rtcm_printer.cc
rtcm.cc
geojson_printer.cc
rtklib_solver.cc
pvt_conf.cc
monitor_pvt_udp_sink.cc
)
set(PVT_LIB_HEADERS
pvt_conf.h
pvt_solution.h
ls_pvt.h
hybrid_ls_pvt.h
kml_printer.h
geojson_printer.h
gpx_printer.h
rinex_printer.h
kml_printer.h
nmea_printer.h
rinex_printer.h
rtcm_printer.h
rtcm.h
geojson_printer.h
rtklib_solver.h
pvt_conf.h
monitor_pvt_udp_sink.h
monitor_pvt.h
serdes_monitor_pvt.h
@ -73,13 +69,11 @@ endif()
target_link_libraries(pvt_libs
PUBLIC
Armadillo::armadillo
Boost::date_time
protobuf::libprotobuf
core_system_parameters
algorithms_libs_rtklib
PRIVATE
algorithms_libs
Gflags::gflags
Glog::glog
Matio::matio
@ -91,16 +85,12 @@ target_include_directories(pvt_libs
PUBLIC
${CMAKE_SOURCE_DIR}/src/core/receiver
SYSTEM ${PROTO_INCLUDE_HEADERS}
PRIVATE
${CMAKE_SOURCE_DIR}/src/algorithms/libs # for gnss_sdr_make_unique.h
)
target_compile_definitions(pvt_libs PRIVATE -DGNSS_SDR_VERSION="${VERSION}")
if(ENABLE_ARMA_NO_DEBUG)
target_compile_definitions(pvt_libs
PUBLIC -DARMA_NO_BOUND_CHECKING=1
)
endif()
if(USE_BOOST_ASIO_IO_CONTEXT)
target_compile_definitions(pvt_libs
PUBLIC

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

@ -1,385 +0,0 @@
/*!
* \file hybrid_ls_pvt.cc
* \brief Implementation 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
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2019 (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.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
*/
#include "hybrid_ls_pvt.h"
#include "GPS_L2C.h"
#include "MATH_CONSTANTS.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <glog/logging.h>
#include <utility>
Hybrid_Ls_Pvt::Hybrid_Ls_Pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file)
{
// init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels;
d_dump_filename = std::move(dump_filename);
d_flag_dump_enabled = flag_dump_to_file;
d_galileo_current_time = 0;
this->set_averaging_flag(false);
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
}
}
}
}
Hybrid_Ls_Pvt::~Hybrid_Ls_Pvt()
{
if (d_dump_file.is_open() == true)
{
try
{
d_dump_file.close();
}
catch (const std::exception& ex)
{
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
}
}
}
bool Hybrid_Ls_Pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, double hybrid_current_time, bool flag_averaging)
{
std::map<int, Gnss_Synchro>::iterator gnss_observables_iter;
std::map<int, Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
std::map<int, Gps_Ephemeris>::iterator gps_ephemeris_iter;
std::map<int, Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
arma::vec W; // channels weight vector
arma::vec obs; // pseudoranges observation vector
arma::mat satpos; // satellite positions matrix
int Galileo_week_number = 0;
int GPS_week = 0;
double utc = 0.0;
double GST = 0.0;
double secondsperweek = 604800.0;
// double utc_tx_corrected = 0.0; //utc computed at tx_time_corrected, added for Galileo constellation (in GPS utc is directly computed at TX_time_corrected_s)
double TX_time_corrected_s = 0.0;
double SV_clock_bias_s = 0.0;
this->set_averaging_flag(flag_averaging);
// ********************************************************************************
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
// ********************************************************************************
int valid_obs = 0; // valid observations counter
for (gnss_observables_iter = gnss_observables_map.begin();
gnss_observables_iter != gnss_observables_map.end();
gnss_observables_iter++)
{
switch (gnss_observables_iter->second.System)
{
case 'E':
{
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W.resize(valid_obs + 1, 1);
W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / SPEED_OF_LIGHT_M_S;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
// store satellite positions in a matrix
satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * SPEED_OF_LIGHT_M_S - this->get_time_offset_s() * SPEED_OF_LIGHT_M_S;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; // for GST
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
valid_obs++;
}
else // the ephemeris are not available for this SV
{
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
}
break;
}
case 'G':
{
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
std::string sig_(gnss_observables_iter->second.Signal);
if (sig_ == "1C")
{
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_ephemeris_iter != gps_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W.resize(valid_obs + 1, 1);
W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / SPEED_OF_LIGHT_M_S;
// 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
// 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
// store satellite positions in a matrix
satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected pseudoranges
// compute code bias: TGD for single frequency
// See IS-GPS-200K section 20.3.3.3.3.2
double sqrt_Gamma = GPS_L1_FREQ_HZ / GPS_L2_FREQ_HZ;
double Gamma = sqrt_Gamma * sqrt_Gamma;
double P1_P2 = (1.0 - Gamma) * (gps_ephemeris_iter->second.d_TGD * SPEED_OF_LIGHT_M_S);
double Code_bias_m = P1_P2 / (1.0 - Gamma);
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * SPEED_OF_LIGHT_M_S - Code_bias_m - this->get_time_offset_s() * SPEED_OF_LIGHT_M_S;
// SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
<< " TX Time corrected=" << TX_time_corrected_s << " X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
valid_obs++;
// compute the UTC time for this SV (just to print the associated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
}
else // the ephemeris are not available for this SV
{
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
}
}
if (sig_ == "2S")
{
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W.resize(valid_obs + 1, 1);
W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / SPEED_OF_LIGHT_M_S;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
// std::cout<<"TX time["<<gps_cnav_ephemeris_iter->second.i_satellite_PRN<<"]="<<TX_time_corrected_s<< '\n';
double dtr = gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
// store satellite positions in a matrix
satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * SPEED_OF_LIGHT_M_S + SV_clock_bias_s * SPEED_OF_LIGHT_M_S;
GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
GPS_week = GPS_week % 1024; // Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV)
// SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF GPS L2M satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
<< " TX Time corrected=" << TX_time_corrected_s
<< " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
valid_obs++;
}
else // the ephemeris are not available for this SV
{
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
}
}
break;
}
default:
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
break;
}
}
// ********************************************************************************
// ****** SOLVE LEAST SQUARES******************************************************
// ********************************************************************************
this->set_num_valid_observations(valid_obs);
LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
if (valid_obs >= 4)
{
arma::vec rx_position_and_time;
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W;
try
{
// check if this is the initial position computation
if (this->get_time_offset_s() == 0)
{
// execute Bancroft's algorithm to estimate initial receiver position and time
DLOG(INFO) << " Executing Bancroft algorithm...";
rx_position_and_time = bancroftPos(satpos.t(), obs);
this->set_rx_pos({rx_position_and_time(0), rx_position_and_time(1), rx_position_and_time(2)}); // save ECEF position for the next iteration
this->set_time_offset_s(rx_position_and_time(3) / SPEED_OF_LIGHT_M_S); // save time for the next iteration [meters]->[seconds]
}
// Execute WLS using previous position as the initialization point
rx_position_and_time = leastSquarePos(satpos, obs, W);
this->set_rx_pos({rx_position_and_time(0), rx_position_and_time(1), rx_position_and_time(2)}); // save ECEF position for the next iteration
this->set_time_offset_s(this->get_time_offset_s() + rx_position_and_time(3) / SPEED_OF_LIGHT_M_S); // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time[0] << ", " << rx_position_and_time[1] << ", " << rx_position_and_time[2] << ", " << rx_position_and_time[4];
DLOG(INFO) << "Accumulated rx clock error=" << this->get_time_offset_s() << " clock error for this iteration=" << rx_position_and_time(3) / SPEED_OF_LIGHT_M_S << " [s]";
// Compute GST and Gregorian time
if (GST != 0.0)
{
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
}
else
{
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
}
// get time string Gregorian calendar
boost::posix_time::time_duration t = boost::posix_time::milliseconds(static_cast<long>(utc * 1000.0)); // NOLINT(google-runtime-int)
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
this->set_position_UTC_time(p_time);
DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()
<< " [deg], Height= " << this->get_height() << " [m]"
<< " RX time offset= " << this->get_time_offset_s() << " [s]";
// ######## LOG FILE #########
if (d_flag_dump_enabled == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
// PVT GPS time
tmp_double = hybrid_current_time;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// ECEF User Position East [m]
tmp_double = rx_position_and_time(0);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// ECEF User Position North [m]
tmp_double = rx_position_and_time(1);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// ECEF User Position Up [m]
tmp_double = rx_position_and_time(2);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// User clock offset [s]
tmp_double = rx_position_and_time(3);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// GEO user position Latitude [deg]
tmp_double = this->get_latitude();
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// GEO user position Longitude [deg]
tmp_double = this->get_longitude();
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// GEO user position Height [m]
tmp_double = this->get_height();
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
}
}
// MOVING AVERAGE PVT
this->perform_pos_averaging();
}
catch (const std::exception& e)
{
this->set_time_offset_s(0.0); // reset rx time estimation
LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
this->set_valid_position(false);
}
}
else
{
this->set_valid_position(false);
}
return this->is_valid_position();
}

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

@ -1,68 +0,0 @@
/*!
* \file hybrid_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
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2019 (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.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_HYBRID_LS_PVT_H
#define GNSS_SDR_HYBRID_LS_PVT_H
#include "galileo_almanac.h"
#include "galileo_navigation_message.h"
#include "gnss_synchro.h"
#include "gps_cnav_navigation_message.h"
#include "gps_navigation_message.h"
#include "ls_pvt.h"
#include "rtklib_rtkcmn.h"
#include <fstream>
#include <map>
#include <string>
/*!
* \brief This class implements a simple PVT Least Squares solution
*/
class Hybrid_Ls_Pvt : public Ls_Pvt
{
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_observables_map, double hybrid_current_time, bool flag_averaging);
std::map<int, Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
std::map<int, Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
std::map<int, Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map;
Galileo_Utc_Model galileo_utc_model;
Galileo_Iono galileo_iono;
Galileo_Almanac galileo_almanac;
Gps_Utc_Model gps_utc_model;
Gps_Iono gps_iono;
Gps_CNAV_Iono gps_cnav_iono;
Gps_CNAV_Utc_Model gps_cnav_utc_model;
private:
bool d_flag_dump_enabled;
std::string d_dump_filename;
std::ofstream d_dump_file;
int d_nchannels; // Number of available channels for positioning
double d_galileo_current_time;
};
#endif

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

@ -1,428 +0,0 @@
/*!
* \file ls_pvt.cc
* \brief Implementation of a base class for Least Squares PVT solutions
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2019 (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.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
*/
#include "ls_pvt.h"
#include "MATH_CONSTANTS.h"
#include "geofunctions.h"
#include <glog/logging.h>
#include <stdexcept>
arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
{
// BANCROFT Calculation of preliminary coordinates for a GPS receiver based on pseudoranges
// to 4 or more satellites. The ECEF coordinates are stored in satpos.
// The observed pseudoranges are stored in obs
// Reference: Bancroft, S. (1985) An Algebraic Solution of the GPS Equations,
// IEEE Trans. Aerosp. and Elec. Systems, AES-21, Issue 1, pp. 56--59
// Based on code by:
// Kai Borre 04-30-95, improved by C.C. Goad 11-24-96
//
// Test values to use in debugging
// B_pass =[ -11716227.778 -10118754.628 21741083.973 22163882.029;
// -12082643.974 -20428242.179 11741374.154 21492579.823;
// 14373286.650 -10448439.349 19596404.858 21492492.771;
// 10278432.244 -21116508.618 -12689101.970 25284588.982 ];
// Solution: 595025.053 -4856501.221 4078329.981
//
// Test values to use in debugging
// B_pass = [14177509.188 -18814750.650 12243944.449 21119263.116;
// 15097198.146 -4636098.555 21326705.426 22527063.486;
// 23460341.997 -9433577.991 8174873.599 23674159.579;
// -8206498.071 -18217989.839 17605227.065 20951643.862;
// 1399135.830 -17563786.820 19705534.862 20155386.649;
// 6995655.459 -23537808.269 -9927906.485 24222112.972 ];
// Solution: 596902.683 -4847843.316 4088216.740
arma::vec pos = arma::zeros(4, 1);
arma::mat B_pass = arma::zeros(obs.size(), 4);
B_pass.submat(0, 0, obs.size() - 1, 2) = satpos;
B_pass.col(3) = obs;
arma::mat B;
arma::mat BBB;
double traveltime = 0;
for (int iter = 0; iter < 2; iter++)
{
B = B_pass;
int m = arma::size(B, 0);
for (int i = 0; i < m; i++)
{
int x = B(i, 0);
int y = B(i, 1);
if (iter == 0)
{
traveltime = 0.072;
}
else
{
int z = B(i, 2);
double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2));
traveltime = sqrt(rho) / SPEED_OF_LIGHT_M_S;
}
double angle = traveltime * 7.292115147e-5;
double cosa = cos(angle);
double sina = sin(angle);
B(i, 0) = cosa * x + sina * y;
B(i, 1) = -sina * x + cosa * y;
} // % i-loop
if (m > 3)
{
BBB = arma::inv(B.t() * B) * B.t();
}
else
{
BBB = arma::inv(B);
}
arma::vec e = arma::ones(m, 1);
arma::vec alpha = arma::zeros(m, 1);
for (int i = 0; i < m; i++)
{
alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0;
}
arma::mat BBBe = BBB * e;
arma::mat BBBalpha = BBB * alpha;
double a = lorentz(BBBe, BBBe);
double b = lorentz(BBBe, BBBalpha) - 1;
double c = lorentz(BBBalpha, BBBalpha);
double root = sqrt(b * b - a * c);
arma::vec r = {(-b - root) / a, (-b + root) / a};
arma::mat possible_pos = arma::zeros(4, 2);
for (int i = 0; i < 2; i++)
{
possible_pos.col(i) = r(i) * BBBe + BBBalpha;
possible_pos(3, i) = -possible_pos(3, i);
}
arma::vec abs_omc = arma::zeros(2, 1);
for (int j = 0; j < m; j++)
{
for (int i = 0; i < 2; i++)
{
double c_dt = possible_pos(3, i);
double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0, 2)) + c_dt;
double omc = obs(j) - calc;
abs_omc(i) = std::abs(omc);
}
} // % j-loop
// discrimination between roots
if (abs_omc(0) > abs_omc(1))
{
pos = possible_pos.col(1);
}
else
{
pos = possible_pos.col(0);
}
} // % iter loop
return pos;
}
double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y)
{
// LORENTZ Calculates the Lorentz inner product of the two
// 4 by 1 vectors x and y
// Based on code by:
// Kai Borre 04-22-95
//
// M = diag([1 1 1 -1]);
// p = x'*M*y;
return (x(0) * y(0) + x(1) * y(1) + x(2) * y(2) - x(3) * y(3));
}
arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, const arma::vec& w_vec)
{
/* Computes the Least Squares Solution.
* Inputs:
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
* obs - Observations - the pseudorange measurements to each satellite
* w - weight vector
*
* Returns:
* pos - receiver position and receiver clock error
* (in ECEF system: [X, Y, Z, dt])
*/
//=== Initialization =======================================================
constexpr double GPS_STARTOFFSET_MS = 68.802; // [ms] Initial signal travel time
int nmbOfIterations = 10; // TODO: include in config
int nmbOfSatellites;
nmbOfSatellites = satpos.n_cols; // Armadillo
arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites);
w.diag() = w_vec; // diagonal weight matrix
std::array<double, 3> rx_pos = this->get_rx_pos();
arma::vec pos = {rx_pos[0], rx_pos[1], rx_pos[2], 0}; // time error in METERS (time x speed)
arma::mat A;
arma::mat omc;
A = arma::zeros(nmbOfSatellites, 4);
omc = arma::zeros(nmbOfSatellites, 1);
arma::mat X = satpos;
arma::vec Rot_X;
double rho2;
double traveltime;
double trop = 0.0;
double dlambda;
double dphi;
double h;
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) / SPEED_OF_LIGHT_M_S;
// --- Correct satellite position (do to earth rotation) -------
std::array<double, 3> rot_x = Ls_Pvt::rotateSatellite(traveltime, {X(0, i), X(1, i), X(2, i)});
Rot_X = {rot_x[0], rot_x[1], rot_x[2]};
// -- Find DOA and range of satellites
double* azim = nullptr;
double* elev = nullptr;
double* dist = nullptr;
topocent(azim, elev, dist, 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));
// Add troposphere correction if the receiver is below the troposphere
if (h > 15000)
{
// receiver is above the troposphere
trop = 0.0;
}
else
{
// --- Find delay due to troposphere (in meters)
Ls_Pvt::tropo(&trop, sin(elev[0] * GNSS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
if (trop > 5.0)
{
trop = 0.0; // check for erratic values
}
}
}
}
// --- 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)
}
}
// check the consistency of the PVT solution
if (((fabs(pos(3)) * 1000.0) / SPEED_OF_LIGHT_M_S) > GPS_STARTOFFSET_MS * 2)
{
LOG(WARNING) << "Receiver time offset out of range! Estimated RX Time error [s]:" << pos(3) / SPEED_OF_LIGHT_M_S;
throw std::runtime_error("Receiver time offset out of range!");
}
return pos;
}
int 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);
const double tkhum = t_kel + tlapse * (hhum_km - htkel_km);
const double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15);
const double e0 = 0.0611 * hum * pow(10, atkel);
const double tksea = t_kel - tlapse * htkel_km;
const double tkelh = tksea + tlapse * hhum_km;
const double e0sea = e0 * pow((tksea / tkelh), (4 * em));
const double tkelp = tksea + tlapse * hp_km;
const 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 (true)
{
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;
}
std::array<double, 3> Ls_Pvt::rotateSatellite(double traveltime, const std::array<double, 3>& 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)
*/
const double omegatau = GNSS_OMEGA_EARTH_DOT * traveltime;
const double cosomg = cos(omegatau);
const double sinomg = sin(omegatau);
const double x = cosomg * X_sat[0] + sinomg * X_sat[1];
const double y = -sinomg * X_sat[0] + cosomg * X_sat[1];
std::array<double, 3> X_sat_rot = {x, y, X_sat[2]};
return X_sat_rot;
}
double Ls_Pvt::get_gdop() const
{
return 0.0; // not implemented
}
double Ls_Pvt::get_pdop() const
{
return 0.0; // not implemented
}
double Ls_Pvt::get_hdop() const
{
return 0.0; // not implemented
}
double Ls_Pvt::get_vdop() const
{
return 0.0; // not implemented
}

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

@ -1,90 +0,0 @@
/*!
* \file ls_pvt.h
* \brief Interface of a base class for Least Squares PVT solutions
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2019 (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.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_LS_PVT_H
#define GNSS_SDR_LS_PVT_H
#if ARMA_NO_BOUND_CHECKING
#define ARMA_NO_DEBUG 1
#endif
#include "pvt_solution.h"
#include <armadillo>
#include <array>
/*!
* \brief Base class for the Least Squares PVT solution
*
*/
class Ls_Pvt : public Pvt_Solution
{
public:
Ls_Pvt() = default;
/*!
* \brief Computes the initial position solution based on the Bancroft algorithm
*/
arma::vec bancroftPos(const arma::mat& satpos, const arma::vec& obs);
/*!
* \brief Computes the Weighted Least Squares position solution
*/
arma::vec leastSquarePos(const arma::mat& satpos, const arma::vec& obs, const arma::vec& w_vec);
double get_hdop() const override;
double get_vdop() const override;
double get_pdop() const override;
double get_gdop() const override;
private:
/*
* Computes the Lorentz inner product between two vectors
*/
double lorentz(const arma::vec& x, const arma::vec& y);
/*
* 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);
std::array<double, 3> rotateSatellite(double traveltime, const std::array<double, 3>& X_sat);
};
#endif