mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-04 17:16:26 +00:00
Remove old, unused code
This commit is contained in:
Carles Fernandez
parent
d3654456e9
commit
399903e491
@ -10,34 +10,30 @@
|
|||||||
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS ${CMAKE_SOURCE_DIR}/docs/protobuf/monitor_pvt.proto)
|
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS ${CMAKE_SOURCE_DIR}/docs/protobuf/monitor_pvt.proto)
|
||||||
|
|
||||||
set(PVT_LIB_SOURCES
|
set(PVT_LIB_SOURCES
|
||||||
|
pvt_conf.cc
|
||||||
pvt_solution.cc
|
pvt_solution.cc
|
||||||
ls_pvt.cc
|
geojson_printer.cc
|
||||||
hybrid_ls_pvt.cc
|
|
||||||
kml_printer.cc
|
|
||||||
gpx_printer.cc
|
gpx_printer.cc
|
||||||
rinex_printer.cc
|
kml_printer.cc
|
||||||
nmea_printer.cc
|
nmea_printer.cc
|
||||||
|
rinex_printer.cc
|
||||||
rtcm_printer.cc
|
rtcm_printer.cc
|
||||||
rtcm.cc
|
rtcm.cc
|
||||||
geojson_printer.cc
|
|
||||||
rtklib_solver.cc
|
rtklib_solver.cc
|
||||||
pvt_conf.cc
|
|
||||||
monitor_pvt_udp_sink.cc
|
monitor_pvt_udp_sink.cc
|
||||||
)
|
)
|
||||||
|
|
||||||
set(PVT_LIB_HEADERS
|
set(PVT_LIB_HEADERS
|
||||||
|
pvt_conf.h
|
||||||
pvt_solution.h
|
pvt_solution.h
|
||||||
ls_pvt.h
|
geojson_printer.h
|
||||||
hybrid_ls_pvt.h
|
|
||||||
kml_printer.h
|
|
||||||
gpx_printer.h
|
gpx_printer.h
|
||||||
rinex_printer.h
|
kml_printer.h
|
||||||
nmea_printer.h
|
nmea_printer.h
|
||||||
|
rinex_printer.h
|
||||||
rtcm_printer.h
|
rtcm_printer.h
|
||||||
rtcm.h
|
rtcm.h
|
||||||
geojson_printer.h
|
|
||||||
rtklib_solver.h
|
rtklib_solver.h
|
||||||
pvt_conf.h
|
|
||||||
monitor_pvt_udp_sink.h
|
monitor_pvt_udp_sink.h
|
||||||
monitor_pvt.h
|
monitor_pvt.h
|
||||||
serdes_monitor_pvt.h
|
serdes_monitor_pvt.h
|
||||||
@ -73,13 +69,11 @@ endif()
|
|||||||
|
|
||||||
target_link_libraries(pvt_libs
|
target_link_libraries(pvt_libs
|
||||||
PUBLIC
|
PUBLIC
|
||||||
Armadillo::armadillo
|
|
||||||
Boost::date_time
|
Boost::date_time
|
||||||
protobuf::libprotobuf
|
protobuf::libprotobuf
|
||||||
core_system_parameters
|
core_system_parameters
|
||||||
algorithms_libs_rtklib
|
algorithms_libs_rtklib
|
||||||
PRIVATE
|
PRIVATE
|
||||||
algorithms_libs
|
|
||||||
Gflags::gflags
|
Gflags::gflags
|
||||||
Glog::glog
|
Glog::glog
|
||||||
Matio::matio
|
Matio::matio
|
||||||
@ -91,16 +85,12 @@ target_include_directories(pvt_libs
|
|||||||
PUBLIC
|
PUBLIC
|
||||||
${CMAKE_SOURCE_DIR}/src/core/receiver
|
${CMAKE_SOURCE_DIR}/src/core/receiver
|
||||||
SYSTEM ${PROTO_INCLUDE_HEADERS}
|
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}")
|
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)
|
if(USE_BOOST_ASIO_IO_CONTEXT)
|
||||||
target_compile_definitions(pvt_libs
|
target_compile_definitions(pvt_libs
|
||||||
PUBLIC
|
PUBLIC
|
||||||
|
|||||||
@ -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();
|
|
||||||
}
|
|
||||||
@ -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
|
|
||||||
@ -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
|
|
||||||
}
|
|
||||||
@ -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
|
|
||||||
Loading…
Reference in New Issue
Block a user