From 1929183e39c228e148aaa5e6961f395784d96bef Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 22 Sep 2022 17:49:02 +0200 Subject: [PATCH 001/165] Add Vector Tracking Loop engine skeleton --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 5 +- src/algorithms/PVT/libs/CMakeLists.txt | 6 ++ src/algorithms/PVT/libs/rtklib_solver.cc | 72 ++++++++++++++++++- src/algorithms/PVT/libs/rtklib_solver.h | 9 ++- src/algorithms/PVT/libs/vtl_conf.cc | 22 ++++++ src/algorithms/PVT/libs/vtl_conf.h | 40 +++++++++++ src/algorithms/PVT/libs/vtl_data.cc | 50 +++++++++++++ src/algorithms/PVT/libs/vtl_data.h | 54 ++++++++++++++ src/algorithms/PVT/libs/vtl_engine.cc | 47 ++++++++++++ src/algorithms/PVT/libs/vtl_engine.h | 54 ++++++++++++++ 10 files changed, 355 insertions(+), 4 deletions(-) create mode 100644 src/algorithms/PVT/libs/vtl_conf.cc create mode 100644 src/algorithms/PVT/libs/vtl_conf.h create mode 100644 src/algorithms/PVT/libs/vtl_data.cc create mode 100644 src/algorithms/PVT/libs/vtl_data.h create mode 100644 src/algorithms/PVT/libs/vtl_engine.cc create mode 100644 src/algorithms/PVT/libs/vtl_engine.h diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 61f141ee8..bb07f6558 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2131,7 +2131,8 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // old_time_debug = d_gnss_observables_map.cbegin()->second.RX_time * 1000.0; uint32_t current_RX_time_ms = 0; // #### solve PVT and store the corrected observable set - if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false)) + bool get_vtl_data = true; + if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false, get_vtl_data)) { d_pvt_errors_counter = 0; // Reset consecutive PVT error counter const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); @@ -2245,7 +2246,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // compute on the fly PVT solution if (flag_compute_pvt_output == true) { - flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false); + flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false); } if (flag_pvt_valid == true) diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 8e2785144..83c708f46 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -22,6 +22,9 @@ set(PVT_LIB_SOURCES monitor_pvt_udp_sink.cc monitor_ephemeris_udp_sink.cc has_simple_printer.cc + vtl_conf.cc + vtl_data.cc + vtl_engine.cc ) set(PVT_LIB_HEADERS @@ -43,6 +46,9 @@ set(PVT_LIB_HEADERS serdes_gps_eph.h monitor_ephemeris_udp_sink.h has_simple_printer.h + vtl_conf.h + vtl_data.h + vtl_engine.h ) list(SORT PVT_LIB_HEADERS) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index bab77a024..2d17cb402 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -34,6 +34,7 @@ #include "Beidou_DNAV.h" #include "gnss_sdr_filesystem.h" #include "rtklib_conversions.h" +#include "rtklib_ephemeris.h" #include "rtklib_rtkpos.h" #include "rtklib_solution.h" #include @@ -55,6 +56,14 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, d_flag_dump_mat_enabled(flag_dump_to_mat), d_use_e6_for_pvt(use_e6_for_pvt) { + // TODO: temporal VTL config parameters are hardcoded here. Move it to PVT adapter config (javi) + Vtl_Conf new_vtl_conf; + //TODO: new_vtl_conf.parameter1=blablabla + + vtl_engine.configure(new_vtl_conf); + vtl_engine.reset(); + + this->set_averaging_flag(false); // see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -457,7 +466,11 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const } -bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging) +void get_vtl_data() +{ +} + +bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging, bool get_vtl_data) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -997,6 +1010,63 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } } + + if (get_vtl_data == true) + { + //VTL input data extraction from rtklib structures + /* satellite positions, velocities and clocks */ + prcopt_t *opt = &d_rtk.opt; + /* count rover/base station observations */ + int n_sats = valid_obs + glo_valid_obs; + int nu; + int nr; + for (nu = 0; nu < n_sats && d_obs_data.data()[nu].rcv == 1; nu++) + { + } + for (nr = 0; nu + nr < n_sats && d_obs_data.data()[nu + nr].rcv == 2; nr++) + { + } + + double *rs; + double *dts; + double *var; + + std::vector svh(MAXOBS * 2); + rs = mat(6, n_sats); + dts = mat(2, n_sats); + var = mat(1, n_sats); + /* satellite positions, velocities and clocks */ + satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); + + Vtl_Data new_vtl_data; + new_vtl_data.init_storage(n_sats); + new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; + + for (int n = 0; n < n_sats; n++) + { + new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; + new_vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; + new_vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; + new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; + new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; + new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; + + new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; + new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; + new_vtl_data.sat_var(n) = var[n]; + new_vtl_data.sat_health_flag(n) = svh.at(n); + + // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) + new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; + new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; + new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; + } + new_vtl_data.debug_print(); + + //Call the VTL engine loop + vtl_engine.vtl_loop(new_vtl_data); + } + result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data); if (result == 0) diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 22d20ba2f..1cc1323d5 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -57,6 +57,8 @@ #include "monitor_pvt.h" #include "pvt_solution.h" #include "rtklib.h" +#include "vtl_data.h" +#include "vtl_engine.h" #include #include #include @@ -84,7 +86,11 @@ public: bool use_e6_for_pvt = true); ~Rtklib_Solver(); - bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); + bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging, bool get_vtl_data); + + void get_vtl_data(); + Vtl_Data vtl_data; + double get_hdop() const override; double get_vdop() const override; @@ -135,6 +141,7 @@ private: bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; bool d_use_e6_for_pvt; + Vtl_Engine vtl_engine; }; diff --git a/src/algorithms/PVT/libs/vtl_conf.cc b/src/algorithms/PVT/libs/vtl_conf.cc new file mode 100644 index 000000000..6353ad7f3 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_conf.cc @@ -0,0 +1,22 @@ +/*! + * \file vtl_conf.c + * \brief Class that contains all the parameters to configure the Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "vtl_conf.h" + +Vtl_Conf::Vtl_Conf() +{ +} diff --git a/src/algorithms/PVT/libs/vtl_conf.h b/src/algorithms/PVT/libs/vtl_conf.h new file mode 100644 index 000000000..eb5bfadb3 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_conf.h @@ -0,0 +1,40 @@ +/*! + * \file vtl_conf.h + * \brief Class that contains all the parameters to configure the Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_VTL_CONF_H +#define GNSS_SDR_VTL_CONF_H + +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Vtl_Conf +{ +public: + Vtl_Conf(); + //TODO: VTL control parameters and config options here +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_VTL_CONF_H diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc new file mode 100644 index 000000000..4718ad7b4 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -0,0 +1,50 @@ +/*! + * \file vtl_data.c + * \brief Class that exchange information to and from the Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "vtl_data.h" + +Vtl_Data::Vtl_Data() +{ + epoch_tow_s = 0; +} + +void Vtl_Data::init_storage(int n_sats) +{ + sat_p = arma::mat(n_sats, 3); + sat_v = arma::mat(n_sats, 3); + sat_dts = arma::mat(n_sats, 2); + sat_var = arma::vec(n_sats); + sat_health_flag = arma::vec(n_sats); + pr_m = arma::vec(n_sats); + doppler_hz = arma::vec(n_sats); + carrier_phase_rads = arma::vec(n_sats); + epoch_tow_s = 0; +} + +void Vtl_Data::debug_print() +{ + std::cout << "vtl_data debug print at TOW: " << epoch_tow_s << "\n "; + sat_p.print("VTL Sat Positions"); + sat_v.print("VTL Sat Velocities"); + sat_dts.print("VTL Sat clocks"); + sat_var.print("VTL Sat clock variances"); + sat_health_flag.print("VTL Sat health"); + + pr_m.print("Satellite Code pseudoranges [m]"); + doppler_hz.print("satellite Carrier Dopplers [Hz]"); + carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); +} diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h new file mode 100644 index 000000000..c4de5c737 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -0,0 +1,54 @@ +/*! + * \file vtl_data.h + * \brief Class that exchange information to and from the Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_VTL_DATA_H +#define GNSS_SDR_VTL_DATA_H + +#include +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Vtl_Data +{ +public: + Vtl_Data(); + void init_storage(int n_sats); + + arma::mat sat_p; //Satellite ECEF Position [m] + arma::mat sat_v; //Satellite Velocity [m/s] + arma::mat sat_dts; //Satellite clock bias and drift [s,s/s] + arma::vec sat_var; //sat position and clock error variance [m^2] + arma::vec sat_health_flag; //sat health flag (0 is ok) + + arma::vec pr_m; //Satellite Code pseudoranges [m] + arma::vec doppler_hz; //satellite Carrier Dopplers [Hz] + arma::vec carrier_phase_rads; //satellite accumulated carrier phases [rads] + + double epoch_tow_s; //current observation RX time [s] + void debug_print(); +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_VTL_DATA_H diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc new file mode 100644 index 000000000..b51fd641e --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -0,0 +1,47 @@ +/*! + * \file vtl_engine.h + * \brief Class that implements a Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "vtl_engine.h" + +Vtl_Engine::Vtl_Engine() +{ +} + +Vtl_Engine::~Vtl_Engine() +{ +} + +bool Vtl_Engine::vtl_loop(Vtl_Data new_data) +{ + //TODO: Implement main VTL loop here + return true; +} + +void Vtl_Engine::reset() +{ + //TODO +} + +void Vtl_Engine::debug_print() +{ + //TODO +} + +void Vtl_Engine::configure(Vtl_Conf config_) +{ + config = config_; + //TODO: initialize internal variables +} diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h new file mode 100644 index 000000000..ed972f058 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -0,0 +1,54 @@ +/*! + * \file vtl_engine.h + * \brief Class that implements a Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_VTL_ENGINE_H +#define GNSS_SDR_VTL_ENGINE_H + +#include "vtl_conf.h" +#include "vtl_data.h" +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Vtl_Engine +{ +public: + Vtl_Engine(); + + ~Vtl_Engine(); + + void configure(Vtl_Conf config_); //set config parameters + + //TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...) + bool vtl_loop(Vtl_Data new_data); + void reset(); // reset all internal states + void debug_print(); // print debug information + +private: + Vtl_Conf config; + //TODO: Internal VTL persistent variables here +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_VTL_ENGINE_H From ccf2b830a2fef375ee5c13d58c95e8c0707cc265 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 23 Sep 2022 12:28:19 +0200 Subject: [PATCH 002/165] Adding TrackingCmd class to VTL engine --- src/algorithms/PVT/libs/rtklib_solver.cc | 7 +------ src/algorithms/PVT/libs/rtklib_solver.h | 2 -- src/algorithms/PVT/libs/vtl_data.cc | 4 +++- src/algorithms/PVT/libs/vtl_data.h | 21 +++++++++++---------- src/algorithms/PVT/libs/vtl_engine.cc | 12 ++++++++++++ src/algorithms/PVT/libs/vtl_engine.h | 4 ++++ src/algorithms/libs/trackingcmd.h | 2 -- 7 files changed, 31 insertions(+), 21 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 2d17cb402..09a1d9007 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -465,11 +465,6 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const return d_monitor_pvt; } - -void get_vtl_data() -{ -} - bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging, bool get_vtl_data) { std::map::const_iterator gnss_observables_iter; @@ -1041,7 +1036,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ Vtl_Data new_vtl_data; new_vtl_data.init_storage(n_sats); new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; - + new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands for (int n = 0; n < n_sats; n++) { new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 1cc1323d5..b03cc886d 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -88,10 +88,8 @@ public: bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging, bool get_vtl_data); - void get_vtl_data(); Vtl_Data vtl_data; - double get_hdop() const override; double get_vdop() const override; double get_pdop() const override; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 4718ad7b4..25824b0cf 100644 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -20,6 +20,7 @@ Vtl_Data::Vtl_Data() { epoch_tow_s = 0; + sample_counter = 0; } void Vtl_Data::init_storage(int n_sats) @@ -33,11 +34,12 @@ void Vtl_Data::init_storage(int n_sats) doppler_hz = arma::vec(n_sats); carrier_phase_rads = arma::vec(n_sats); epoch_tow_s = 0; + sample_counter = 0; } void Vtl_Data::debug_print() { - std::cout << "vtl_data debug print at TOW: " << epoch_tow_s << "\n "; + std::cout << "vtl_data debug print at RX TOW: " << epoch_tow_s << ", TRK sample counter: " << sample_counter << "\n"; sat_p.print("VTL Sat Positions"); sat_v.print("VTL Sat Velocities"); sat_dts.print("VTL Sat clocks"); diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index c4de5c737..8507f6d1d 100644 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -19,7 +19,6 @@ #include #include -#include #include /** \addtogroup PVT @@ -34,17 +33,19 @@ public: Vtl_Data(); void init_storage(int n_sats); - arma::mat sat_p; //Satellite ECEF Position [m] - arma::mat sat_v; //Satellite Velocity [m/s] - arma::mat sat_dts; //Satellite clock bias and drift [s,s/s] - arma::vec sat_var; //sat position and clock error variance [m^2] - arma::vec sat_health_flag; //sat health flag (0 is ok) + arma::mat sat_p; // Satellite ECEF Position [m] + arma::mat sat_v; // Satellite Velocity [m/s] + arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] + arma::vec sat_var; // sat position and clock error variance [m^2] + arma::vec sat_health_flag; // sat health flag (0 is ok) - arma::vec pr_m; //Satellite Code pseudoranges [m] - arma::vec doppler_hz; //satellite Carrier Dopplers [Hz] - arma::vec carrier_phase_rads; //satellite accumulated carrier phases [rads] + arma::vec pr_m; // Satellite Code pseudoranges [m] + arma::vec doppler_hz; // satellite Carrier Dopplers [Hz] + arma::vec carrier_phase_rads; // satellite accumulated carrier phases [rads] - double epoch_tow_s; //current observation RX time [s] + // time handling + double epoch_tow_s; // current observation RX time [s] + uint64_t sample_counter; // current sample counter associated with RX time [samples from start] void debug_print(); }; diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index b51fd641e..31a8781e7 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -27,6 +27,18 @@ Vtl_Engine::~Vtl_Engine() bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here + + //TODO: Fill the tracking commands outputs + // Notice: keep the same satellite order as in the Vtl_Data matrices + // sample code + TrackingCmd trk_cmd; + trk_cmd.carrier_freq_hz = 0; + trk_cmd.carrier_freq_rate_hz_s = 0; + trk_cmd.code_freq_chips = 0; + trk_cmd.enable_carrier_nco_cmd = true; + trk_cmd.enable_code_nco_cmd = true; + trk_cmd.sample_counter = new_data.sample_counter; + trk_cmd_outs.push_back(trk_cmd); return true; } diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index ed972f058..ac8a750df 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -17,11 +17,13 @@ #ifndef GNSS_SDR_VTL_ENGINE_H #define GNSS_SDR_VTL_ENGINE_H +#include "trackingcmd.h" #include "vtl_conf.h" #include "vtl_data.h" #include #include #include +#include /** \addtogroup PVT * \{ */ @@ -43,6 +45,8 @@ public: void reset(); // reset all internal states void debug_print(); // print debug information + std::vector trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs + private: Vtl_Conf config; //TODO: Internal VTL persistent variables here diff --git a/src/algorithms/libs/trackingcmd.h b/src/algorithms/libs/trackingcmd.h index cf56ed5f8..77115167b 100644 --- a/src/algorithms/libs/trackingcmd.h +++ b/src/algorithms/libs/trackingcmd.h @@ -28,8 +28,6 @@ class TrackingCmd { public: - TrackingCmd(); - bool enable_carrier_nco_cmd = false; bool enable_code_nco_cmd = false; double code_freq_chips = 0.0; From aeb57a8698a459b7b6ca1e511ea1a2e79dd4e907 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 7 Oct 2022 21:59:40 +0000 Subject: [PATCH 003/165] MOD: vtl_data updated --- src/algorithms/PVT/libs/vtl_data.cc | 7 +++++++ src/algorithms/PVT/libs/vtl_data.h | 4 ++++ 2 files changed, 11 insertions(+) mode change 100644 => 100755 src/algorithms/PVT/libs/vtl_data.cc mode change 100644 => 100755 src/algorithms/PVT/libs/vtl_data.h diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc old mode 100644 new mode 100755 index 25824b0cf..58b5ebcfe --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -30,9 +30,16 @@ void Vtl_Data::init_storage(int n_sats) sat_dts = arma::mat(n_sats, 2); sat_var = arma::vec(n_sats); sat_health_flag = arma::vec(n_sats); + pr_m = arma::vec(n_sats); doppler_hz = arma::vec(n_sats); carrier_phase_rads = arma::vec(n_sats); + + rx_p = arma::mat(1, 3); + rx_v = arma::mat(1, 3); + rx_dts = arma::mat(1, 2); + rx_var = arma::vec(1); + epoch_tow_s = 0; sample_counter = 0; } diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h old mode 100644 new mode 100755 index 8507f6d1d..a35dd7502 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -43,6 +43,10 @@ public: arma::vec doppler_hz; // satellite Carrier Dopplers [Hz] arma::vec carrier_phase_rads; // satellite accumulated carrier phases [rads] + arma::mat rx_p; // Receiver ENU Position [m] + arma::mat rx_v; // Receiver Velocity [m/s] + arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] + arma::vec rx_var; // Receiver position and clock error variance [m^2] // time handling double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] From 759e31e8f7bcb547657f66cb954f694776428894 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 7 Oct 2022 21:59:59 +0000 Subject: [PATCH 004/165] MOD: rtklib solver adaption --- src/algorithms/PVT/libs/rtklib_solver.cc | 125 ++++++++++++----------- 1 file changed, 68 insertions(+), 57 deletions(-) mode change 100644 => 100755 src/algorithms/PVT/libs/rtklib_solver.cc diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc old mode 100644 new mode 100755 index 09a1d9007..fd69b0b90 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1005,63 +1005,6 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } } - - if (get_vtl_data == true) - { - //VTL input data extraction from rtklib structures - /* satellite positions, velocities and clocks */ - prcopt_t *opt = &d_rtk.opt; - /* count rover/base station observations */ - int n_sats = valid_obs + glo_valid_obs; - int nu; - int nr; - for (nu = 0; nu < n_sats && d_obs_data.data()[nu].rcv == 1; nu++) - { - } - for (nr = 0; nu + nr < n_sats && d_obs_data.data()[nu + nr].rcv == 2; nr++) - { - } - - double *rs; - double *dts; - double *var; - - std::vector svh(MAXOBS * 2); - rs = mat(6, n_sats); - dts = mat(2, n_sats); - var = mat(1, n_sats); - /* satellite positions, velocities and clocks */ - satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); - - Vtl_Data new_vtl_data; - new_vtl_data.init_storage(n_sats); - new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; - new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands - for (int n = 0; n < n_sats; n++) - { - new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; - new_vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; - new_vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; - new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; - new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; - new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; - - new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; - new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; - new_vtl_data.sat_var(n) = var[n]; - new_vtl_data.sat_health_flag(n) = svh.at(n); - - // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; - new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; - new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; - } - new_vtl_data.debug_print(); - - //Call the VTL engine loop - vtl_engine.vtl_loop(new_vtl_data); - } - result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data); if (result == 0) @@ -1121,7 +1064,75 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ rx_position_and_time[3] = pvt_sol.dtr[2] + pvt_sol.dtr[0] / SPEED_OF_LIGHT_M_S; } 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 + + if (get_vtl_data == true) + { + + //VTL input data extraction from rtklib structures + /* satellite positions, velocities and clocks */ + prcopt_t *opt = &d_rtk.opt; + /* count rover/base station observations */ + int n_sats = valid_obs + glo_valid_obs; + int nu; + int nr; + for (nu = 0; nu < n_sats && d_obs_data.data()[nu].rcv == 1; nu++) + { + } + for (nr = 0; nu + nr < n_sats && d_obs_data.data()[nu + nr].rcv == 2; nr++) + { + } + double *rs; + double *dts; + double *var; + + std::vector svh(MAXOBS * 2); + rs = mat(6, n_sats); + dts = mat(2, n_sats); + var = mat(1, n_sats); + /* satellite positions, velocities and clocks */ + satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); + + Vtl_Data new_vtl_data; + new_vtl_data.init_storage(n_sats); + new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; + new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands + for (int n = 0; n < n_sats; n++) + { + new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; + new_vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; + new_vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; + new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; + new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; + new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; + + new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; + new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; + new_vtl_data.sat_var(n) = var[n]; + new_vtl_data.sat_health_flag(n) = svh.at(n); + + // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) + new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; + new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; + new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; + } + //VTL input data extraction from rtklib structures + /* Receiver position, velocitie and clock */ + /* position/velocity (m|m/s):{x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ + new_vtl_data.rx_p(0) =pvt_sol.rr[0]; + new_vtl_data.rx_p(1) =pvt_sol.rr[1]; + new_vtl_data.rx_p(2) =pvt_sol.rr[2]; + new_vtl_data.rx_v(0) =pvt_sol.rr[3] ; + new_vtl_data.rx_v(1) =pvt_sol.rr[4] ; + new_vtl_data.rx_v(2) =pvt_sol.rr[5] ; + //receiver clock offset and receiver clock drift + new_vtl_data.rx_dts(0)=rx_position_and_time[3]; + new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]; + + new_vtl_data.debug_print(); + //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? + vtl_engine.vtl_loop(new_vtl_data); + } // compute Ground speed and COG double ground_speed_ms = 0.0; std::array pos{}; From 44cc4027661b4d2bc486ccf86a1c25a31faa65d1 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 7 Oct 2022 22:00:31 +0000 Subject: [PATCH 005/165] ADD: vtl_engine persistent variables --- src/algorithms/PVT/libs/vtl_engine.cc | 138 ++++++++++++++++++++++++++ src/algorithms/PVT/libs/vtl_engine.h | 18 ++++ 2 files changed, 156 insertions(+) mode change 100644 => 100755 src/algorithms/PVT/libs/vtl_engine.cc mode change 100644 => 100755 src/algorithms/PVT/libs/vtl_engine.h diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc old mode 100644 new mode 100755 index 31a8781e7..b0d031444 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -27,6 +27,144 @@ Vtl_Engine::~Vtl_Engine() bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here +// // ################## Kalman filter initialization ###################################### + +// // covariances (static) +// kf_P_x_ini = arma::zeros(8, 8); +// kf_x_pri = arma::zeros(8, 1); +// kf_R = arma::zeros(2*n_sats, 2*n_sats); +// kf_dt=1e-3; +// kf_Q = arma::zeros(8, 8); + +// kf_F = arma::zeros(8, 8); +// kf_F(0, 0) = 1.0; kf_F(0, 3) = kf_dt; +// kf_F(1, 1) = 1.0; kf_F(1, 4) = kf_dt; +// kf_F(2, 2) = 1.0; kf_F(2, 5) = kf_dt; +// kf_F(3, 3) = 1.0; +// kf_F(4, 4) = 1.0; +// kf_F(5, 5) = 1.0; +// kf_F(6, 6) = 1.0; kf_F(6, 7) = kf_dt; +// kf_F(7, 7) = 1.0; + +// kf_H = arma::zeros(8, 2*n_sats); +// kf_x = arma::zeros(8, 1); +// kf_y = arma::zeros(2*n_sats, 1); +// kf_P_y = arma::zeros(2*n_sats, 2*n_sats); + +// // ################## Kalman Tracking ###################################### +// // receiver solution from rtklib_solver +// kf_x(0)=new_vtl_data.rx_p(0); +// kf_x(1)=new_vtl_data.rx_p(1); +// kf_x(2)=new_vtl_data.rx_p(2); +// kf_x(3)=new_vtl_data.rx_v(0); +// kf_x(4)=new_vtl_data.rx_v(1); +// kf_x(5)=new_vtl_data.rx_v(2); +// kf_x(6)=new_vtl_data.rx_dts(0); +// kf_x(7)=new_vtl_data.rx_dts(1); + +// // Kalman state prediction (time update) +// kf_x_pri = kf_F * kf_x; // state prediction +// //kf_P_x_pri = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction + +// //from error state variables to variables +// //x_u=x_u0+kf_x_pri(0); +// //y_u=y_u0+kf_x_pri(1); +// //z_u=z_u0+kf_x_pri(2); +// //xDot_u=xDot_u0+kf_x_pri(3); +// //yDot_u=yDot_u0+kf_x_pri(4); +// //zDot_u=zDot_u0+kf_x_pri(5); +// //cdeltat_u=cdeltat_u0+kf_x_pri(6); +// //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); +// //from state variables definition +// x_u=kf_x_pri(0); +// y_u=kf_x_pri(1); +// z_u=kf_x_pri(2); +// xDot_u=kf_x_pri(3); +// yDot_u=kf_x_pri(4); +// zDot_u=kf_x_pri(5); +// cdeltat_u=kf_x_pri(6); +// cdeltatDot_u=kf_x_pri(7); +// for (int32_t i = 0; i < n_sats; n++) //neccesary quantities +// { +// d(i)=sqrt(square(new_vtl_data.sat_p(i, 0)-x_u)+square(new_vtl_data.sat_p(i, 1)-y_u)+square(new_vtl_data.sat_p(i, 2)-z_u)); +// //compute pseudorange estimation +// rho_pri(i)=d(i)+cdeltat_u; +// //compute LOS sat-receiver vector components +// a_x(i)=-(new_vtl_data.sat_p(i, 0)-x_u)/d(i); +// a_y(i)=-(new_vtl_data.sat_p(i, 1)-y_u)/d(i);; +// a_z(i)=-(new_vtl_data.sat_p(i, 2)-z_u)/d(i);; +// //compute pseudorange rate estimation +// rhoDot_pri(i)=(new_vtl_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_vtl_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_vtl_data.sat_v(i, 2)-zDot_u)*a_z(i)+cdeltatDot_u; +// } + +// kf_H = arma::zeros(8, 2*n_sats); + +// for (int32_t i = 0; i < n_sats; n++) // Measurement matrix H assembling +// { +// // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) +// kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; +// kf_H(i+n_sats, 3) = a_x(i); kf_H(i+n_sats, 4) = a_y(i); kf_H(i+n_sats, 5) = a_z(i); kf_H(i+n_sats, 7) = 1.0; +// } + +// // Kalman estimation (measurement update) +// for (int32_t i = 0; i < n_sats; n++) // Measurement vector +// { +// kf_y(i) = delta_rho(i); // i-Satellite +// kf_y(i+n_sats) = delta_rhoDot(i); // i-Satellite +// } + +// for (int32_t i = 0; i < n_sats; n++) // Measurement error Covariance Matrix R assembling +// { +// // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) +// kf_R(i, i) = 1.0; +// kf_R(i+n_sats, i+n_sats) = 1.0; +// } + +// // Kalman filter update step +// // kf_P_y = kf_H * kf_P_x_pri * kf_H.t() + kf_R; // innovation covariance matrix (S) +// // kf_K = (kf_P_x_pri * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain + +// for (int32_t i = 0; i < n_sats; n++) //Error measurement vector +// { +// // kf_delta_y(i)=rho(i)+delta_rho(i)-rho_pri(i); // pseudorange error +// // kf_delta_y(i+n_sats)=rhoDot(i)+delta_F*(-lambdaC)-rhoDot_pri(i); // pseudorange rate error +// } + +// // kf_delta_x = kf_K * kf_delta_y; // updated error state estimation +// // kf_P_x = (arma::eye(size(kf_P_x_pri)) - kf_K * kf_H) * kf_P_x_pri; // update state estimation error covariance matrix + +// // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) + + +// // ################## Geometric Transformation ###################################### + +// for (int32_t i = 0; i < n_sats; n++) //neccesary quantities at posteriori +// { +// //compute pseudorange posteriori estimation +// // rho_est(i)=; +// //compute LOS sat-receiver vector components posteriori +// // a_x(i)=; +// // a_y(i)=; +// // a_z(i)=; +// //compute pseudorange rate posteriori estimation +// // rhoDot_est(i)=; +// } + +// kf_H = arma::zeros(8, 2*n_sats); + +// for (int32_t i = 0; i < n_sats; n++) // Measurement matrix H posteriori assembling +// { +// // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) +// kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; +// kf_H(i+n_sats, 3) = a_x(i); kf_H(i+n_sats, 4) = a_y(i); kf_H(i+n_sats, 5) = a_z(i); kf_H(i+n_sats, 7) = 1.0; +// } + +// //Re-calculate error measurement vector with the most recent data available +// //kf_delta_y=kf_H*kf_delta_x +// //Filtered pseudorange error measurement (in m): +// //delta_rho_filt=; +// //Filtered Doppler error measurement (in Hz): +// //delta_doppler_filt=; //TODO: Fill the tracking commands outputs // Notice: keep the same satellite order as in the Vtl_Data matrices diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h old mode 100644 new mode 100755 index ac8a750df..daba6d141 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -50,6 +50,24 @@ public: private: Vtl_Conf config; //TODO: Internal VTL persistent variables here + // Kalman filter variables + arma::mat kf_P_x_ini; // initial state error covariance matrix + arma::mat kf_P_x; // state error covariance matrix + arma::mat kf_P_x_pre; // Predicted state error covariance matrix + arma::mat kf_P_y; // innovation covariance matrix + + arma::mat kf_F; // state transition matrix + arma::mat kf_H; // system matrix + arma::mat kf_R; // measurement error covariance matrix + arma::mat kf_Q; // system error covariance matrix + + arma::colvec kf_x; // state vector + arma::colvec kf_x_pre; // predicted state vector + arma::colvec kf_y; // measurement vector + arma::mat kf_K; // Kalman gain matrix + + // Gaussian estimator + arma::mat kf_R_est; // measurement error covariance }; From e9aa46fc58ff2fd7a3dff0c19b382eebc57411c6 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 9 Oct 2022 22:35:45 +0000 Subject: [PATCH 006/165] MOD: add n_sats, type namechange for readability (colvec from vec) --- src/algorithms/PVT/libs/vtl_data.cc | 3 ++- src/algorithms/PVT/libs/vtl_data.h | 15 ++++++++------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 58b5ebcfe..9cd1fef4d 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -30,7 +30,8 @@ void Vtl_Data::init_storage(int n_sats) sat_dts = arma::mat(n_sats, 2); sat_var = arma::vec(n_sats); sat_health_flag = arma::vec(n_sats); - + int sat_number = n_sats; + pr_m = arma::vec(n_sats); doppler_hz = arma::vec(n_sats); carrier_phase_rads = arma::vec(n_sats); diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index a35dd7502..d63dc865b 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -36,17 +36,18 @@ public: arma::mat sat_p; // Satellite ECEF Position [m] arma::mat sat_v; // Satellite Velocity [m/s] arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] - arma::vec sat_var; // sat position and clock error variance [m^2] - arma::vec sat_health_flag; // sat health flag (0 is ok) - - arma::vec pr_m; // Satellite Code pseudoranges [m] - arma::vec doppler_hz; // satellite Carrier Dopplers [Hz] - arma::vec carrier_phase_rads; // satellite accumulated carrier phases [rads] + arma::colvec sat_var; // sat position and clock error variance [m^2] + arma::colvec sat_health_flag; // sat health flag (0 is ok) + int sat_number; // on-view sat number + + arma::colvec pr_m; // Satellite Code pseudoranges [m] + arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz] + arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads] arma::mat rx_p; // Receiver ENU Position [m] arma::mat rx_v; // Receiver Velocity [m/s] arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] - arma::vec rx_var; // Receiver position and clock error variance [m^2] + arma::colvec rx_var; // Receiver position and clock error variance [m^2] // time handling double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] From b9394bd79d3ab252d5956500ff84c093afc7e0d6 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 9 Oct 2022 22:36:40 +0000 Subject: [PATCH 007/165] ADD: Necessary variables for KF --- src/algorithms/PVT/libs/vtl_engine.h | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index daba6d141..64fcdb104 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -50,11 +50,30 @@ public: private: Vtl_Conf config; //TODO: Internal VTL persistent variables here - // Kalman filter variables + + //State variables + double x_u; + double y_u; + double z_u; + double xDot_u; + double yDot_u; + double zDot_u; + double cdeltat_u; + double cdeltatDot_u; + + // Transformation variables + arma::colvec d; + arma::colvec rho_pri; + arma::colvec rhoDot_pri; + arma::colvec a_x; + arma::colvec a_y; + arma::colvec a_z; + + // Kalman filter matrices arma::mat kf_P_x_ini; // initial state error covariance matrix arma::mat kf_P_x; // state error covariance matrix arma::mat kf_P_x_pre; // Predicted state error covariance matrix - arma::mat kf_P_y; // innovation covariance matrix + arma::mat kf_S; // innovation covariance matrix arma::mat kf_F; // state transition matrix arma::mat kf_H; // system matrix From 3c8a4f3d3fdf76caaa320488f95d75dabe824e75 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 9 Oct 2022 22:36:57 +0000 Subject: [PATCH 008/165] ADD: first prototype of VTL KF --- src/algorithms/PVT/libs/vtl_engine.cc | 177 ++++++++++++++------------ 1 file changed, 95 insertions(+), 82 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index b0d031444..ea8abf83a 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -27,44 +27,46 @@ Vtl_Engine::~Vtl_Engine() bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here + using arma::as_scalar; + using arma::dot; // // ################## Kalman filter initialization ###################################### // // covariances (static) -// kf_P_x_ini = arma::zeros(8, 8); -// kf_x_pri = arma::zeros(8, 1); -// kf_R = arma::zeros(2*n_sats, 2*n_sats); -// kf_dt=1e-3; -// kf_Q = arma::zeros(8, 8); + kf_P_x_ini = arma::zeros(8, 8); //TODO: use a real value. + kf_x = arma::zeros(8, 1); + kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); + double kf_dt=1e-3; + kf_Q = arma::zeros(8, 8); -// kf_F = arma::zeros(8, 8); -// kf_F(0, 0) = 1.0; kf_F(0, 3) = kf_dt; -// kf_F(1, 1) = 1.0; kf_F(1, 4) = kf_dt; -// kf_F(2, 2) = 1.0; kf_F(2, 5) = kf_dt; -// kf_F(3, 3) = 1.0; -// kf_F(4, 4) = 1.0; -// kf_F(5, 5) = 1.0; -// kf_F(6, 6) = 1.0; kf_F(6, 7) = kf_dt; -// kf_F(7, 7) = 1.0; + kf_F = arma::zeros(8, 8); + kf_F(0, 0) = 1.0; kf_F(0, 3) = kf_dt; + kf_F(1, 1) = 1.0; kf_F(1, 4) = kf_dt; + kf_F(2, 2) = 1.0; kf_F(2, 5) = kf_dt; + kf_F(3, 3) = 1.0; + kf_F(4, 4) = 1.0; + kf_F(5, 5) = 1.0; + kf_F(6, 6) = 1.0; kf_F(6, 7) = kf_dt; + kf_F(7, 7) = 1.0; -// kf_H = arma::zeros(8, 2*n_sats); -// kf_x = arma::zeros(8, 1); -// kf_y = arma::zeros(2*n_sats, 1); -// kf_P_y = arma::zeros(2*n_sats, 2*n_sats); + kf_H = arma::zeros(8, 2*new_data.sat_number); + kf_x = arma::zeros(8, 1); + kf_y = arma::zeros(2*new_data.sat_number, 1); + kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix // // ################## Kalman Tracking ###################################### // // receiver solution from rtklib_solver -// kf_x(0)=new_vtl_data.rx_p(0); -// kf_x(1)=new_vtl_data.rx_p(1); -// kf_x(2)=new_vtl_data.rx_p(2); -// kf_x(3)=new_vtl_data.rx_v(0); -// kf_x(4)=new_vtl_data.rx_v(1); -// kf_x(5)=new_vtl_data.rx_v(2); -// kf_x(6)=new_vtl_data.rx_dts(0); -// kf_x(7)=new_vtl_data.rx_dts(1); + kf_x(0)=new_data.rx_p(0); + kf_x(1)=new_data.rx_p(1); + kf_x(2)=new_data.rx_p(2); + kf_x(3)=new_data.rx_v(0); + kf_x(4)=new_data.rx_v(1); + kf_x(5)=new_data.rx_v(2); + kf_x(6)=new_data.rx_dts(0); + kf_x(7)=new_data.rx_dts(1); // // Kalman state prediction (time update) -// kf_x_pri = kf_F * kf_x; // state prediction -// //kf_P_x_pri = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction + kf_x = kf_F * kf_x; // state prediction + kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction // //from error state variables to variables // //x_u=x_u0+kf_x_pri(0); @@ -75,70 +77,81 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // //zDot_u=zDot_u0+kf_x_pri(5); // //cdeltat_u=cdeltat_u0+kf_x_pri(6); // //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); -// //from state variables definition -// x_u=kf_x_pri(0); -// y_u=kf_x_pri(1); -// z_u=kf_x_pri(2); -// xDot_u=kf_x_pri(3); -// yDot_u=kf_x_pri(4); -// zDot_u=kf_x_pri(5); -// cdeltat_u=kf_x_pri(6); -// cdeltatDot_u=kf_x_pri(7); -// for (int32_t i = 0; i < n_sats; n++) //neccesary quantities -// { -// d(i)=sqrt(square(new_vtl_data.sat_p(i, 0)-x_u)+square(new_vtl_data.sat_p(i, 1)-y_u)+square(new_vtl_data.sat_p(i, 2)-z_u)); -// //compute pseudorange estimation -// rho_pri(i)=d(i)+cdeltat_u; -// //compute LOS sat-receiver vector components -// a_x(i)=-(new_vtl_data.sat_p(i, 0)-x_u)/d(i); -// a_y(i)=-(new_vtl_data.sat_p(i, 1)-y_u)/d(i);; -// a_z(i)=-(new_vtl_data.sat_p(i, 2)-z_u)/d(i);; -// //compute pseudorange rate estimation -// rhoDot_pri(i)=(new_vtl_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_vtl_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_vtl_data.sat_v(i, 2)-zDot_u)*a_z(i)+cdeltatDot_u; -// } +// From state variables definition + x_u=kf_x(0); + y_u=kf_x(1); + z_u=kf_x(2); + xDot_u=kf_x(3); + yDot_u=kf_x(4); + zDot_u=kf_x(5); + cdeltat_u=kf_x(6); + cdeltatDot_u=kf_x(7); -// kf_H = arma::zeros(8, 2*n_sats); + d = arma::zeros(new_data.sat_number, 1); + rho_pri = arma::zeros(new_data.sat_number, 1); + rhoDot_pri = arma::zeros(new_data.sat_number, 1); + a_x = arma::zeros(new_data.sat_number, 1); + a_y = arma::zeros(new_data.sat_number, 1); + a_z = arma::zeros(new_data.sat_number, 1); -// for (int32_t i = 0; i < n_sats; n++) // Measurement matrix H assembling -// { -// // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) -// kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; -// kf_H(i+n_sats, 3) = a_x(i); kf_H(i+n_sats, 4) = a_y(i); kf_H(i+n_sats, 5) = a_z(i); kf_H(i+n_sats, 7) = 1.0; -// } + for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities + { + d(i)=(sqrt((new_data.sat_p(i, 0)-x_u)*(new_data.sat_p(i, 0)-x_u)+(new_data.sat_p(i, 1)-y_u)*(new_data.sat_p(i, 1)-y_u)+(new_data.sat_p(i, 2)-z_u)*(new_data.sat_p(i, 2)-z_u))); + //compute pseudorange estimation + rho_pri(i)=d(i)+cdeltat_u; + //compute LOS sat-receiver vector components + a_x(i)=-(new_data.sat_p(i, 0)-x_u)/d(i); + a_y(i)=-(new_data.sat_p(i, 1)-y_u)/d(i);; + a_z(i)=-(new_data.sat_p(i, 2)-z_u)/d(i);; + //compute pseudorange rate estimation + rhoDot_pri(i)=(new_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_data.sat_v(i, 2)-zDot_u)*a_z(i)+cdeltatDot_u; + } -// // Kalman estimation (measurement update) -// for (int32_t i = 0; i < n_sats; n++) // Measurement vector -// { -// kf_y(i) = delta_rho(i); // i-Satellite -// kf_y(i+n_sats) = delta_rhoDot(i); // i-Satellite -// } + kf_H = arma::zeros(8, 2*new_data.sat_number); -// for (int32_t i = 0; i < n_sats; n++) // Measurement error Covariance Matrix R assembling -// { -// // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) -// kf_R(i, i) = 1.0; -// kf_R(i+n_sats, i+n_sats) = 1.0; -// } + for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling + { + // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) + kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; + kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; + } -// // Kalman filter update step -// // kf_P_y = kf_H * kf_P_x_pri * kf_H.t() + kf_R; // innovation covariance matrix (S) -// // kf_K = (kf_P_x_pri * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain + // Kalman estimation (measurement update) + for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector + { + //kf_y(i) = delta_rho(i); // i-Satellite + kf_y(i)=new_data.pr_m(i); + //kf_y(i+new_data.sat_number) = delta_rhoDot(i); // i-Satellite + kf_y(i+new_data.sat_number)=new_data.doppler_hz(i); + } + + for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling + { + // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) + kf_R(i, i) = 1.0; //TODO: use a real value. + kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0; + } + + // Kalman filter update step + kf_S = kf_H * kf_P_x* kf_H.t() + kf_R; // innovation covariance matrix (S) + kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain -// for (int32_t i = 0; i < n_sats; n++) //Error measurement vector -// { -// // kf_delta_y(i)=rho(i)+delta_rho(i)-rho_pri(i); // pseudorange error -// // kf_delta_y(i+n_sats)=rhoDot(i)+delta_F*(-lambdaC)-rhoDot_pri(i); // pseudorange rate error -// } + //for (int32_t i = 0; i < new_data.sat_number; i++) //Error measurement vector + //{ + // kf_delta_y(i)=new_data.pr_m(i)+delta_rho(i)-rho_pri(i); // pseudorange error + // kf_delta_y(i+new_data.sat_number)=new_data.doppler_hz(i)+delta_F*(-lambdaC)-rhoDot_pri(i); // pseudorange rate error + //} -// // kf_delta_x = kf_K * kf_delta_y; // updated error state estimation -// // kf_P_x = (arma::eye(size(kf_P_x_pri)) - kf_K * kf_H) * kf_P_x_pri; // update state estimation error covariance matrix + //kf_delta_x = kf_K * kf_delta_y; // updated error state estimation + kf_x = kf_K * (kf_y-dot(kf_H,kf_x)); // updated error state estimation + kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) // // ################## Geometric Transformation ###################################### -// for (int32_t i = 0; i < n_sats; n++) //neccesary quantities at posteriori +// for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori // { // //compute pseudorange posteriori estimation // // rho_est(i)=; @@ -150,13 +163,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // // rhoDot_est(i)=; // } -// kf_H = arma::zeros(8, 2*n_sats); +// kf_H = arma::zeros(8, 2*new_data.sat_number); -// for (int32_t i = 0; i < n_sats; n++) // Measurement matrix H posteriori assembling +// for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling // { // // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) // kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; -// kf_H(i+n_sats, 3) = a_x(i); kf_H(i+n_sats, 4) = a_y(i); kf_H(i+n_sats, 5) = a_z(i); kf_H(i+n_sats, 7) = 1.0; +// kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; // } // //Re-calculate error measurement vector with the most recent data available From 6f404626ff1a2ddbb41a1cd4a33d51f1c974d40c Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 10 Oct 2022 11:59:22 +0200 Subject: [PATCH 009/165] ADD: SNR and variance estimation in Vtl_Data --- src/algorithms/PVT/libs/rtklib_solver.cc | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index fd69b0b90..1d878f0fc 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1105,19 +1105,19 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; - + new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; new_vtl_data.sat_var(n) = var[n]; new_vtl_data.sat_health_flag(n) = svh.at(n); - + new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; } //VTL input data extraction from rtklib structures - /* Receiver position, velocitie and clock */ + /* Receiver position, velocity and clock */ /* position/velocity (m|m/s):{x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ new_vtl_data.rx_p(0) =pvt_sol.rr[0]; new_vtl_data.rx_p(1) =pvt_sol.rr[1]; @@ -1125,6 +1125,16 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_v(0) =pvt_sol.rr[3] ; new_vtl_data.rx_v(1) =pvt_sol.rr[4] ; new_vtl_data.rx_v(2) =pvt_sol.rr[5] ; + /* Receiver position, velocity and clock variances*/ + new_vtl_data.rx_pvt_var[0] = pvt_sol.qr[0]; + new_vtl_data.rx_pvt_var[1] = pvt_sol.qr[1]; + new_vtl_data.rx_pvt_var[2] = pvt_sol.qr[2]; + //TODO: get direct estimations for V T variances, instead: + new_vtl_data.rx_pvt_var[3] = pvt_sol.qr[0]*0.1; //in general minor than position. + new_vtl_data.rx_pvt_var[4] = pvt_sol.qr[1]*0.1; + new_vtl_data.rx_pvt_var[5] = pvt_sol.qr[2]*0.1; + new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time + new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler //receiver clock offset and receiver clock drift new_vtl_data.rx_dts(0)=rx_position_and_time[3]; new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]; From c34782762c25235b181cc9d6036e7ee56248f47d Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 10 Oct 2022 12:00:33 +0200 Subject: [PATCH 010/165] ADD: vtl variables and Q estimation. --- src/algorithms/PVT/libs/vtl_data.h | 4 +++- src/algorithms/PVT/libs/vtl_engine.cc | 10 ++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index d63dc865b..e8f6519f3 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -38,6 +38,7 @@ public: arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] arma::colvec sat_var; // sat position and clock error variance [m^2] arma::colvec sat_health_flag; // sat health flag (0 is ok) + arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz int sat_number; // on-view sat number arma::colvec pr_m; // Satellite Code pseudoranges [m] @@ -46,8 +47,9 @@ public: arma::mat rx_p; // Receiver ENU Position [m] arma::mat rx_v; // Receiver Velocity [m/s] + arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] - arma::colvec rx_var; // Receiver position and clock error variance [m^2] + arma::colvec rx_var; // Receiver position and clock error variance [m^2] // time handling double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index ea8abf83a..10fb0f831 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -64,6 +64,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x(6)=new_data.rx_dts(0); kf_x(7)=new_data.rx_dts(1); + for (int32_t i = 0; i < 8; i++) // State error Covariance Matrix Q (PVT) + { + // It is diagonal 8x8 matrix + kf_Q(i, i) = new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate. + } + // // Kalman state prediction (time update) kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction @@ -128,7 +134,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 1.0; //TODO: use a real value. + kf_R(i, i) = 1.0; //TODO: use a valid value. kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0; } @@ -143,7 +149,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //} //kf_delta_x = kf_K * kf_delta_y; // updated error state estimation - kf_x = kf_K * (kf_y-dot(kf_H,kf_x)); // updated error state estimation + kf_x = kf_x + kf_K * (kf_y-dot(kf_H,kf_x)); // updated state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) From bece48e90479a643976826e70f392032e459aa94 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 10 Oct 2022 16:12:31 +0200 Subject: [PATCH 011/165] FIX: vtl_engine debugged --- src/algorithms/PVT/libs/rtklib_solver.cc | 4 +- src/algorithms/PVT/libs/vtl_data.cc | 19 ++--- src/algorithms/PVT/libs/vtl_engine.cc | 90 +++++++++++++----------- 3 files changed, 62 insertions(+), 51 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 1d878f0fc..816851ca3 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1097,6 +1097,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.init_storage(n_sats); new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands + new_vtl_data.sat_number=n_sats; for (int n = 0; n < n_sats; n++) { new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; @@ -1139,9 +1140,10 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_dts(0)=rx_position_and_time[3]; new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]; - new_vtl_data.debug_print(); //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? vtl_engine.vtl_loop(new_vtl_data); + + new_vtl_data.debug_print(); } // compute Ground speed and COG double ground_speed_ms = 0.0; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 9cd1fef4d..c2a8f6bff 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -30,6 +30,7 @@ void Vtl_Data::init_storage(int n_sats) sat_dts = arma::mat(n_sats, 2); sat_var = arma::vec(n_sats); sat_health_flag = arma::vec(n_sats); + sat_CN0_dB_hz = arma::colvec(n_sats); int sat_number = n_sats; pr_m = arma::vec(n_sats); @@ -40,7 +41,7 @@ void Vtl_Data::init_storage(int n_sats) rx_v = arma::mat(1, 3); rx_dts = arma::mat(1, 2); rx_var = arma::vec(1); - + rx_pvt_var = arma::vec(8); epoch_tow_s = 0; sample_counter = 0; } @@ -48,13 +49,13 @@ void Vtl_Data::init_storage(int n_sats) void Vtl_Data::debug_print() { std::cout << "vtl_data debug print at RX TOW: " << epoch_tow_s << ", TRK sample counter: " << sample_counter << "\n"; - sat_p.print("VTL Sat Positions"); - sat_v.print("VTL Sat Velocities"); - sat_dts.print("VTL Sat clocks"); - sat_var.print("VTL Sat clock variances"); + // sat_p.print("VTL Sat Positions"); + // sat_v.print("VTL Sat Velocities"); + // sat_dts.print("VTL Sat clocks"); + // sat_var.print("VTL Sat clock variances"); sat_health_flag.print("VTL Sat health"); - - pr_m.print("Satellite Code pseudoranges [m]"); - doppler_hz.print("satellite Carrier Dopplers [Hz]"); - carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); + + // pr_m.print("Satellite Code pseudoranges [m]"); + // doppler_hz.print("satellite Carrier Dopplers [Hz]"); + // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 10fb0f831..f2da2ba86 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -28,14 +28,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; - using arma::dot; -// // ################## Kalman filter initialization ###################################### - -// // covariances (static) - kf_P_x_ini = arma::zeros(8, 8); //TODO: use a real value. - kf_x = arma::zeros(8, 1); - kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); - double kf_dt=1e-3; + // using arma::dot; + // ################## Kalman filter initialization ###################################### + // covariances (static) + kf_P_x = arma::zeros(8, 8); //TODO: use a real value. + kf_x = arma::zeros(8, 1); + kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); + double kf_dt=1e-1; kf_Q = arma::zeros(8, 8); kf_F = arma::zeros(8, 8); @@ -53,8 +52,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_y = arma::zeros(2*new_data.sat_number, 1); kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix -// // ################## Kalman Tracking ###################################### -// // receiver solution from rtklib_solver + // ################## Kalman Tracking ###################################### + // receiver solution from rtklib_solver kf_x(0)=new_data.rx_p(0); kf_x(1)=new_data.rx_p(1); kf_x(2)=new_data.rx_p(2); @@ -83,6 +82,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // //zDot_u=zDot_u0+kf_x_pri(5); // //cdeltat_u=cdeltat_u0+kf_x_pri(6); // //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); +// // //from error state variables to variables +// // //x_u=x_u0+kf_x_pri(0); +// // //y_u=y_u0+kf_x_pri(1); +// // //z_u=z_u0+kf_x_pri(2); +// // //xDot_u=xDot_u0+kf_x_pri(3); +// // //yDot_u=yDot_u0+kf_x_pri(4); +// // //zDot_u=zDot_u0+kf_x_pri(5); +// // //cdeltat_u=cdeltat_u0+kf_x_pri(6); +// // //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); // From state variables definition x_u=kf_x(0); y_u=kf_x(1); @@ -113,7 +121,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) rhoDot_pri(i)=(new_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_data.sat_v(i, 2)-zDot_u)*a_z(i)+cdeltatDot_u; } - kf_H = arma::zeros(8, 2*new_data.sat_number); + kf_H = arma::zeros(2*new_data.sat_number,8); for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling { @@ -149,45 +157,45 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //} //kf_delta_x = kf_K * kf_delta_y; // updated error state estimation - kf_x = kf_x + kf_K * (kf_y-dot(kf_H,kf_x)); // updated state estimation + kf_x = kf_x + kf_K * (kf_y-kf_H*kf_x); // updated state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix -// // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) +// // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) -// // ################## Geometric Transformation ###################################### +// // // ################## Geometric Transformation ###################################### -// for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori -// { -// //compute pseudorange posteriori estimation -// // rho_est(i)=; -// //compute LOS sat-receiver vector components posteriori -// // a_x(i)=; -// // a_y(i)=; -// // a_z(i)=; -// //compute pseudorange rate posteriori estimation -// // rhoDot_est(i)=; -// } +// // for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori +// // { +// // //compute pseudorange posteriori estimation +// // // rho_est(i)=; +// // //compute LOS sat-receiver vector components posteriori +// // // a_x(i)=; +// // // a_y(i)=; +// // // a_z(i)=; +// // //compute pseudorange rate posteriori estimation +// // // rhoDot_est(i)=; +// // } -// kf_H = arma::zeros(8, 2*new_data.sat_number); +// // kf_H = arma::zeros(8, 2*new_data.sat_number); -// for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling -// { -// // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) -// kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; -// kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; -// } +// // for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling +// // { +// // // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) +// // kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; +// // kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; +// // } -// //Re-calculate error measurement vector with the most recent data available -// //kf_delta_y=kf_H*kf_delta_x -// //Filtered pseudorange error measurement (in m): -// //delta_rho_filt=; -// //Filtered Doppler error measurement (in Hz): -// //delta_doppler_filt=; +// // //Re-calculate error measurement vector with the most recent data available +// // //kf_delta_y=kf_H*kf_delta_x +// // //Filtered pseudorange error measurement (in m): +// // //delta_rho_filt=; +// // //Filtered Doppler error measurement (in Hz): +// // //delta_doppler_filt=; - //TODO: Fill the tracking commands outputs - // Notice: keep the same satellite order as in the Vtl_Data matrices - // sample code +// //TODO: Fill the tracking commands outputs +// // Notice: keep the same satellite order as in the Vtl_Data matrices +// // sample code TrackingCmd trk_cmd; trk_cmd.carrier_freq_hz = 0; trk_cmd.carrier_freq_rate_hz_s = 0; From 229c9115e429546fdc898e790b337a60bef050e8 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 10 Oct 2022 16:13:17 +0200 Subject: [PATCH 012/165] FORMAT: cleaning comments --- src/algorithms/PVT/libs/vtl_engine.cc | 31 +++++++++------------------ 1 file changed, 10 insertions(+), 21 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index f2da2ba86..3456a3eda 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -28,7 +28,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; - // using arma::dot; // ################## Kalman filter initialization ###################################### // covariances (static) kf_P_x = arma::zeros(8, 8); //TODO: use a real value. @@ -72,26 +71,16 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // // Kalman state prediction (time update) kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction - -// //from error state variables to variables -// //x_u=x_u0+kf_x_pri(0); -// //y_u=y_u0+kf_x_pri(1); -// //z_u=z_u0+kf_x_pri(2); -// //xDot_u=xDot_u0+kf_x_pri(3); -// //yDot_u=yDot_u0+kf_x_pri(4); -// //zDot_u=zDot_u0+kf_x_pri(5); -// //cdeltat_u=cdeltat_u0+kf_x_pri(6); -// //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); -// // //from error state variables to variables -// // //x_u=x_u0+kf_x_pri(0); -// // //y_u=y_u0+kf_x_pri(1); -// // //z_u=z_u0+kf_x_pri(2); -// // //xDot_u=xDot_u0+kf_x_pri(3); -// // //yDot_u=yDot_u0+kf_x_pri(4); -// // //zDot_u=zDot_u0+kf_x_pri(5); -// // //cdeltat_u=cdeltat_u0+kf_x_pri(6); -// // //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); -// From state variables definition + //from error state variables to variables + //x_u=x_u0+kf_x_pri(0); + //y_u=y_u0+kf_x_pri(1); + //z_u=z_u0+kf_x_pri(2); + //xDot_u=xDot_u0+kf_x_pri(3); + //yDot_u=yDot_u0+kf_x_pri(4); + //zDot_u=zDot_u0+kf_x_pri(5); + //cdeltat_u=cdeltat_u0+kf_x_pri(6); + //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); + // From state variables definition x_u=kf_x(0); y_u=kf_x(1); z_u=kf_x(2); From 6fad047070c0a4fce3e21f3f2c06cee0fa682c2b Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 10 Oct 2022 18:30:51 +0200 Subject: [PATCH 013/165] [MOD] debug over state added --- src/algorithms/PVT/libs/vtl_data.cc | 4 +++- src/algorithms/PVT/libs/vtl_data.h | 1 + src/algorithms/PVT/libs/vtl_engine.cc | 10 ++++++++-- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index c2a8f6bff..8017692f9 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -42,6 +42,7 @@ void Vtl_Data::init_storage(int n_sats) rx_dts = arma::mat(1, 2); rx_var = arma::vec(1); rx_pvt_var = arma::vec(8); + kf_state = arma::vec(8); epoch_tow_s = 0; sample_counter = 0; } @@ -54,8 +55,9 @@ void Vtl_Data::debug_print() // sat_dts.print("VTL Sat clocks"); // sat_var.print("VTL Sat clock variances"); sat_health_flag.print("VTL Sat health"); + // kf_state.print("EKF STATE"); - // pr_m.print("Satellite Code pseudoranges [m]"); + pr_m.print("Satellite Code pseudoranges [m]"); // doppler_hz.print("satellite Carrier Dopplers [Hz]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index e8f6519f3..469151636 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -50,6 +50,7 @@ public: arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] arma::colvec rx_var; // Receiver position and clock error variance [m^2] + arma::colvec kf_state; // KF STATE // time handling double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 3456a3eda..1501154b9 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -15,6 +15,9 @@ */ #include "vtl_engine.h" +#include "iostream" + +using namespace std; Vtl_Engine::Vtl_Engine() { @@ -30,7 +33,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) using arma::as_scalar; // ################## Kalman filter initialization ###################################### // covariances (static) - kf_P_x = arma::zeros(8, 8); //TODO: use a real value. + kf_P_x = arma::eye(8, 8); //TODO: use a real value. kf_x = arma::zeros(8, 1); kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); double kf_dt=1e-1; @@ -69,8 +72,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } // // Kalman state prediction (time update) + cout << " KF RTKlib STATE" << kf_x; kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction + new_data.kf_state=kf_x; + cout << " KF priori STATE" << kf_x; //from error state variables to variables //x_u=x_u0+kf_x_pri(0); //y_u=y_u0+kf_x_pri(1); @@ -148,7 +154,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //kf_delta_x = kf_K * kf_delta_y; // updated error state estimation kf_x = kf_x + kf_K * (kf_y-kf_H*kf_x); // updated state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix - + cout << " KF posteriori STATE" << kf_x; // // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) From 73098ffe34f57d2debcb65a54449397c40deb75f Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 13 Oct 2022 13:33:10 +0200 Subject: [PATCH 014/165] Adding complete VTL loop messaging mechanism between PVT and Tracking. Debug prints enabled. --- conf/gnss-sdr_labsat_kf.conf | 4 +- conf/gnss-sdr_labsat_kf_vtl.conf | 187 ++++++++++++++++++ src/algorithms/PVT/adapters/rtklib_pvt.cc | 3 + .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 67 +++++-- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 3 + src/algorithms/PVT/libs/pvt_conf.h | 2 + src/algorithms/PVT/libs/rtklib_solver.cc | 131 ++++++------ src/algorithms/PVT/libs/rtklib_solver.h | 2 +- .../gnuradio_blocks/pcps_acquisition.cc | 1 + src/algorithms/libs/trackingcmd.h | 1 + .../tracking/gnuradio_blocks/kf_tracking.cc | 28 ++- .../tracking/gnuradio_blocks/kf_tracking.h | 3 + src/core/system_parameters/gnss_synchro.h | 5 + 13 files changed, 354 insertions(+), 83 deletions(-) create mode 100644 conf/gnss-sdr_labsat_kf_vtl.conf diff --git a/conf/gnss-sdr_labsat_kf.conf b/conf/gnss-sdr_labsat_kf.conf index 31787abab..15d77743b 100644 --- a/conf/gnss-sdr_labsat_kf.conf +++ b/conf/gnss-sdr_labsat_kf.conf @@ -16,7 +16,7 @@ SignalSource.selected_channel=1 ;# Labsat sile source automatically increments the file name when the signal is split in several files ;# the adapter adds "_0000.LS3" to this base path and filename. Next file will be "_0001.LS3" and so on ;# in this example, the first file complete path will be ../signals/GPS_025_ -SignalSource.filename=/home/javier/signals/satgen_30mins/output/output +SignalSource.filename=/home/javier/signals/satgen_30m/output/output SignalSource.item_type=gr_complex SignalSource.sampling_frequency=16368000 SignalSource.samples=0 @@ -99,7 +99,7 @@ Acquisition_1B.dump_filename=./acq_dump.dat ;######### TRACKING GPS CONFIG ############ Tracking_1C.implementation=GPS_L1_CA_KF_Tracking Tracking_1C.item_type=gr_complex -Tracking_1C.dump=true +Tracking_1C.dump=false Tracking_1C.dump_filename=./tracking_ch_ Tracking_1C.extend_correlation_symbols=20; Tracking_1C.early_late_space_chips=0.5; diff --git a/conf/gnss-sdr_labsat_kf_vtl.conf b/conf/gnss-sdr_labsat_kf_vtl.conf new file mode 100644 index 000000000..976df11d8 --- /dev/null +++ b/conf/gnss-sdr_labsat_kf_vtl.conf @@ -0,0 +1,187 @@ +; This is a GNSS-SDR configuration file +; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/ +; SPDX-License-Identifier: GPL-3.0-or-later +; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors) + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +GNSS-SDR.internal_fs_sps=5456000 +GNSS-SDR.use_acquisition_resampler=true + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=Labsat_Signal_Source +SignalSource.selected_channel=1 +;#filename: path to file with the captured GNSS signal samples to be processed +;# Labsat sile source automatically increments the file name when the signal is split in several files +;# the adapter adds "_0000.LS3" to this base path and filename. Next file will be "_0001.LS3" and so on +;# in this example, the first file complete path will be ../signals/GPS_025_ +SignalSource.filename=/home/javier/signals/satgen_30m/output/output +SignalSource.item_type=gr_complex +SignalSource.sampling_frequency=16368000 +SignalSource.samples=0 +SignalSource.repeat=false +SignalSource.dump=false +SignalSource.dump_filename=./out.dat +SignalSource.enable_throttle_control=false + + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner + +;######### DATA_TYPE_ADAPTER CONFIG ############ +DataTypeAdapter.implementation=Pass_Through +DataTypeAdapter.item_type=gr_complex + +;######### INPUT_FILTER CONFIG ############ +InputFilter.implementation=Freq_Xlating_Fir_Filter +InputFilter.dump=false +InputFilter.dump_filename=/media/javier/WDNASNTFS/output_5.456Msps_gr_complex.dat + +InputFilter.input_item_type=gr_complex +InputFilter.output_item_type=gr_complex +InputFilter.taps_item_type=float +InputFilter.number_of_taps=5 +InputFilter.number_of_bands=2 + +InputFilter.band1_begin=0.0 +InputFilter.band1_end=0.45 +InputFilter.band2_begin=0.55 +InputFilter.band2_end=1.0 + +InputFilter.ampl1_begin=1.0 +InputFilter.ampl1_end=1.0 +InputFilter.ampl2_begin=0.0 +InputFilter.ampl2_end=0.0 + +InputFilter.band1_error=1.0 +InputFilter.band2_error=1.0 + +InputFilter.filter_type=lowpass +InputFilter.grid_density=16 +InputFilter.sampling_frequency=16368000 +InputFilter.IF=0 +InputFilter.decimation_factor=3 + + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1C.count=6 +Channels_1B.count=0 +Channels_L5.count=0 +Channels_5X.count=0 + +Channels.in_acquisition=1 + +;######### GPS ACQUISITION CONFIG ############ +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.item_type=gr_complex +Acquisition_1C.threshold=3.0 +Acquisition_1C.use_CFAR_algorithm=false +Acquisition_1C.blocking=true +Acquisition_1C.doppler_max=5000 +Acquisition_1C.doppler_step=125 +Acquisition_1C.dump=false +Acquisition_1C.dump_filename=./acq_dump.dat + + +;######### GALILEO ACQUISITION CONFIG ############ +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.item_type=gr_complex +Acquisition_1B.threshold=2.8 +Acquisition_1B.use_CFAR_algorithm=false +Acquisition_1B.blocking=false +Acquisition_1B.doppler_max=5000 +Acquisition_1B.doppler_step=125 +Acquisition_1B.dump=false +Acquisition_1B.dump_filename=./acq_dump.dat + + +;######### TRACKING GPS CONFIG ############ +Tracking_1C.implementation=GPS_L1_CA_KF_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.dump=false +Tracking_1C.dump_filename=./tracking_ch_ +Tracking_1C.extend_correlation_symbols=1; +Tracking_1C.early_late_space_chips=0.5; +Tracking_1C.early_late_space_narrow_chips=0.15 + +;Tracking_1C.code_disc_sd_chips=0.2; // Initial R +;Tracking_1C.carrier_disc_sd_rads=0.3; // Initial R + +;Tracking_1C.init_code_phase_sd_chips=0.5; // Initial P_0_0 +;Tracking_1C.init_carrier_phase_sd_rad=0.7; +;Tracking_1C.init_carrier_freq_sd_hz=5; +;Tracking_1C.init_carrier_freq_rate_sd_hz_s=1; + +;Tracking_1C.code_phase_sd_chips=0.15; // Initial Q +;Tracking_1C.carrier_phase_sd_rad=0.25; +;Tracking_1C.carrier_freq_sd_hz=0.6; +;Tracking_1C.carrier_freq_rate_sd_hz_s=0.01; + + +;######### TRACKING GALILEO CONFIG ############ +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +Tracking_1B.item_type=gr_complex +Tracking_1B.pll_bw_hz=15.0; +Tracking_1B.dll_bw_hz=0.75; +Tracking_1B.early_late_space_chips=0.15; +Tracking_1B.very_early_late_space_chips=0.5; +Tracking_1B.early_late_space_narrow_chips=0.10; +Tracking_1B.very_early_late_space_narrow_chips=0.5; +Tracking_1B.pll_bw_narrow_hz=2.5 +Tracking_1B.dll_bw_narrow_hz=0.2 +Tracking_1B.extend_correlation_symbols=5 +Tracking_1B.track_pilot=true +Tracking_1B.enable_fll_pull_in=true; +;Tracking_1B.pull_in_time_s=60 +Tracking_1B.enable_fll_steady_state=false +Tracking_1B.fll_bw_hz=10 +Tracking_1B.dump=false +Tracking_1B.dump_filename=tracking_ch_ + +;######### TELEMETRY DECODER GALILEO CONFIG ############ +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false + + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false + + +;######### OBSERVABLES CONFIG ############ +;#implementation: +Observables.implementation=Hybrid_Observables +Observables.dump=false +Observables.dump_filename=./observables.dat +Observables.enable_carrier_smoothing=false +Observables.smoothing_factor=200 + + + +;######### PVT CONFIG ############ +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.enable_rx_clock_correction=false +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.output_rate_ms=100; +PVT.rinexobs_rate_ms=1000; +PVT.display_rate_ms=1000; +PVT.elevation_mask=15; +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 +PVT.dump=false +PVT.dump_filename=./PVT +PVT.enable_monitor=false +PVT.monitor_udp_port=1337 +PVT.monitor_client_addresses=127.0.0.1 +PVT.enable_vtl=false +PVT.close_vtl_loop=true + +;######### MONITOR CONFIG ############ +Monitor.enable_monitor=false +Monitor.decimation_factor=1 +Monitor.client_addresses=127.0.0.1 +Monitor.udp_port=1234 diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index ddadd7242..abb17567f 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -879,6 +879,9 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, // Use E6 for PVT pvt_output_parameters.use_e6_for_pvt = configuration->property(role + ".use_e6_for_pvt", pvt_output_parameters.use_e6_for_pvt); + // Vector Tracking Loop (VTL) + pvt_output_parameters.enable_vtl = configuration->property(role + ".enable_vtl", pvt_output_parameters.enable_vtl); + pvt_output_parameters.close_vtl_loop = configuration->property(role + ".close_vtl_loop", pvt_output_parameters.close_vtl_loop); // make PVT object pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index bb07f6558..e34bf8fa1 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -176,7 +176,9 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_enable_rx_clock_correction(conf_.enable_rx_clock_correction), d_an_printer_enabled(conf_.an_output_enabled), d_log_timetag(conf_.log_source_timetag), - d_use_e6_for_pvt(conf_.use_e6_for_pvt) + d_use_e6_for_pvt(conf_.use_e6_for_pvt), + d_enable_vtl(conf_.enable_vtl), + d_close_vtl_loop(conf_.close_vtl_loop) { // Send feedback message to observables block with the receiver clock offset this->message_port_register_out(pmt::mp("pvt_to_observables")); @@ -2131,9 +2133,57 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // old_time_debug = d_gnss_observables_map.cbegin()->second.RX_time * 1000.0; uint32_t current_RX_time_ms = 0; // #### solve PVT and store the corrected observable set - bool get_vtl_data = true; - if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false, get_vtl_data)) + if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false, d_enable_vtl, d_close_vtl_loop)) { + // ****** experimental VTL tests + if (d_close_vtl_loop == true) + { + std::map::const_iterator gnss_observables_iter; + for (gnss_observables_iter = d_gnss_observables_map.cbegin(); + gnss_observables_iter != d_gnss_observables_map.cend(); + ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system + { + // test complete loop + if (gnss_observables_iter->second.last_vtl_cmd_sample_counter == 0) + { + // send new tracking command + const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); + trk_cmd_test->carrier_freq_hz = 12345.4; + trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; + trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; + this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); + d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; + //std::cout << "msg pvt_to_trk sent.\n"; + } + else + { + // std::cout << "CH " << gnss_observables_iter->second.Channel_ID + // << " T_RX: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs) + // << " T_last_vtl_trk: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast(gnss_observables_iter->second.fs) + // << " T_map: " << static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast(gnss_observables_iter->second.fs) + // << " T2: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) + // << " T3: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) << "\n"; + + // To.Do: check if satellite change, check if there is a possibility to not find the last cmd timestamp in the map... + if (gnss_observables_iter->second.last_vtl_cmd_sample_counter >= d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) + { + std::cout << "CH " << gnss_observables_iter->second.Channel_ID << " processed VTL cmd, total loop time is " + << ((static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast(gnss_observables_iter->second.fs)) * 1000.0 + << " [ms]!\n"; + // send new tracking command + const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); + trk_cmd_test->carrier_freq_hz = 12345.4; + trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; + trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; + this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); + d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; + //std::cout << "msg pvt_to_trk sent.\n"; + } + } + } + } + // ***************************** + d_pvt_errors_counter = 0; // Reset consecutive PVT error counter const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); @@ -2246,18 +2296,13 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // compute on the fly PVT solution if (flag_compute_pvt_output == true) { - flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false); + // VTP To.Do: Check why get_PVT is triggered twice. Leave only one get_PVT. + + flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); } if (flag_pvt_valid == true) { - // experimental VTL tests - // send tracking command - // const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); - // trk_cmd_test->carrier_freq_hz = 12345.4; - // trk_cmd_test->sample_counter = d_gnss_observables_map.begin()->second.Tracking_sample_counter; - // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); - // initialize (if needed) the accumulated phase offset and apply it to the active channels // required to report accumulated phase cycles comparable to pseudoranges initialize_and_apply_carrier_phase_offset(); diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index cb8a2053b..3f803260b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -289,6 +289,9 @@ private: bool d_an_printer_enabled; bool d_log_timetag; bool d_use_e6_for_pvt; + bool d_enable_vtl; + bool d_close_vtl_loop; + std::map d_last_sent_vtl_cmd_samplestamp_map; }; diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index 34852c9c8..1924cc9eb 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -92,6 +92,8 @@ public: bool dump_mat = true; bool log_source_timetag; bool use_e6_for_pvt = true; + bool enable_vtl = false; + bool close_vtl_loop = true; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 816851ca3..ec584acce 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -465,7 +465,7 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const return d_monitor_pvt; } -bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging, bool get_vtl_data) +bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging, bool enable_vtl, bool close_vtl_loop) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -1064,87 +1064,86 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ rx_position_and_time[3] = pvt_sol.dtr[2] + pvt_sol.dtr[0] / SPEED_OF_LIGHT_M_S; } 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 - - if (get_vtl_data == true) - { - - //VTL input data extraction from rtklib structures - /* satellite positions, velocities and clocks */ - prcopt_t *opt = &d_rtk.opt; - /* count rover/base station observations */ - int n_sats = valid_obs + glo_valid_obs; - int nu; - int nr; - for (nu = 0; nu < n_sats && d_obs_data.data()[nu].rcv == 1; nu++) - { - } - for (nr = 0; nu + nr < n_sats && d_obs_data.data()[nu + nr].rcv == 2; nr++) - { - } - double *rs; - double *dts; - double *var; + if (enable_vtl == true) + { + //VTL input data extraction from rtklib structures + /* satellite positions, velocities and clocks */ + prcopt_t *opt = &d_rtk.opt; + /* count rover/base station observations */ + int n_sats = valid_obs + glo_valid_obs; + int nu; + int nr; + for (nu = 0; nu < n_sats && d_obs_data.data()[nu].rcv == 1; nu++) + { + } + for (nr = 0; nu + nr < n_sats && d_obs_data.data()[nu + nr].rcv == 2; nr++) + { + } - std::vector svh(MAXOBS * 2); - rs = mat(6, n_sats); - dts = mat(2, n_sats); - var = mat(1, n_sats); - /* satellite positions, velocities and clocks */ - satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); + double *rs; + double *dts; + double *var; - Vtl_Data new_vtl_data; - new_vtl_data.init_storage(n_sats); - new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; - new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands - new_vtl_data.sat_number=n_sats; - for (int n = 0; n < n_sats; n++) - { - new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; - new_vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; - new_vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; - new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; - new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; - new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; - - new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; - new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; - new_vtl_data.sat_var(n) = var[n]; - new_vtl_data.sat_health_flag(n) = svh.at(n); - new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; - // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; - new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; - new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; - } + std::vector svh(MAXOBS * 2); + rs = mat(6, n_sats); + dts = mat(2, n_sats); + var = mat(1, n_sats); + /* satellite positions, velocities and clocks */ + satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); + + Vtl_Data new_vtl_data; + new_vtl_data.init_storage(n_sats); + new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; + new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands + new_vtl_data.sat_number = n_sats; + for (int n = 0; n < n_sats; n++) + { + new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; + new_vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; + new_vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; + new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; + new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; + new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; + + new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; + new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; + new_vtl_data.sat_var(n) = var[n]; + new_vtl_data.sat_health_flag(n) = svh.at(n); + new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; + // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) + new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; + new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; + new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; + } //VTL input data extraction from rtklib structures /* Receiver position, velocity and clock */ /* position/velocity (m|m/s):{x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ - new_vtl_data.rx_p(0) =pvt_sol.rr[0]; - new_vtl_data.rx_p(1) =pvt_sol.rr[1]; - new_vtl_data.rx_p(2) =pvt_sol.rr[2]; - new_vtl_data.rx_v(0) =pvt_sol.rr[3] ; - new_vtl_data.rx_v(1) =pvt_sol.rr[4] ; - new_vtl_data.rx_v(2) =pvt_sol.rr[5] ; + new_vtl_data.rx_p(0) = pvt_sol.rr[0]; + new_vtl_data.rx_p(1) = pvt_sol.rr[1]; + new_vtl_data.rx_p(2) = pvt_sol.rr[2]; + new_vtl_data.rx_v(0) = pvt_sol.rr[3]; + new_vtl_data.rx_v(1) = pvt_sol.rr[4]; + new_vtl_data.rx_v(2) = pvt_sol.rr[5]; /* Receiver position, velocity and clock variances*/ new_vtl_data.rx_pvt_var[0] = pvt_sol.qr[0]; new_vtl_data.rx_pvt_var[1] = pvt_sol.qr[1]; new_vtl_data.rx_pvt_var[2] = pvt_sol.qr[2]; //TODO: get direct estimations for V T variances, instead: - new_vtl_data.rx_pvt_var[3] = pvt_sol.qr[0]*0.1; //in general minor than position. - new_vtl_data.rx_pvt_var[4] = pvt_sol.qr[1]*0.1; - new_vtl_data.rx_pvt_var[5] = pvt_sol.qr[2]*0.1; - new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time - new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler + new_vtl_data.rx_pvt_var[3] = pvt_sol.qr[0] * 0.1; //in general minor than position. + new_vtl_data.rx_pvt_var[4] = pvt_sol.qr[1] * 0.1; + new_vtl_data.rx_pvt_var[5] = pvt_sol.qr[2] * 0.1; + new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time + new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler //receiver clock offset and receiver clock drift - new_vtl_data.rx_dts(0)=rx_position_and_time[3]; - new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]; - + new_vtl_data.rx_dts(0) = rx_position_and_time[3]; + new_vtl_data.rx_dts(1) = pvt_sol.dtr[5]; + //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? vtl_engine.vtl_loop(new_vtl_data); new_vtl_data.debug_print(); - } + } // compute Ground speed and COG double ground_speed_ms = 0.0; std::array pos{}; diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index b03cc886d..0c8b3dd26 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -86,7 +86,7 @@ public: bool use_e6_for_pvt = true); ~Rtklib_Solver(); - bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging, bool get_vtl_data); + bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging, bool enable_vtl, bool close_vtl_loop); Vtl_Data vtl_data; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index a8ac0428e..4148a71c5 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -255,6 +255,7 @@ void pcps_acquisition::init() d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->last_vtl_cmd_sample_counter = 0ULL; d_mag = 0.0; d_input_power = 0.0; diff --git a/src/algorithms/libs/trackingcmd.h b/src/algorithms/libs/trackingcmd.h index 77115167b..7e51d5e97 100644 --- a/src/algorithms/libs/trackingcmd.h +++ b/src/algorithms/libs/trackingcmd.h @@ -34,6 +34,7 @@ public: double carrier_freq_hz = 0.0; double carrier_freq_rate_hz_s = 0.0; uint64_t sample_counter = 0UL; + uint32_t channel_id = 0; }; /** \} */ diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 53856bb4a..66ad1e51c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -129,7 +129,9 @@ kf_tracking::kf_tracking(const Kf_Conf &conf_) d_cloop(true), d_dump(d_trk_parameters.dump), d_dump_mat(d_trk_parameters.dump_mat && d_dump), - d_acc_carrier_phase_initialized(false) + d_acc_carrier_phase_initialized(false), + d_vtl_cmd_applied_now(false), + d_vtl_cmd_samplestamp(0LL) { // prevent telemetry symbols accumulation in output buffers this->set_max_noutput_items(1); @@ -626,8 +628,23 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr).hash_code()) { const auto cmd = wht::any_cast>(pmt::any_ref(msg)); - // std::cout << "RX pvt-to-trk cmd with delay: " - // << static_cast(nitems_read(0) - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n"; + if (cmd->channel_id == this->d_channel) + { + gr::thread::scoped_lock lock(d_setlock); + //To.Do: apply VTL corrections to the KF states + // code + // d_code_error_kf_chips; + // d_code_freq_kf_chips_s; + // // carrier + // d_carrier_phase_kf_rad; + // d_carrier_doppler_kf_hz; + // d_carrier_doppler_rate_kf_hz_s; + // set vtl corrections flag to inform VTL from gnss_synchro object + d_vtl_cmd_applied_now = true; + d_vtl_cmd_samplestamp = cmd->sample_counter; + std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " + << static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n"; + } } else { @@ -2071,6 +2088,11 @@ int kf_tracking::general_work(int noutput_items __attribute__((unused)), gr_vect { current_synchro_data.fs = static_cast(d_trk_parameters.fs_in); current_synchro_data.Tracking_sample_counter = d_sample_counter; + if (d_vtl_cmd_applied_now == true) + { + d_vtl_cmd_applied_now = false; + } + current_synchro_data.last_vtl_cmd_sample_counter = d_vtl_cmd_samplestamp; *out[0] = current_synchro_data; return 1; } diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.h b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.h index 37e38b679..bbdfc51cd 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.h @@ -230,6 +230,9 @@ private: bool d_dump; bool d_dump_mat; bool d_acc_carrier_phase_initialized; + // VTL cmd control + bool d_vtl_cmd_applied_now; + uint64_t d_vtl_cmd_samplestamp; bool d_enable_extended_integration; }; diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index f2934f5de..a8cab3ec8 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -80,6 +80,9 @@ public: bool Flag_valid_pseudorange{}; //!< Set by Observables processing block bool Flag_PLL_180_deg_phase_locked{}; //!< Set by Telemetry Decoder processing block + // VTL + uint64_t last_vtl_cmd_sample_counter{}; //!< Set by Tracking processing block + /// Copy constructor Gnss_Synchro(const Gnss_Synchro& other) noexcept { @@ -120,6 +123,7 @@ public: this->Flag_valid_word = rhs.Flag_valid_word; this->Flag_valid_pseudorange = rhs.Flag_valid_pseudorange; this->Flag_PLL_180_deg_phase_locked = rhs.Flag_PLL_180_deg_phase_locked; + this->last_vtl_cmd_sample_counter = rhs.last_vtl_cmd_sample_counter; } return *this; }; @@ -163,6 +167,7 @@ public: this->Flag_valid_word = other.Flag_valid_word; this->Flag_valid_pseudorange = other.Flag_valid_pseudorange; this->Flag_PLL_180_deg_phase_locked = other.Flag_PLL_180_deg_phase_locked; + this->last_vtl_cmd_sample_counter = other.last_vtl_cmd_sample_counter; } return *this; }; From 86f2e6b5d32c5dd78f83cc308be594284b3d4bdd Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 13 Oct 2022 18:13:35 +0200 Subject: [PATCH 015/165] FIX: VTL-KF --- src/algorithms/PVT/libs/rtklib_solver.cc | 4 +- src/algorithms/PVT/libs/vtl_data.cc | 2 +- src/algorithms/PVT/libs/vtl_engine.cc | 101 +++++++++++------------ src/algorithms/PVT/libs/vtl_engine.h | 3 + 4 files changed, 57 insertions(+), 53 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 816851ca3..60ae8f906 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -42,7 +42,9 @@ #include #include #include +#include "iostream" +using namespace std; Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, const std::string &dump_filename, @@ -1138,7 +1140,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler //receiver clock offset and receiver clock drift new_vtl_data.rx_dts(0)=rx_position_and_time[3]; - new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]; + new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]/1e6; // [ppm] to [s] //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? vtl_engine.vtl_loop(new_vtl_data); diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 8017692f9..cad9400e4 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -57,7 +57,7 @@ void Vtl_Data::debug_print() sat_health_flag.print("VTL Sat health"); // kf_state.print("EKF STATE"); - pr_m.print("Satellite Code pseudoranges [m]"); + //pr_m.print("Satellite Code pseudoranges [m]"); // doppler_hz.print("satellite Carrier Dopplers [Hz]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 1501154b9..c344202b2 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -52,6 +52,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_H = arma::zeros(8, 2*new_data.sat_number); kf_x = arma::zeros(8, 1); kf_y = arma::zeros(2*new_data.sat_number, 1); + kf_yerr = arma::zeros(2*new_data.sat_number, 1); + kf_xerr = arma::zeros(8, 1); kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix // ################## Kalman Tracking ###################################### @@ -73,19 +75,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // // Kalman state prediction (time update) cout << " KF RTKlib STATE" << kf_x; + new_data.kf_state=kf_x; kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction - new_data.kf_state=kf_x; - cout << " KF priori STATE" << kf_x; + // cout << " KF priori STATE diference" << kf_x-new_data.kf_state; //from error state variables to variables - //x_u=x_u0+kf_x_pri(0); - //y_u=y_u0+kf_x_pri(1); - //z_u=z_u0+kf_x_pri(2); - //xDot_u=xDot_u0+kf_x_pri(3); - //yDot_u=yDot_u0+kf_x_pri(4); - //zDot_u=zDot_u0+kf_x_pri(5); - //cdeltat_u=cdeltat_u0+kf_x_pri(6); - //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); + kf_xerr=kf_x-new_data.kf_state; // From state variables definition x_u=kf_x(0); y_u=kf_x(1); @@ -93,8 +88,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) xDot_u=kf_x(3); yDot_u=kf_x(4); zDot_u=kf_x(5); - cdeltat_u=kf_x(6); - cdeltatDot_u=kf_x(7); + cdeltat_u=kf_x(6)*SPEED_OF_LIGHT_M_S; + cdeltatDot_u=kf_x(7)*SPEED_OF_LIGHT_M_S; d = arma::zeros(new_data.sat_number, 1); rho_pri = arma::zeros(new_data.sat_number, 1); @@ -102,16 +97,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) a_x = arma::zeros(new_data.sat_number, 1); a_y = arma::zeros(new_data.sat_number, 1); a_z = arma::zeros(new_data.sat_number, 1); - for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities { - d(i)=(sqrt((new_data.sat_p(i, 0)-x_u)*(new_data.sat_p(i, 0)-x_u)+(new_data.sat_p(i, 1)-y_u)*(new_data.sat_p(i, 1)-y_u)+(new_data.sat_p(i, 2)-z_u)*(new_data.sat_p(i, 2)-z_u))); + d(i)=sqrt((new_data.sat_p(i, 0)-x_u)*(new_data.sat_p(i, 0)-x_u)+(new_data.sat_p(i, 1)-y_u)*(new_data.sat_p(i, 1)-y_u)+(new_data.sat_p(i, 2)-z_u)*(new_data.sat_p(i, 2)-z_u)); //compute pseudorange estimation - rho_pri(i)=d(i)+cdeltat_u; + rho_pri(i)=d(i);//+cdeltat_u; //compute LOS sat-receiver vector components a_x(i)=-(new_data.sat_p(i, 0)-x_u)/d(i); - a_y(i)=-(new_data.sat_p(i, 1)-y_u)/d(i);; - a_z(i)=-(new_data.sat_p(i, 2)-z_u)/d(i);; + a_y(i)=-(new_data.sat_p(i, 1)-y_u)/d(i); + a_z(i)=-(new_data.sat_p(i, 2)-z_u)/d(i); //compute pseudorange rate estimation rhoDot_pri(i)=(new_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_data.sat_v(i, 2)-zDot_u)*a_z(i)+cdeltatDot_u; } @@ -124,16 +118,17 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; } - // Kalman estimation (measurement update) for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector { //kf_y(i) = delta_rho(i); // i-Satellite kf_y(i)=new_data.pr_m(i); + kf_yerr(i)=kf_y(i)-rho_pri(i); //kf_y(i+new_data.sat_number) = delta_rhoDot(i); // i-Satellite - kf_y(i+new_data.sat_number)=new_data.doppler_hz(i); + kf_y(i+new_data.sat_number)=-new_data.doppler_hz(i)/0.1902937+cdeltatDot_u; + kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-rhoDot_pri(i); } - + //cout << " KF yerr" << kf_yerr; for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) @@ -152,45 +147,49 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //} //kf_delta_x = kf_K * kf_delta_y; // updated error state estimation - kf_x = kf_x + kf_K * (kf_y-kf_H*kf_x); // updated state estimation - kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix + //cout << " KF measures ERROR" << kf_yerr; + //cout << " KF measure ERROR stimation" << kf_H*kf_x; + cout << " KF innovation" << kf_yerr-kf_H*kf_xerr; + kf_x = kf_x + kf_K * (kf_yerr-kf_H*kf_xerr); // updated state estimation + kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix cout << " KF posteriori STATE" << kf_x; -// // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) + //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; + // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) -// // // ################## Geometric Transformation ###################################### +// // ################## Geometric Transformation ###################################### -// // for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori -// // { -// // //compute pseudorange posteriori estimation -// // // rho_est(i)=; -// // //compute LOS sat-receiver vector components posteriori -// // // a_x(i)=; -// // // a_y(i)=; -// // // a_z(i)=; -// // //compute pseudorange rate posteriori estimation -// // // rhoDot_est(i)=; -// // } +// for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori +// { +// //compute pseudorange posteriori estimation +// // rho_est(i)=; +// //compute LOS sat-receiver vector components posteriori +// // a_x(i)=; +// // a_y(i)=; +// // a_z(i)=; +// //compute pseudorange rate posteriori estimation +// // rhoDot_est(i)=; +// } -// // kf_H = arma::zeros(8, 2*new_data.sat_number); +// kf_H = arma::zeros(8, 2*new_data.sat_number); -// // for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling -// // { -// // // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) -// // kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; -// // kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; -// // } +// for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling +// { +// // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) +// kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; +// kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; +// } -// // //Re-calculate error measurement vector with the most recent data available -// // //kf_delta_y=kf_H*kf_delta_x -// // //Filtered pseudorange error measurement (in m): -// // //delta_rho_filt=; -// // //Filtered Doppler error measurement (in Hz): -// // //delta_doppler_filt=; +// //Re-calculate error measurement vector with the most recent data available +// //kf_delta_y=kf_H*kf_delta_x +// //Filtered pseudorange error measurement (in m): +// //delta_rho_filt=; +// //Filtered Doppler error measurement (in Hz): +// //delta_doppler_filt=; -// //TODO: Fill the tracking commands outputs -// // Notice: keep the same satellite order as in the Vtl_Data matrices -// // sample code + //TODO: Fill the tracking commands outputs + // Notice: keep the same satellite order as in the Vtl_Data matrices + // sample code TrackingCmd trk_cmd; trk_cmd.carrier_freq_hz = 0; trk_cmd.carrier_freq_rate_hz_s = 0; diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 64fcdb104..eb5ddc45d 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -20,6 +20,7 @@ #include "trackingcmd.h" #include "vtl_conf.h" #include "vtl_data.h" +#include "MATH_CONSTANTS.h" #include #include #include @@ -83,6 +84,8 @@ private: arma::colvec kf_x; // state vector arma::colvec kf_x_pre; // predicted state vector arma::colvec kf_y; // measurement vector + arma::colvec kf_yerr; // ERROR measurement vector + arma::colvec kf_xerr; // ERROR state vector arma::mat kf_K; // Kalman gain matrix // Gaussian estimator From b699173208bfeea94ffeeb2bc1ef5935874ed31c Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 13 Oct 2022 22:26:11 +0200 Subject: [PATCH 016/165] Extract code bias corrected PR, iono and tropo data from rtklib to VTL --- src/algorithms/PVT/libs/rtklib_solver.cc | 20 +++++++---- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 39 +++++++++++++++++---- src/algorithms/libs/rtklib/rtklib_pntpos.h | 10 ++++-- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 12 ++++--- src/algorithms/libs/rtklib/rtklib_rtkpos.h | 6 +++- src/algorithms/libs/rtklib/rtklib_rtksvr.cc | 7 +++- 6 files changed, 71 insertions(+), 23 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 7fe7119c5..0f59b6dbd 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -39,10 +39,10 @@ #include "rtklib_solution.h" #include #include +#include "iostream" #include #include #include -#include "iostream" using namespace std; @@ -1006,8 +1006,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_nav_data.lam[i][j] = satwavelen(i + 1, d_rtklib_freq_index[j], &d_nav_data); } } - - result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data); + std::vector tropo_vec; + std::vector iono_vec; + std::vector pr_corrected_code_bias_vec; + result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, tropo_vec, + iono_vec, pr_corrected_code_bias_vec); if (result == 0) { @@ -1114,7 +1117,10 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.sat_health_flag(n) = svh.at(n); new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; + //new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges + //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); + //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) + new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]; new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; } @@ -1138,9 +1144,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler //receiver clock offset and receiver clock drift - new_vtl_data.rx_dts(0)=rx_position_and_time[3]; - new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]/1e6; // [ppm] to [s] - + new_vtl_data.rx_dts(0) = rx_position_and_time[3]; + new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] + //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? vtl_engine.vtl_loop(new_vtl_data); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 594815996..f163ad1be 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -34,7 +34,7 @@ #include "rtklib_ionex.h" #include "rtklib_sbas.h" #include -#include + /* pseudorange measurement error variance ------------------------------------*/ double varerr(const prcopt_t *opt, double el, int sys) @@ -403,7 +403,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const double *x, const prcopt_t *opt, double *v, double *H, double *var, double *azel, int *vsat, - double *resp, int *ns) + double *resp, int *ns, double *tropo_m, double *iono_m, double *pr_corrected_code_bias) { double r; double dion; @@ -500,6 +500,11 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, /* pseudorange residual */ v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); + /* MOD ARRIBAS */ + pr_corrected_code_bias[i] = P; + tropo_m[i] = dtrp; + iono_m[i] = dion; + /* design matrix */ for (j = 0; j < NX; j++) { @@ -603,7 +608,8 @@ int valsol(const double *azel, const int *vsat, int n, int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, - double *resp, char *msg) + double *resp, char *msg, + std::vector &tropo_vec, std::vector &iono_vec, std::vector &pr_corrected_code_bias_vec) { double x[NX] = {0}; double dx[NX]; @@ -621,6 +627,11 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, int ns; char msg_aux[128]; + double *tropo_m, *iono_m, *pr_corrected_code_bias; + tropo_m = mat(n + 4, 1); + iono_m = mat(n + 4, 1); + pr_corrected_code_bias = mat(n + 4, 1); + trace(3, "estpos : n=%d\n", n); v = mat(n + 4, 1); @@ -635,7 +646,8 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, for (i = 0; i < MAXITR; i++) { /* pseudorange residuals */ - nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp, &ns); + nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp, + &ns, tropo_m, iono_m, pr_corrected_code_bias); if (nv < NX) { @@ -690,6 +702,13 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, { sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE; } + + for (k = 0; k < n; k++) + { + tropo_vec.push_back(tropo_m[k]); + iono_vec.push_back(iono_m[k]); + pr_corrected_code_bias_vec.push_back(pr_corrected_code_bias[k]); + } free(v); free(H); free(var); @@ -766,9 +785,12 @@ int raim_fde(const obsd_t *obs, int n, const double *rs, vare_e[k] = vare[j]; svh_e[k++] = svh[j]; } + std::vector tropo_vec; + std::vector iono_vec; + std::vector pr_corrected_code_bias_vec; /* estimate receiver position without a satellite */ if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, - vsat_e, resp_e, msg_e)) + vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec)) { trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg); continue; @@ -978,7 +1000,9 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, *-----------------------------------------------------------------------------*/ int pntpos(const obsd_t *obs, int n, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, - char *msg) + char *msg, std::vector &tropo_vec, + std::vector &iono_vec, + std::vector &pr_corrected_code_bias_vec) { prcopt_t opt_ = *opt; double *rs; @@ -1022,7 +1046,8 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh.data()); /* estimate receiver position with pseudorange */ - stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg); + stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg, + iono_vec, tropo_vec, pr_corrected_code_bias_vec); /* raim fde */ if (!stat && n >= 6 && opt->posopt[4]) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h index 735df2905..686e15d14 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.h +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.h @@ -35,6 +35,7 @@ #include "rtklib.h" #include "rtklib_rtkcmn.h" +#include /* constants -----------------------------------------------------------------*/ const int NX = 4 + 3; //!< # of estimated parameters @@ -92,7 +93,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const double *x, const prcopt_t *opt, double *v, double *H, double *var, double *azel, int *vsat, - double *resp, int *ns); + double *resp, int *ns, double *tropo_m, double *iono_m, double *pr_corrected_code_bias); /* validate solution ---------------------------------------------------------*/ int valsol(const double *azel, const int *vsat, int n, @@ -103,7 +104,8 @@ int valsol(const double *azel, const int *vsat, int n, int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, - double *resp, char *msg); + double *resp, char *msg, std::vector &tropo_vec, + std::vector &iono_vec, std::vector &pr_corrected_code_bias_vec); /* raim fde (failure detection and exclution) -------------------------------*/ int raim_fde(const obsd_t *obs, int n, const double *rs, @@ -140,6 +142,8 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, */ int pntpos(const obsd_t *obs, int n, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, - char *msg); + char *msg, std::vector &tropo_vec, + std::vector &iono_vec, + std::vector &pr_corrected_code_bias_vec); #endif // GNSS_SDR_RTKLIB_PNTPOS_H diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 2e13bde39..6afc7b56f 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -38,7 +38,6 @@ #include #include #include -#include static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)), const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute__((unused)), const nav_t *nav __attribute((unused)), @@ -2762,7 +2761,10 @@ void rtkfree(rtk_t *rtk) * notes : before calling function, base station position rtk->sol.rb[] should * be properly set for relative mode except for moving-baseline *-----------------------------------------------------------------------------*/ -int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, + std::vector &tropo_vec, + std::vector &iono_vec, + std::vector &pr_corrected_code_bias_vec) { prcopt_t *opt = &rtk->opt; sol_t solb = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; @@ -2797,7 +2799,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) time = rtk->sol.time; /* previous epoch */ /* rover position by single point positioning */ - if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg)) + if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg, tropo_vec, + iono_vec, pr_corrected_code_bias_vec)) { errmsg(rtk, "point pos error (%s)\n", msg); if (!rtk->opt.dynamics) @@ -2839,7 +2842,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) if (opt->mode == PMODE_MOVEB) { /* moving baseline */ /* estimate position/velocity of base station */ - if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg)) + if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, + iono_vec, pr_corrected_code_bias_vec)) { errmsg(rtk, "base station position error (%s)\n", msg); return 0; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.h b/src/algorithms/libs/rtklib/rtklib_rtkpos.h index 29f1924d2..9fcb7ea47 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.h @@ -34,6 +34,7 @@ #include "rtklib.h" #include "rtklib_rtkcmn.h" +#include /** \addtogroup PVT * \{ */ @@ -174,7 +175,10 @@ void rtkinit(rtk_t *rtk, const prcopt_t *opt); void rtkfree(rtk_t *rtk); -int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); +int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, + std::vector &tropo_vec, + std::vector &iono_vec, + std::vector &pr_corrected_code_bias_vec); /** \} */ diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index 975a61f86..5464c1ef3 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -38,6 +38,7 @@ #include "rtklib_solution.h" #include "rtklib_stream.h" #include +#include /* write solution header to output stream ------------------------------------*/ void writesolhead(stream_t *stream, const solopt_t *solopt) @@ -595,7 +596,11 @@ void *rtksvrthread(void *arg) } /* rtk positioning */ rtksvrlock(svr); - rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav); + std::vector tropo_vec; + std::vector iono_vec; + std::vector pr_corrected_code_bias_vec; + rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, tropo_vec, + iono_vec, pr_corrected_code_bias_vec); rtksvrunlock(svr); if (svr->rtk.sol.stat != SOLQ_NONE) From 3285b4d639dbc3d735f1618d7e6c39dbd05aff7a Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 13 Oct 2022 23:25:01 +0000 Subject: [PATCH 017/165] fix: VTL engine update --- src/algorithms/PVT/libs/rtklib_solver.cc | 3 ++- src/algorithms/PVT/libs/vtl_engine.cc | 33 +++++++++--------------- 2 files changed, 14 insertions(+), 22 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 0f59b6dbd..fb108b83c 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1120,7 +1120,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ //new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) - new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]; + //cout<<"dtr "< Date: Wed, 19 Oct 2022 19:35:47 +0200 Subject: [PATCH 018/165] MOD: Working with SPIRENT record, doppler and RAW pseudorange --- src/algorithms/PVT/libs/rtklib_solver.cc | 6 ++--- src/algorithms/PVT/libs/vtl_data.cc | 10 +++++--- src/algorithms/PVT/libs/vtl_data.h | 1 + src/algorithms/PVT/libs/vtl_engine.cc | 32 ++++++++++++++++++------ 4 files changed, 35 insertions(+), 14 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index fb108b83c..27b2d464b 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1117,11 +1117,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.sat_health_flag(n) = svh.at(n); new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - //new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges + new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) //cout<<"dtr "< &gnss_observables_ //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? vtl_engine.vtl_loop(new_vtl_data); - new_vtl_data.debug_print(); + //new_vtl_data.debug_print(); } // compute Ground speed and COG double ground_speed_ms = 0.0; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index cad9400e4..002b66a94 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -31,6 +31,7 @@ void Vtl_Data::init_storage(int n_sats) sat_var = arma::vec(n_sats); sat_health_flag = arma::vec(n_sats); sat_CN0_dB_hz = arma::colvec(n_sats); + sat_LOS = arma::mat(n_sats, 3); int sat_number = n_sats; pr_m = arma::vec(n_sats); @@ -51,13 +52,14 @@ void Vtl_Data::debug_print() { std::cout << "vtl_data debug print at RX TOW: " << epoch_tow_s << ", TRK sample counter: " << sample_counter << "\n"; // sat_p.print("VTL Sat Positions"); - // sat_v.print("VTL Sat Velocities"); + sat_v.print("VTL Sat Velocities"); // sat_dts.print("VTL Sat clocks"); // sat_var.print("VTL Sat clock variances"); - sat_health_flag.print("VTL Sat health"); + // sat_health_flag.print("VTL Sat health"); + sat_LOS.print("VTL SAT LOS"); // kf_state.print("EKF STATE"); - //pr_m.print("Satellite Code pseudoranges [m]"); - // doppler_hz.print("satellite Carrier Dopplers [Hz]"); + pr_m.print("Satellite Code pseudoranges [m]"); + doppler_hz.print("satellite Carrier Dopplers [Hz]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 469151636..79fbe96f5 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -39,6 +39,7 @@ public: arma::colvec sat_var; // sat position and clock error variance [m^2] arma::colvec sat_health_flag; // sat health flag (0 is ok) arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz + arma::mat sat_LOS; // sat LOS int sat_number; // on-view sat number arma::colvec pr_m; // Satellite Code pseudoranges [m] diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 3b0f26e8f..51057d781 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -76,12 +76,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // // Kalman state prediction (time update) cout << " KF RTKlib STATE" << kf_x; new_data.kf_state=kf_x; - kf_x = kf_F * kf_x; // state prediction + //kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction // cout << " KF priori STATE diference" << kf_x-new_data.kf_state; //from error state variables to variables kf_xerr=kf_x-new_data.kf_state; // From state variables definition + // TODO: cast to type properly x_u=kf_x(0); y_u=kf_x(1); z_u=kf_x(2); @@ -99,16 +100,27 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) a_z = arma::zeros(new_data.sat_number, 1); for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities { - d(i)=sqrt((new_data.sat_p(i, 0)-x_u)*(new_data.sat_p(i, 0)-x_u)+(new_data.sat_p(i, 1)-y_u)*(new_data.sat_p(i, 1)-y_u)+(new_data.sat_p(i, 2)-z_u)*(new_data.sat_p(i, 2)-z_u)); + //d(i) is the distance sat(i) to receiver + d(i)=(new_data.sat_p(i, 0)-x_u)*(new_data.sat_p(i, 0)-x_u); + d(i)=d(i)+(new_data.sat_p(i, 1)-y_u)*(new_data.sat_p(i, 1)-y_u); + d(i)=d(i)+(new_data.sat_p(i, 2)-z_u)*(new_data.sat_p(i, 2)-z_u); + d(i)=sqrt(d(i)); + //compute pseudorange estimation rho_pri(i)=d(i)+cdeltat_u; //compute LOS sat-receiver vector components a_x(i)=-(new_data.sat_p(i, 0)-x_u)/d(i); a_y(i)=-(new_data.sat_p(i, 1)-y_u)/d(i); a_z(i)=-(new_data.sat_p(i, 2)-z_u)/d(i); + new_data.sat_LOS(i,0)=a_x(i); + new_data.sat_LOS(i,1)=a_y(i); + new_data.sat_LOS(i,2)=a_z(i); //compute pseudorange rate estimation rhoDot_pri(i)=(new_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_data.sat_v(i, 2)-zDot_u)*a_z(i); + //rhoDot_pri(i)=(new_data.sat_v(i, 0)-0)*a_x(i)+(new_data.sat_v(i, 1)-0)*a_y(i)+(new_data.sat_v(i, 2)-0)*a_z(i); + } + cout << " V_LOS sat" << rhoDot_pri; kf_H = arma::zeros(2*new_data.sat_number,8); @@ -124,13 +136,18 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //kf_y(i) = delta_rho(i); // i-Satellite //kf_y(i+new_data.sat_number) = delta_rhoDot(i); // i-Satellite kf_y(i)=new_data.pr_m(i); - kf_yerr(i)=kf_y(i)-rho_pri(i)-0.000157*SPEED_OF_LIGHT_M_S; + kf_yerr(i)=kf_y(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S; float Lambda_GPS_L1=0.1902937; - kf_y(i+new_data.sat_number)=-new_data.doppler_hz(i)*Lambda_GPS_L1; - kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-rhoDot_pri(i); + kf_y(i+new_data.sat_number)=(rhoDot_pri(i))/Lambda_GPS_L1; + kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-new_data.doppler_hz(i); + + //rhoDot_pri(i)=(rhoDot_pri(i))/Lambda_GPS_L1; } - cout << " KF measurement vector difference" << kf_yerr; + //cout << " KF measurement vector difference" << kf_yerr; + cout << " kf_yerr" << kf_yerr; + //rhoDot_pri.print("DOPPLER stimated [Hz]"); + for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) @@ -146,7 +163,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x = kf_x + kf_K * (kf_yerr); // updated state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix //cout << " KF posteriori STATE" << kf_x; - cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; + //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; // // ################## Geometric Transformation ###################################### @@ -189,6 +206,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) trk_cmd.enable_code_nco_cmd = true; trk_cmd.sample_counter = new_data.sample_counter; trk_cmd_outs.push_back(trk_cmd); + new_data.debug_print(); return true; } From 90e3a5e1f8792472cb840e83d3e6d3a31db48d76 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 20 Oct 2022 12:53:41 +0200 Subject: [PATCH 019/165] MOD: output print signals --- src/algorithms/PVT/libs/vtl_engine.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 51057d781..b27731b4b 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -74,7 +74,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } // // Kalman state prediction (time update) - cout << " KF RTKlib STATE" << kf_x; + kf_x.print(" KF RTKlib STATE"); new_data.kf_state=kf_x; //kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction @@ -120,7 +120,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //rhoDot_pri(i)=(new_data.sat_v(i, 0)-0)*a_x(i)+(new_data.sat_v(i, 1)-0)*a_y(i)+(new_data.sat_v(i, 2)-0)*a_z(i); } - cout << " V_LOS sat" << rhoDot_pri; + rhoDot_pri.print("V_LOS sat"); kf_H = arma::zeros(2*new_data.sat_number,8); @@ -145,8 +145,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //rhoDot_pri(i)=(rhoDot_pri(i))/Lambda_GPS_L1; } //cout << " KF measurement vector difference" << kf_yerr; - cout << " kf_yerr" << kf_yerr; //rhoDot_pri.print("DOPPLER stimated [Hz]"); + kf_yerr.print("KF measurement vector difference"); for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling { From d03d94116f163667f00d9d187b7cec2de892fa81 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 23 Oct 2022 15:12:44 +0000 Subject: [PATCH 020/165] MOD: add vtlEKF time update and corrected pseudoranges --- src/algorithms/PVT/libs/rtklib_solver.cc | 4 ++-- src/algorithms/PVT/libs/vtl_engine.cc | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 27b2d464b..da16db15d 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1117,11 +1117,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.sat_health_flag(n) = svh.at(n); new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges + //new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) //cout<<"dtr "< Date: Tue, 25 Oct 2022 19:15:28 +0000 Subject: [PATCH 021/165] mod: cleaning and formating --- src/algorithms/PVT/libs/rtklib_solver.cc | 4 +-- src/algorithms/PVT/libs/vtl_data.h | 2 ++ src/algorithms/PVT/libs/vtl_engine.cc | 43 ++++++++++++------------ 3 files changed, 25 insertions(+), 24 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index da16db15d..27b2d464b 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1117,11 +1117,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.sat_health_flag(n) = svh.at(n); new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - //new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges + new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) //cout<<"dtr "< #include diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 13342dfb3..84cea0945 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -73,14 +73,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_Q(i, i) = new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate. } -// // Kalman state prediction (time update) + // Kalman state prediction (time update) kf_x.print(" KF RTKlib STATE"); new_data.kf_state=kf_x; kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction - // cout << " KF priori STATE diference" << kf_x-new_data.kf_state; //from error state variables to variables - kf_xerr=kf_x-new_data.kf_state; // From state variables definition // TODO: cast to type properly x_u=kf_x(0); @@ -133,37 +131,38 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // Kalman estimation (measurement update) for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector { - //kf_y(i) = delta_rho(i); // i-Satellite - //kf_y(i+new_data.sat_number) = delta_rhoDot(i); // i-Satellite - kf_y(i)=new_data.pr_m(i); - kf_yerr(i)=kf_y(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S; - - float Lambda_GPS_L1=0.1902937; - kf_y(i+new_data.sat_number)=(rhoDot_pri(i))/Lambda_GPS_L1; - kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-new_data.doppler_hz(i); - - //rhoDot_pri(i)=(rhoDot_pri(i))/Lambda_GPS_L1; + //kf_y(i) = new_data.pr_m(i); // i-Satellite + //kf_y(i+new_data.sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; // i-Satellite + kf_yerr(i)=new_data.pr_m(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S; + kf_yerr(i+new_data.sat_number)=rhoDot_pri(i)/Lambda_GPS_L1-new_data.doppler_hz(i); } - //cout << " KF measurement vector difference" << kf_yerr; - //rhoDot_pri.print("DOPPLER stimated [Hz]"); kf_yerr.print("KF measurement vector difference"); + // DOUBLES DIFFERENCES + // kf_yerr = arma::zeros(2*new_data.sat_number, 1); + // for (int32_t i = 1; i < new_data.sat_number; i++) // Measurement vector + // { + // kf_y(i)=new_data.pr_m(i)-new_data.pr_m(i-1); + // kf_yerr(i)=kf_y(i)-(rho_pri(i)+rho_pri(i-1)); + // kf_y(i+new_data.sat_number)=(rhoDot_pri(i)-rhoDot_pri(i-1))/Lambda_GPS_L1; + // kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-(new_data.doppler_hz(i)-new_data.doppler_hz(i-1)); + // } + // kf_yerr.print("DOUBLES DIFFERENCES"); + for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 0.1; //TODO: use a valid value. + kf_R(i, i) = 0.1; //TODO: fill with real values. kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0; } // Kalman filter update step - kf_S = kf_H * kf_P_x* kf_H.t() + kf_R; // innovation covariance matrix (S) - kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain - - //cout << " KF measures ERROR" << kf_yerr; + kf_S = kf_H * kf_P_x* kf_H.t() + kf_R; // innovation covariance matrix (S) + kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain kf_x = kf_x + kf_K * (kf_yerr); // updated state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix - //cout << " KF posteriori STATE" << kf_x; - //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; + + cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; // // ################## Geometric Transformation ###################################### From 122c53c1851e37ee90bd9030b7d6860bf6c773e5 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 12 Nov 2022 13:00:01 +0000 Subject: [PATCH 022/165] FIX: pseudorange rate error measurements --- src/algorithms/PVT/libs/rtklib_solver.cc | 5 +++-- src/algorithms/PVT/libs/vtl_data.cc | 4 ++-- src/algorithms/PVT/libs/vtl_engine.cc | 17 ++++++++--------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 27b2d464b..8be195bbe 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1117,11 +1117,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.sat_health_flag(n) = svh.at(n); new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges + //new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) //cout<<"dtr "< &gnss_observables_ new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? + new_vtl_data.debug_print(); vtl_engine.vtl_loop(new_vtl_data); //new_vtl_data.debug_print(); diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 002b66a94..e3e69558a 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -52,11 +52,11 @@ void Vtl_Data::debug_print() { std::cout << "vtl_data debug print at RX TOW: " << epoch_tow_s << ", TRK sample counter: " << sample_counter << "\n"; // sat_p.print("VTL Sat Positions"); - sat_v.print("VTL Sat Velocities"); + //sat_v.print("VTL Sat Velocities"); // sat_dts.print("VTL Sat clocks"); // sat_var.print("VTL Sat clock variances"); // sat_health_flag.print("VTL Sat health"); - sat_LOS.print("VTL SAT LOS"); + //sat_LOS.print("VTL SAT LOS"); // kf_state.print("EKF STATE"); pr_m.print("Satellite Code pseudoranges [m]"); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 84cea0945..74b92403e 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -64,7 +64,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x(3)=new_data.rx_v(0); kf_x(4)=new_data.rx_v(1); kf_x(5)=new_data.rx_v(2); - kf_x(6)=new_data.rx_dts(0); + kf_x(6)=new_data.rx_dts(0); kf_x(7)=new_data.rx_dts(1); for (int32_t i = 0; i < 8; i++) // State error Covariance Matrix Q (PVT) @@ -74,7 +74,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } // Kalman state prediction (time update) - kf_x.print(" KF RTKlib STATE"); + //kf_x.print(" KF RTKlib STATE"); new_data.kf_state=kf_x; kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction @@ -114,11 +114,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) new_data.sat_LOS(i,1)=a_y(i); new_data.sat_LOS(i,2)=a_z(i); //compute pseudorange rate estimation - rhoDot_pri(i)=(new_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_data.sat_v(i, 2)-zDot_u)*a_z(i); - //rhoDot_pri(i)=(new_data.sat_v(i, 0)-0)*a_x(i)+(new_data.sat_v(i, 1)-0)*a_y(i)+(new_data.sat_v(i, 2)-0)*a_z(i); - + rhoDot_pri(i)=(new_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_data.sat_v(i, 2)-zDot_u)*a_z(i)+cdeltatDot_u; } - rhoDot_pri.print("V_LOS sat"); kf_H = arma::zeros(2*new_data.sat_number,8); @@ -134,7 +131,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //kf_y(i) = new_data.pr_m(i); // i-Satellite //kf_y(i+new_data.sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; // i-Satellite kf_yerr(i)=new_data.pr_m(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S; - kf_yerr(i+new_data.sat_number)=rhoDot_pri(i)/Lambda_GPS_L1-new_data.doppler_hz(i); + //kf_yerr(i+new_data.sat_number)=(rhoDot_pri(i)-cdeltatDot_u)/Lambda_GPS_L1-new_data.doppler_hz(i); + kf_yerr(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(i); + } kf_yerr.print("KF measurement vector difference"); @@ -162,7 +161,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x = kf_x + kf_K * (kf_yerr); // updated state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix - cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; + //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; // // ################## Geometric Transformation ###################################### @@ -205,7 +204,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) trk_cmd.enable_code_nco_cmd = true; trk_cmd.sample_counter = new_data.sample_counter; trk_cmd_outs.push_back(trk_cmd); - new_data.debug_print(); + //new_data.debug_print(); return true; } From d0e215d3ddbda0af3ef58788fb9d2c782bd3bfc4 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 12 Nov 2022 13:06:44 +0000 Subject: [PATCH 023/165] fix: USER clock adjust from m/s to s --- src/algorithms/PVT/libs/vtl_engine.cc | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 74b92403e..8ca617e5b 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -74,7 +74,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } // Kalman state prediction (time update) - //kf_x.print(" KF RTKlib STATE"); + kf_x.print(" KF RTKlib STATE"); new_data.kf_state=kf_x; kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction @@ -131,7 +131,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //kf_y(i) = new_data.pr_m(i); // i-Satellite //kf_y(i+new_data.sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; // i-Satellite kf_yerr(i)=new_data.pr_m(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S; - //kf_yerr(i+new_data.sat_number)=(rhoDot_pri(i)-cdeltatDot_u)/Lambda_GPS_L1-new_data.doppler_hz(i); kf_yerr(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(i); } @@ -161,7 +160,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x = kf_x + kf_K * (kf_yerr); // updated state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix - //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; + // States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) + + kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; + kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; + + cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; // // ################## Geometric Transformation ###################################### From 9c7c5c9acf3ef2cf059e39af928cadd1fb3e88ff Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 12 Nov 2022 14:02:44 +0000 Subject: [PATCH 024/165] MOD: Sat instru delay accounted for --- src/algorithms/PVT/libs/rtklib_solver.cc | 4 ++-- src/algorithms/PVT/libs/vtl_engine.cc | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 8be195bbe..a3b253415 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1121,7 +1121,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) //cout<<"dtr "< &gnss_observables_ new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? - new_vtl_data.debug_print(); + //new_vtl_data.debug_print(); vtl_engine.vtl_loop(new_vtl_data); //new_vtl_data.debug_print(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 8ca617e5b..6c8407fb0 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -74,7 +74,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } // Kalman state prediction (time update) - kf_x.print(" KF RTKlib STATE"); + //kf_x.print(" KF RTKlib STATE"); new_data.kf_state=kf_x; kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction @@ -164,8 +164,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; - + new_data.kf_state.print(" KF RTKlib STATE"); cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; + cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100; // // ################## Geometric Transformation ###################################### @@ -208,7 +209,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) trk_cmd.enable_code_nco_cmd = true; trk_cmd.sample_counter = new_data.sample_counter; trk_cmd_outs.push_back(trk_cmd); - //new_data.debug_print(); + new_data.debug_print(); return true; } From 1155895f70cc620e597540e16d0b9dc465976a4e Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 12 Nov 2022 15:47:35 +0000 Subject: [PATCH 025/165] ADD: filtered geometric transformation added --- src/algorithms/PVT/libs/vtl_engine.cc | 73 ++++++++++++++++++--------- 1 file changed, 48 insertions(+), 25 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 6c8407fb0..92f73d353 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -157,8 +157,10 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // Kalman filter update step kf_S = kf_H * kf_P_x* kf_H.t() + kf_R; // innovation covariance matrix (S) kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain - kf_x = kf_x + kf_K * (kf_yerr); // updated state estimation + kf_xerr = kf_K * (kf_yerr); // Error state estimation + kf_x = kf_x + kf_xerr; // updated state estimation (a priori + error) kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix + // States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) @@ -170,34 +172,55 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // // ################## Geometric Transformation ###################################### -// for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori -// { -// //compute pseudorange posteriori estimation -// // rho_est(i)=; -// //compute LOS sat-receiver vector components posteriori -// // a_x(i)=; -// // a_y(i)=; -// // a_z(i)=; -// //compute pseudorange rate posteriori estimation -// // rhoDot_est(i)=; -// } +// // x_u=kf_x(0); +// // y_u=kf_x(1); +// // z_u=kf_x(2); +// // xDot_u=kf_x(3); +// // yDot_u=kf_x(4); +// // zDot_u=kf_x(5); +// // cdeltat_u=kf_x(6)*SPEED_OF_LIGHT_M_S; +// // cdeltatDot_u=kf_x(7)*SPEED_OF_LIGHT_M_S; -// kf_H = arma::zeros(8, 2*new_data.sat_number); + for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities + { + //d(i) is the distance sat(i) to receiver + d(i)=(new_data.sat_p(i, 0)-kf_x(0))*(new_data.sat_p(i, 0)-kf_x(0)); + d(i)=d(i)+(new_data.sat_p(i, 1)-kf_x(1))*(new_data.sat_p(i, 1)-kf_x(1)); + d(i)=d(i)+(new_data.sat_p(i, 2)-kf_x(2))*(new_data.sat_p(i, 2)-kf_x(2)); + d(i)=sqrt(d(i)); -// for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling -// { -// // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) -// kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; -// kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; -// } + //compute pseudorange estimation + rho_pri(i)=d(i)+kf_x(6)*SPEED_OF_LIGHT_M_S; + //compute LOS sat-receiver vector components + a_x(i)=-(new_data.sat_p(i, 0)-kf_x(0))/d(i); + a_y(i)=-(new_data.sat_p(i, 1)-kf_x(1))/d(i); + a_z(i)=-(new_data.sat_p(i, 2)-kf_x(2))/d(i); + //compute pseudorange rate estimation + rhoDot_pri(i)=(new_data.sat_v(i, 0)-kf_x(3))*a_x(i)+(new_data.sat_v(i, 1)-kf_x(4))*a_y(i)+(new_data.sat_v(i, 2)-kf_x(5))*a_z(i)+kf_x(7)*SPEED_OF_LIGHT_M_S; + } -// //Re-calculate error measurement vector with the most recent data available -// //kf_delta_y=kf_H*kf_delta_x -// //Filtered pseudorange error measurement (in m): -// //delta_rho_filt=; -// //Filtered Doppler error measurement (in Hz): -// //delta_doppler_filt=; + kf_H = arma::zeros(2*new_data.sat_number,8); + for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling + { + // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) + kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; + kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; + } + +// Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x + kf_yerr=kf_H*kf_xerr; +// Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): + + for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector + { + + rho_pri(i)=new_data.pr_m(i)-kf_yerr(i); // now filtered + rhoDot_pri(i)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-kf_yerr(i+new_data.sat_number); // now filtered + // TO DO: convert rhoDot_pri to doppler shift! + // Doppler shift defined as pseudorange rate measurement divided by the negative of carrier wavelength. + rhoDot_pri(i)=-rhoDot_pri(i)/Lambda_GPS_L1; + } //TODO: Fill the tracking commands outputs // Notice: keep the same satellite order as in the Vtl_Data matrices // sample code From 8bfdb1343e02cc754d08894c613ea6480b0019b1 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 13 Nov 2022 19:15:12 +0000 Subject: [PATCH 026/165] add: pseudorange residual --- src/algorithms/PVT/libs/rtklib_solver.cc | 7 +++++-- src/algorithms/PVT/libs/vtl_data.cc | 1 + src/algorithms/PVT/libs/vtl_data.h | 3 ++- src/algorithms/PVT/libs/vtl_engine.cc | 9 +++++---- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 21 +++++++++++++++------ src/algorithms/libs/rtklib/rtklib_pntpos.h | 4 +++- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 10 ++++++---- src/algorithms/libs/rtklib/rtklib_rtkpos.h | 4 +++- src/algorithms/libs/rtklib/rtklib_rtksvr.cc | 4 +++- 9 files changed, 43 insertions(+), 20 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index a3b253415..94494f5ac 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1009,9 +1009,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ std::vector tropo_vec; std::vector iono_vec; std::vector pr_corrected_code_bias_vec; + std::vector pr_residual_vec; + std::vector doppler_residual_vec; result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, tropo_vec, - iono_vec, pr_corrected_code_bias_vec); - + iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec); + if (result == 0) { LOG(INFO) << "RTKLIB rtkpos error: " << d_rtk.errbuf; @@ -1124,6 +1126,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]+SPEED_OF_LIGHT_M_S * dts[n * 2]; new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; + new_vtl_data.pr_res(n) = pr_residual_vec[n]; } //VTL input data extraction from rtklib structures /* Receiver position, velocity and clock */ diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index e3e69558a..2f4fccd83 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -37,6 +37,7 @@ void Vtl_Data::init_storage(int n_sats) pr_m = arma::vec(n_sats); doppler_hz = arma::vec(n_sats); carrier_phase_rads = arma::vec(n_sats); + pr_res = arma::vec(n_sats); rx_p = arma::mat(1, 3); rx_v = arma::mat(1, 3); diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 81ea16255..15e403476 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -47,7 +47,8 @@ public: arma::colvec pr_m; // Satellite Code pseudoranges [m] arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz] arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads] - + arma::colvec pr_res; // pseudorange residual + arma::mat rx_p; // Receiver ENU Position [m] arma::mat rx_v; // Receiver Velocity [m/s] arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 92f73d353..5ecb947c1 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -76,7 +76,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // Kalman state prediction (time update) //kf_x.print(" KF RTKlib STATE"); new_data.kf_state=kf_x; - kf_x = kf_F * kf_x; // state prediction + //kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction //from error state variables to variables // From state variables definition @@ -166,9 +166,10 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; - new_data.kf_state.print(" KF RTKlib STATE"); - cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; - cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100; + new_data.pr_res.print(" pr RESIDUALS"); + //new_data.kf_state.print(" KF RTKlib STATE"); + //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; + //cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100; // // ################## Geometric Transformation ###################################### diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index f163ad1be..d2824c4ba 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -609,7 +609,11 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, double *resp, char *msg, - std::vector &tropo_vec, std::vector &iono_vec, std::vector &pr_corrected_code_bias_vec) + std::vector &tropo_vec, + std::vector &iono_vec, + std::vector &pr_corrected_code_bias_vec, + std::vector &pr_residual_vec, + std::vector &doppler_residual_vec) { double x[NX] = {0}; double dx[NX]; @@ -630,6 +634,7 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, double *tropo_m, *iono_m, *pr_corrected_code_bias; tropo_m = mat(n + 4, 1); iono_m = mat(n + 4, 1); + resp = mat(n + 4, 1); pr_corrected_code_bias = mat(n + 4, 1); trace(3, "estpos : n=%d\n", n); @@ -708,6 +713,7 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, tropo_vec.push_back(tropo_m[k]); iono_vec.push_back(iono_m[k]); pr_corrected_code_bias_vec.push_back(pr_corrected_code_bias[k]); + pr_residual_vec.push_back(resp[k]); } free(v); free(H); @@ -788,9 +794,11 @@ int raim_fde(const obsd_t *obs, int n, const double *rs, std::vector tropo_vec; std::vector iono_vec; std::vector pr_corrected_code_bias_vec; + std::vector pr_residual_vec; + std::vector doppler_residual_vec; /* estimate receiver position without a satellite */ - if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, - vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec)) + if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, + vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec,pr_residual_vec,doppler_residual_vec)) { trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg); continue; @@ -1002,7 +1010,9 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, char *msg, std::vector &tropo_vec, std::vector &iono_vec, - std::vector &pr_corrected_code_bias_vec) + std::vector &pr_corrected_code_bias_vec, + std::vector &pr_residual_vec, + std::vector &doppler_residual_vec) { prcopt_t opt_ = *opt; double *rs; @@ -1047,8 +1057,7 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, /* estimate receiver position with pseudorange */ stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg, - iono_vec, tropo_vec, pr_corrected_code_bias_vec); - + iono_vec, tropo_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec); /* raim fde */ if (!stat && n >= 6 && opt->posopt[4]) { diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h index 686e15d14..531d79ffe 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.h +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.h @@ -144,6 +144,8 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, char *msg, std::vector &tropo_vec, std::vector &iono_vec, - std::vector &pr_corrected_code_bias_vec); + std::vector &pr_corrected_code_bias_vec, + std::vector &pr_residual, + std::vector &doppler_residual); #endif // GNSS_SDR_RTKLIB_PNTPOS_H diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 6afc7b56f..02ab2dce2 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -2764,7 +2764,9 @@ void rtkfree(rtk_t *rtk) int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, std::vector &tropo_vec, std::vector &iono_vec, - std::vector &pr_corrected_code_bias_vec) + std::vector &pr_corrected_code_bias_vec, + std::vector &pr_residual_vec, + std::vector &doppler_residual_vec) { prcopt_t *opt = &rtk->opt; sol_t solb = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; @@ -2800,7 +2802,7 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, /* rover position by single point positioning */ if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg, tropo_vec, - iono_vec, pr_corrected_code_bias_vec)) + iono_vec, pr_corrected_code_bias_vec,pr_residual_vec,doppler_residual_vec)) { errmsg(rtk, "point pos error (%s)\n", msg); if (!rtk->opt.dynamics) @@ -2842,8 +2844,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, if (opt->mode == PMODE_MOVEB) { /* moving baseline */ /* estimate position/velocity of base station */ - if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, - iono_vec, pr_corrected_code_bias_vec)) + if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, + iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec)) { errmsg(rtk, "base station position error (%s)\n", msg); return 0; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.h b/src/algorithms/libs/rtklib/rtklib_rtkpos.h index 9fcb7ea47..a75cf30fd 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.h @@ -178,7 +178,9 @@ void rtkfree(rtk_t *rtk); int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, std::vector &tropo_vec, std::vector &iono_vec, - std::vector &pr_corrected_code_bias_vec); + std::vector &pr_corrected_code_bias_vec, + std::vector &pr_residual_vec, + std::vector &doppler_residual_vec); /** \} */ diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index 5464c1ef3..998a067a1 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -599,8 +599,10 @@ void *rtksvrthread(void *arg) std::vector tropo_vec; std::vector iono_vec; std::vector pr_corrected_code_bias_vec; + std::vector pr_residual; + std::vector doppler_residual; rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, tropo_vec, - iono_vec, pr_corrected_code_bias_vec); + iono_vec, pr_corrected_code_bias_vec,pr_residual,doppler_residual); rtksvrunlock(svr); if (svr->rtk.sol.stat != SOLQ_NONE) From f734f8ca0ca9b9f5c916e346bae59e081f4b8ad3 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 21 Nov 2022 19:00:53 +0100 Subject: [PATCH 027/165] ADD: first KF close loop --- src/algorithms/PVT/libs/rtklib_solver.cc | 8 +++- src/algorithms/PVT/libs/vtl_data.cc | 5 ++- src/algorithms/PVT/libs/vtl_data.h | 1 + src/algorithms/PVT/libs/vtl_engine.cc | 57 +++++++++++++++--------- 4 files changed, 46 insertions(+), 25 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 94494f5ac..707367254 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1151,10 +1151,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_dts(0) = rx_position_and_time[3]; new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] - //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? //new_vtl_data.debug_print(); vtl_engine.vtl_loop(new_vtl_data); - + pvt_sol.rr[0]=new_vtl_data.kf_state(0);//rx_p + pvt_sol.rr[1]=new_vtl_data.kf_state(1);//rx_p + pvt_sol.rr[2]=new_vtl_data.kf_state(2);//rx_p + pvt_sol.rr[3]=new_vtl_data.kf_state(3);//rx_v + pvt_sol.rr[4]=new_vtl_data.kf_state(4);//rx_v + pvt_sol.rr[5]=new_vtl_data.kf_state(5);//rx_v //new_vtl_data.debug_print(); } // compute Ground speed and COG diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 2f4fccd83..4926aa2ee 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -45,6 +45,7 @@ void Vtl_Data::init_storage(int n_sats) rx_var = arma::vec(1); rx_pvt_var = arma::vec(8); kf_state = arma::vec(8); + kf_P = arma::mat(8,8); epoch_tow_s = 0; sample_counter = 0; } @@ -60,7 +61,7 @@ void Vtl_Data::debug_print() //sat_LOS.print("VTL SAT LOS"); // kf_state.print("EKF STATE"); - pr_m.print("Satellite Code pseudoranges [m]"); - doppler_hz.print("satellite Carrier Dopplers [Hz]"); + //pr_m.print("Satellite Code pseudoranges [m]"); + //doppler_hz.print("satellite Carrier Dopplers [Hz]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 15e403476..68d9dbcc1 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -55,6 +55,7 @@ public: arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] arma::colvec rx_var; // Receiver position and clock error variance [m^2] arma::colvec kf_state; // KF STATE + arma::mat kf_P; // KF STATE // time handling double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 5ecb947c1..cec5a725d 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -31,6 +31,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; + // ################## Kalman filter initialization ###################################### // covariances (static) kf_P_x = arma::eye(8, 8); //TODO: use a real value. @@ -55,17 +56,27 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_yerr = arma::zeros(2*new_data.sat_number, 1); kf_xerr = arma::zeros(8, 1); kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix - + // ################## Kalman Tracking ###################################### - // receiver solution from rtklib_solver - kf_x(0)=new_data.rx_p(0); - kf_x(1)=new_data.rx_p(1); - kf_x(2)=new_data.rx_p(2); - kf_x(3)=new_data.rx_v(0); - kf_x(4)=new_data.rx_v(1); - kf_x(5)=new_data.rx_v(2); - kf_x(6)=new_data.rx_dts(0); - kf_x(7)=new_data.rx_dts(1); + static uint32_t counter=0; //counter + counter=counter+1; //uint64_t + cout << "counter" << counter; + + if(counter<500){ // + // receiver solution from rtklib_solver + kf_x(0)=new_data.rx_p(0); + kf_x(1)=new_data.rx_p(1); + kf_x(2)=new_data.rx_p(2); + kf_x(3)=new_data.rx_v(0); + kf_x(4)=new_data.rx_v(1); + kf_x(5)=new_data.rx_v(2); + kf_x(6)=new_data.rx_dts(0); + kf_x(7)=new_data.rx_dts(1); + } + else{ + kf_x=new_data.kf_state; + kf_P_x=new_data.kf_P; + } for (int32_t i = 0; i < 8; i++) // State error Covariance Matrix Q (PVT) { @@ -74,9 +85,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } // Kalman state prediction (time update) - //kf_x.print(" KF RTKlib STATE"); - new_data.kf_state=kf_x; - //kf_x = kf_F * kf_x; // state prediction + // kf_x.print(" KF RTKlib STATE"); + new_data.kf_state=kf_x; + kf_x = kf_F * kf_x; // state prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction //from error state variables to variables // From state variables definition @@ -160,16 +171,20 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_xerr = kf_K * (kf_yerr); // Error state estimation kf_x = kf_x + kf_xerr; // updated state estimation (a priori + error) kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix - - + new_data.kf_state=kf_x; //updated state estimation + new_data.kf_P=kf_P_x; //update state estimation error covariance // States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) - kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; - kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; - new_data.pr_res.print(" pr RESIDUALS"); - //new_data.kf_state.print(" KF RTKlib STATE"); - //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; - //cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100; + // kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; + // kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; + + kf_x(6)=cdeltat_u/SPEED_OF_LIGHT_M_S; + kf_x(7)=cdeltatDot_u/SPEED_OF_LIGHT_M_S; + + //new_data.pr_res.print(" pr RESIDUALS"); + //!new_data.kf_state.print(" KF RTKlib STATE"); + //!cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; + //!cout << " KF posteriori STATE diference %1" << (kf_x-new_data.kf_state)/new_data.kf_state; // // ################## Geometric Transformation ###################################### From f5be456f91e69bd25495e576d6016696df59f01a Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 21 Nov 2022 23:47:50 +0000 Subject: [PATCH 028/165] MOD: kf filtering --- src/algorithms/PVT/libs/rtklib_solver.cc | 15 ++++++++------- src/algorithms/PVT/libs/vtl_data.cc | 2 +- src/algorithms/PVT/libs/vtl_data.h | 5 +++-- src/algorithms/PVT/libs/vtl_engine.cc | 6 +++--- src/algorithms/PVT/libs/vtl_engine.h | 10 +++++----- 5 files changed, 20 insertions(+), 18 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 707367254..5c2427761 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1152,13 +1152,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] //new_vtl_data.debug_print(); - vtl_engine.vtl_loop(new_vtl_data); - pvt_sol.rr[0]=new_vtl_data.kf_state(0);//rx_p - pvt_sol.rr[1]=new_vtl_data.kf_state(1);//rx_p - pvt_sol.rr[2]=new_vtl_data.kf_state(2);//rx_p - pvt_sol.rr[3]=new_vtl_data.kf_state(3);//rx_v - pvt_sol.rr[4]=new_vtl_data.kf_state(4);//rx_v - pvt_sol.rr[5]=new_vtl_data.kf_state(5);//rx_v + pvt_sol.rr[0]=vtl_engine.vtl_loop(new_vtl_data); + + // pvt_sol.rr[0]=new_vtl_data.rx_p(0); + // pvt_sol.rr[1]=new_vtl_data.rx_p(1); + // pvt_sol.rr[2]=new_vtl_data.rx_p(2); + // pvt_sol.rr[3]=new_vtl_data.rx_v(0); + // pvt_sol.rr[4]=new_vtl_data.rx_v(1); + // pvt_sol.rr[5]=new_vtl_data.rx_v(2); //new_vtl_data.debug_print(); } // compute Ground speed and COG diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 4926aa2ee..f03f26df8 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -44,7 +44,7 @@ void Vtl_Data::init_storage(int n_sats) rx_dts = arma::mat(1, 2); rx_var = arma::vec(1); rx_pvt_var = arma::vec(8); - kf_state = arma::vec(8); + kf_state = arma::mat(8,1); kf_P = arma::mat(8,8); epoch_tow_s = 0; sample_counter = 0; diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 68d9dbcc1..787c69f13 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -54,9 +54,10 @@ public: arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] arma::colvec rx_var; // Receiver position and clock error variance [m^2] - arma::colvec kf_state; // KF STATE - arma::mat kf_P; // KF STATE + arma::mat kf_state; // KF STATE + arma::mat kf_P; // KF STATE covariance // time handling + double PV[6]; // position and Velocity double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] void debug_print(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index cec5a725d..d50c0a105 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -62,7 +62,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) counter=counter+1; //uint64_t cout << "counter" << counter; - if(counter<500){ // + if(counter<3000){ // // receiver solution from rtklib_solver kf_x(0)=new_data.rx_p(0); kf_x(1)=new_data.rx_p(1); @@ -161,8 +161,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 0.1; //TODO: fill with real values. - kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0; + kf_R(i, i) = 20.0; //TODO: fill with real values. + kf_R(i+new_data.sat_number, i+new_data.sat_number) = 10.0; } // Kalman filter update step diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index eb5ddc45d..88e564508 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -81,11 +81,11 @@ private: arma::mat kf_R; // measurement error covariance matrix arma::mat kf_Q; // system error covariance matrix - arma::colvec kf_x; // state vector - arma::colvec kf_x_pre; // predicted state vector - arma::colvec kf_y; // measurement vector - arma::colvec kf_yerr; // ERROR measurement vector - arma::colvec kf_xerr; // ERROR state vector + arma::mat kf_x; // state vector + arma::mat kf_x_pre; // predicted state vector + arma::mat kf_y; // measurement vector + arma::mat kf_yerr; // ERROR measurement vector + arma::mat kf_xerr; // ERROR state vector arma::mat kf_K; // Kalman gain matrix // Gaussian estimator From e3daff4dcffb1dea8c003aabe2a0807cc3688ea9 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Tue, 22 Nov 2022 15:56:52 +0100 Subject: [PATCH 029/165] mod: output from vtl loop --- src/algorithms/PVT/libs/rtklib_solver.cc | 6 ++++-- src/algorithms/PVT/libs/vtl_engine.cc | 9 +++++++-- src/algorithms/PVT/libs/vtl_engine.h | 2 +- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 5c2427761..f19eafe9e 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1152,9 +1152,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] //new_vtl_data.debug_print(); - pvt_sol.rr[0]=vtl_engine.vtl_loop(new_vtl_data); + double PVT[6]={0}; + vtl_engine.vtl_loop(new_vtl_data, PVT); - // pvt_sol.rr[0]=new_vtl_data.rx_p(0); + pvt_sol.rr[0]=PVT[0]; + pvt_sol.rr[1]=PVT[1]; // pvt_sol.rr[1]=new_vtl_data.rx_p(1); // pvt_sol.rr[2]=new_vtl_data.rx_p(2); // pvt_sol.rr[3]=new_vtl_data.rx_v(0); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index d50c0a105..7deb893d9 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -27,7 +27,7 @@ Vtl_Engine::~Vtl_Engine() { } -bool Vtl_Engine::vtl_loop(Vtl_Data new_data) +bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6]) { //TODO: Implement main VTL loop here using arma::as_scalar; @@ -174,7 +174,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) new_data.kf_state=kf_x; //updated state estimation new_data.kf_P=kf_P_x; //update state estimation error covariance // States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) - + PVT_out[0]=kf_x(0); + PVT_out[1]=kf_x(1); + PVT_out[2]=kf_x(2); + PVT_out[3]=kf_x(3); + PVT_out[4]=kf_x(4); + PVT_out[5]=kf_x(5); // kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; // kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 88e564508..b9ae6a703 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -42,7 +42,7 @@ public: void configure(Vtl_Conf config_); //set config parameters //TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...) - bool vtl_loop(Vtl_Data new_data); + bool vtl_loop(Vtl_Data new_data, double PVT_out[6]); void reset(); // reset all internal states void debug_print(); // print debug information From 0ddc3626e9744fd98817ebaca74ed352455e20ed Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Tue, 22 Nov 2022 16:12:31 +0100 Subject: [PATCH 030/165] MOD: kf complete pvt output --- src/algorithms/PVT/libs/rtklib_solver.cc | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index f19eafe9e..10b7aec59 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1157,12 +1157,10 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ pvt_sol.rr[0]=PVT[0]; pvt_sol.rr[1]=PVT[1]; - // pvt_sol.rr[1]=new_vtl_data.rx_p(1); - // pvt_sol.rr[2]=new_vtl_data.rx_p(2); - // pvt_sol.rr[3]=new_vtl_data.rx_v(0); - // pvt_sol.rr[4]=new_vtl_data.rx_v(1); - // pvt_sol.rr[5]=new_vtl_data.rx_v(2); - //new_vtl_data.debug_print(); + pvt_sol.rr[2]=PVT[2]; + pvt_sol.rr[3]=PVT[3]; + pvt_sol.rr[4]=PVT[4]; + pvt_sol.rr[5]=PVT[5]; } // compute Ground speed and COG double ground_speed_ms = 0.0; From 25362551e63bda8859a5f0a3a8fe854708cd27bb Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Wed, 23 Nov 2022 14:42:25 +0100 Subject: [PATCH 031/165] mod: enable_vtl 'else' added --- src/algorithms/PVT/libs/rtklib_solver.cc | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 10b7aec59..7e438035d 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1154,7 +1154,6 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ //new_vtl_data.debug_print(); double PVT[6]={0}; vtl_engine.vtl_loop(new_vtl_data, PVT); - pvt_sol.rr[0]=PVT[0]; pvt_sol.rr[1]=PVT[1]; pvt_sol.rr[2]=PVT[2]; @@ -1162,6 +1161,17 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ pvt_sol.rr[4]=PVT[4]; pvt_sol.rr[5]=PVT[5]; } + else{ + //MAGL: the code should not enter here once the vtl has started + // .. but it does! + //and not only that but pvt_sol.rr seems to have NOT reasonable values + pvt_sol.rr[0]=rx_position_and_time[0]; // [m] + pvt_sol.rr[1]=rx_position_and_time[1]; // [m] + pvt_sol.rr[2]=rx_position_and_time[2]; // [m] + pvt_sol.rr[3]=4.2e6; + pvt_sol.rr[4]=4.2e6; + pvt_sol.rr[5]=4.2e6; + } // compute Ground speed and COG double ground_speed_ms = 0.0; std::array pos{}; From f716b040027a569b11e7aec33859d45fd45205cf Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 24 Nov 2022 17:04:18 +0000 Subject: [PATCH 032/165] ADD: vtl file output --- src/algorithms/PVT/libs/rtklib_solver.cc | 29 +++++----- src/algorithms/PVT/libs/vtl_engine.cc | 73 +++++++++++++++++------- 2 files changed, 68 insertions(+), 34 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 7e438035d..f9030d641 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1053,6 +1053,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ dops(index_aux, azel.data(), 0.0, d_dop.data()); } this->set_valid_position(true); + //this->set_averaging_flag(true); + //this->set_averaging_depth(100); + //this->perform_pos_averaging(); std::array rx_position_and_time{}; rx_position_and_time[0] = pvt_sol.rr[0]; // [m] rx_position_and_time[1] = pvt_sol.rr[1]; // [m] @@ -1119,10 +1122,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.sat_health_flag(n) = svh.at(n); new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - //new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) - //cout<<"dtr "< &gnss_observables_ //new_vtl_data.debug_print(); double PVT[6]={0}; vtl_engine.vtl_loop(new_vtl_data, PVT); - pvt_sol.rr[0]=PVT[0]; - pvt_sol.rr[1]=PVT[1]; - pvt_sol.rr[2]=PVT[2]; - pvt_sol.rr[3]=PVT[3]; - pvt_sol.rr[4]=PVT[4]; - pvt_sol.rr[5]=PVT[5]; + // pvt_sol.rr[0]=PVT[0]; + // pvt_sol.rr[1]=PVT[1]; + // pvt_sol.rr[2]=PVT[2]; + // pvt_sol.rr[3]=PVT[3]; + // pvt_sol.rr[4]=PVT[4]; + // pvt_sol.rr[5]=PVT[5]; } else{ //MAGL: the code should not enter here once the vtl has started // .. but it does! //and not only that but pvt_sol.rr seems to have NOT reasonable values - pvt_sol.rr[0]=rx_position_and_time[0]; // [m] - pvt_sol.rr[1]=rx_position_and_time[1]; // [m] - pvt_sol.rr[2]=rx_position_and_time[2]; // [m] - pvt_sol.rr[3]=4.2e6; - pvt_sol.rr[4]=4.2e6; - pvt_sol.rr[5]=4.2e6; + // pvt_sol.rr[0]=rx_position_and_time[0]; // [m] + // pvt_sol.rr[1]=rx_position_and_time[1]; // [m] + // pvt_sol.rr[2]=rx_position_and_time[2]; // [m] + // pvt_sol.rr[3]=4.2e6; + // pvt_sol.rr[4]=4.2e6; + // pvt_sol.rr[5]=4.2e6; } // compute Ground speed and COG double ground_speed_ms = 0.0; diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 7deb893d9..817f0cbe0 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -16,6 +16,7 @@ #include "vtl_engine.h" #include "iostream" +#include using namespace std; @@ -37,7 +38,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6]) kf_P_x = arma::eye(8, 8); //TODO: use a real value. kf_x = arma::zeros(8, 1); kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); - double kf_dt=1e-1; + double kf_dt=0.1; kf_Q = arma::zeros(8, 8); kf_F = arma::zeros(8, 8); @@ -59,8 +60,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6]) // ################## Kalman Tracking ###################################### static uint32_t counter=0; //counter + static double prueba[6]={0}; //counter counter=counter+1; //uint64_t - cout << "counter" << counter; + cout << "counter" << counter< Date: Sun, 27 Nov 2022 00:25:42 +0100 Subject: [PATCH 033/165] MOD: reference use of Vtl_data --- src/algorithms/PVT/libs/rtklib_solver.cc | 89 ++++++++++++------------ src/algorithms/PVT/libs/vtl_data.cc | 7 +- src/algorithms/PVT/libs/vtl_engine.cc | 83 +++++++++------------- src/algorithms/PVT/libs/vtl_engine.h | 2 +- 4 files changed, 83 insertions(+), 98 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index f9030d641..65503c9d1 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1102,65 +1102,66 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ /* satellite positions, velocities and clocks */ satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); - Vtl_Data new_vtl_data; - new_vtl_data.init_storage(n_sats); - new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; - new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands - new_vtl_data.sat_number = n_sats; + //Vtl_Data vtl_data; + vtl_data.init_storage(n_sats); + vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; + vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands + vtl_data.sat_number = n_sats; for (int n = 0; n < n_sats; n++) { - new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; - new_vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; - new_vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; - new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; - new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; - new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; + vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; + vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; + vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; + vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; + vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; + vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; - new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; - new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; - new_vtl_data.sat_var(n) = var[n]; - new_vtl_data.sat_health_flag(n) = svh.at(n); - new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; + vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; + vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; + vtl_data.sat_var(n) = var[n]; + vtl_data.sat_health_flag(n) = svh.at(n); + vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) - new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]+SPEED_OF_LIGHT_M_S * dts[n * 2]; - new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; - new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; - new_vtl_data.pr_res(n) = pr_residual_vec[n]; + vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]+SPEED_OF_LIGHT_M_S * dts[n * 2]; + vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; + vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; + vtl_data.pr_res(n) = pr_residual_vec[n]; } //VTL input data extraction from rtklib structures /* Receiver position, velocity and clock */ /* position/velocity (m|m/s):{x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ - new_vtl_data.rx_p(0) = pvt_sol.rr[0]; - new_vtl_data.rx_p(1) = pvt_sol.rr[1]; - new_vtl_data.rx_p(2) = pvt_sol.rr[2]; - new_vtl_data.rx_v(0) = pvt_sol.rr[3]; - new_vtl_data.rx_v(1) = pvt_sol.rr[4]; - new_vtl_data.rx_v(2) = pvt_sol.rr[5]; + vtl_data.rx_p(0) = pvt_sol.rr[0]; + vtl_data.rx_p(1) = pvt_sol.rr[1]; + vtl_data.rx_p(2) = pvt_sol.rr[2]; + vtl_data.rx_v(0) = pvt_sol.rr[3]; + vtl_data.rx_v(1) = pvt_sol.rr[4]; + vtl_data.rx_v(2) = pvt_sol.rr[5]; /* Receiver position, velocity and clock variances*/ - new_vtl_data.rx_pvt_var[0] = pvt_sol.qr[0]; - new_vtl_data.rx_pvt_var[1] = pvt_sol.qr[1]; - new_vtl_data.rx_pvt_var[2] = pvt_sol.qr[2]; + vtl_data.rx_pvt_var[0] = pvt_sol.qr[0]; + vtl_data.rx_pvt_var[1] = pvt_sol.qr[1]; + vtl_data.rx_pvt_var[2] = pvt_sol.qr[2]; //TODO: get direct estimations for V T variances, instead: - new_vtl_data.rx_pvt_var[3] = pvt_sol.qr[0] * 0.1; //in general minor than position. - new_vtl_data.rx_pvt_var[4] = pvt_sol.qr[1] * 0.1; - new_vtl_data.rx_pvt_var[5] = pvt_sol.qr[2] * 0.1; - new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time - new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler + vtl_data.rx_pvt_var[3] = pvt_sol.qr[0] * 0.1; //in general minor than position. + vtl_data.rx_pvt_var[4] = pvt_sol.qr[1] * 0.1; + vtl_data.rx_pvt_var[5] = pvt_sol.qr[2] * 0.1; + vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time + vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler //receiver clock offset and receiver clock drift - new_vtl_data.rx_dts(0) = rx_position_and_time[3]; - new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] + vtl_data.rx_dts(0) = rx_position_and_time[3]; + vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] //new_vtl_data.debug_print(); - double PVT[6]={0}; - vtl_engine.vtl_loop(new_vtl_data, PVT); - // pvt_sol.rr[0]=PVT[0]; - // pvt_sol.rr[1]=PVT[1]; - // pvt_sol.rr[2]=PVT[2]; - // pvt_sol.rr[3]=PVT[3]; - // pvt_sol.rr[4]=PVT[4]; - // pvt_sol.rr[5]=PVT[5]; + //vtl_data.kf_state.print("kf_state_input"); + vtl_engine.vtl_loop(vtl_data); + //vtl_data.kf_state.print("kf_state_output"); + // pvt_sol.rr[0] = vtl_data.kf_state[0]; + // pvt_sol.rr[1] = vtl_data.kf_state[1]; + // pvt_sol.rr[2] = vtl_data.kf_state[2]; + // pvt_sol.rr[3] = vtl_data.kf_state[3]; + // pvt_sol.rr[4] = vtl_data.kf_state[4]; + // pvt_sol.rr[5] = vtl_data.kf_state[5]; } else{ //MAGL: the code should not enter here once the vtl has started diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index f03f26df8..f1c5e48b8 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -21,6 +21,8 @@ Vtl_Data::Vtl_Data() { epoch_tow_s = 0; sample_counter = 0; + kf_state = arma::mat(8,1); + kf_P = arma::mat(8,8); } void Vtl_Data::init_storage(int n_sats) @@ -44,8 +46,7 @@ void Vtl_Data::init_storage(int n_sats) rx_dts = arma::mat(1, 2); rx_var = arma::vec(1); rx_pvt_var = arma::vec(8); - kf_state = arma::mat(8,1); - kf_P = arma::mat(8,8); + epoch_tow_s = 0; sample_counter = 0; } @@ -59,7 +60,7 @@ void Vtl_Data::debug_print() // sat_var.print("VTL Sat clock variances"); // sat_health_flag.print("VTL Sat health"); //sat_LOS.print("VTL SAT LOS"); - // kf_state.print("EKF STATE"); + kf_state.print("EKF STATE"); //pr_m.print("Satellite Code pseudoranges [m]"); //doppler_hz.print("satellite Carrier Dopplers [Hz]"); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 817f0cbe0..eb96c739c 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -28,7 +28,7 @@ Vtl_Engine::~Vtl_Engine() { } -bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6]) +bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; @@ -59,31 +59,31 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6]) kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix // ################## Kalman Tracking ###################################### - static uint32_t counter=0; //counter - static double prueba[6]={0}; //counter - counter=counter+1; //uint64_t + static uint32_t counter=0; //counter + counter = counter+1; //uint64_t cout << "counter" << counter< Date: Sun, 4 Dec 2022 22:19:49 +0000 Subject: [PATCH 034/165] MOD: kf weights --- src/algorithms/PVT/libs/vtl_engine.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index eb96c739c..3c0871b12 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -35,7 +35,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // ################## Kalman filter initialization ###################################### // covariances (static) - kf_P_x = arma::eye(8, 8); //TODO: use a real value. + kf_P_x = arma::eye(8, 8)*100.0; //TODO: use a real value. kf_x = arma::zeros(8, 1); kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); double kf_dt=0.1; @@ -90,13 +90,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) for (int32_t i = 0; i < 8; i++) { // It is diagonal 8x8 matrix - kf_Q(i, i) = 50.0;//new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate. + kf_Q(i, i) = 1.0;//new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate. } // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 5.0; //TODO: fill with real values. + kf_R(i, i) = 40.0; //TODO: fill with real values. kf_R(i+new_data.sat_number, i+new_data.sat_number) = 10.0; } From 4831c78898281907a485ebc2e0caa72af49a3508 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 4 Dec 2022 22:20:18 +0000 Subject: [PATCH 035/165] ADD: P&V of 5 sats in view in mat file --- src/algorithms/PVT/libs/rtklib_solver.cc | 88 +++++++++++++++++++++++- 1 file changed, 87 insertions(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 65503c9d1..c1a293d65 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -204,11 +204,13 @@ bool Rtklib_Solver::save_matfile() const { // READ DUMP FILE const std::string dump_filename = d_dump_filename; - const int32_t number_of_double_vars = 21; + const int32_t number_of_double_vars = 22; //+1 MAGL + const int32_t number_of_double_vars_sat = 6; //+ pos/vel por satelite const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + + sizeof(double) * number_of_double_vars_sat*5+ sizeof(uint32_t) * number_of_uint32_vars + sizeof(uint8_t) * number_of_uint8_vars + sizeof(float) * number_of_float_vars; @@ -253,6 +255,15 @@ bool Rtklib_Solver::save_matfile() const auto cov_xy = std::vector(num_epoch); auto cov_yz = std::vector(num_epoch); auto cov_zx = std::vector(num_epoch); + + auto vel_z2 = std::vector(num_epoch); + auto sat_posX_m = std::vector>(5, std::vector(num_epoch)); + auto sat_posY_m = std::vector>(5, std::vector(num_epoch)); + auto sat_posZ_m = std::vector>(5, std::vector(num_epoch)); + auto sat_velX = std::vector>(5, std::vector(num_epoch)); + auto sat_velY = std::vector>(5, std::vector(num_epoch)); + auto sat_velZ = std::vector>(5, std::vector(num_epoch)); + auto latitude = std::vector(num_epoch); auto longitude = std::vector(num_epoch); auto height = std::vector(num_epoch); @@ -288,6 +299,17 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&cov_xy[i]), sizeof(double)); dump_file.read(reinterpret_cast(&cov_yz[i]), sizeof(double)); dump_file.read(reinterpret_cast(&cov_zx[i]), sizeof(double)); + + dump_file.read(reinterpret_cast(&vel_z2[i]), sizeof(double)); + for (uint32_t chan = 0; chan < 5; chan++) + { + dump_file.read(reinterpret_cast(&sat_posX_m[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_posY_m[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_posZ_m[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_velX[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_velY[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_velZ[chan][i]), sizeof(double)); + } dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); @@ -309,6 +331,28 @@ bool Rtklib_Solver::save_matfile() const std::cerr << "Problem reading dump file:" << e.what() << '\n'; return false; } + + auto sat_posX_m_aux = std::vector(5 * num_epoch); + auto sat_posY_m_aux = std::vector(5 * num_epoch); + auto sat_posZ_m_aux = std::vector(5 * num_epoch); + auto sat_velX_aux = std::vector(5 * num_epoch); + auto sat_velY_aux = std::vector(5 * num_epoch); + auto sat_velZ_aux = std::vector(5 * num_epoch); + + uint32_t k = 0U; + for (int64_t j = 0; j < num_epoch; j++) + { + for (uint32_t i = 0; i < 5; i++) + { + sat_posX_m_aux[k] = sat_posX_m[i][j]; + sat_posY_m_aux[k] = sat_posY_m[i][j]; + sat_posZ_m_aux[k] = sat_posZ_m[i][j]; + sat_velX_aux[k] = sat_velX[i][j]; + sat_velY_aux[k] = sat_velY[i][j]; + sat_velZ_aux[k] = sat_velZ[i][j]; + k++; + } + } // WRITE MAT FILE mat_t *matfp; @@ -384,6 +428,30 @@ bool Rtklib_Solver::save_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("vel_z2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z2.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + std::array dims_sat{static_cast(5), static_cast(num_epoch)}; + matvar = Mat_VarCreate("sat_posX_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posX_m_aux.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("sat_posY_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posY_m_aux.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("sat_posZ_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posZ_m_aux.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("sat_velX", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velX_aux.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("sat_velY", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velY_aux.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("sat_velZ", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velZ_aux.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -1318,6 +1386,24 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = pvt_sol.qr[5]; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.rr[5]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + for (int n = 0; n<5; n++) + { + tmp_double = vtl_data.sat_p(n, 0); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = vtl_data.sat_p(n, 1); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = vtl_data.sat_p(n, 2); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = vtl_data.sat_v(n, 0); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = vtl_data.sat_v(n, 1); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = vtl_data.sat_v(n, 2); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // GEO user position Latitude [deg] tmp_double = this->get_latitude(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); From c2e161efc221a68ba0b89a31844c8a51b6e46833 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 4 Dec 2022 23:35:31 +0000 Subject: [PATCH 036/165] ADD: more info to out mat file --- src/algorithms/PVT/libs/rtklib_solver.cc | 35 ++++++++++++++++++------ src/algorithms/PVT/libs/vtl_engine.cc | 3 +- 2 files changed, 29 insertions(+), 9 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index c1a293d65..b4246cd7a 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -205,7 +205,7 @@ bool Rtklib_Solver::save_matfile() const // READ DUMP FILE const std::string dump_filename = d_dump_filename; const int32_t number_of_double_vars = 22; //+1 MAGL - const int32_t number_of_double_vars_sat = 6; //+ pos/vel por satelite + const int32_t number_of_double_vars_sat = 8; //+ pos/vel/psrange/doppler for sat const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; @@ -263,6 +263,8 @@ bool Rtklib_Solver::save_matfile() const auto sat_velX = std::vector>(5, std::vector(num_epoch)); auto sat_velY = std::vector>(5, std::vector(num_epoch)); auto sat_velZ = std::vector>(5, std::vector(num_epoch)); + auto sat_prg_m = std::vector>(5, std::vector(num_epoch)); + auto sat_dopp_hz = std::vector>(5, std::vector(num_epoch)); auto latitude = std::vector(num_epoch); auto longitude = std::vector(num_epoch); @@ -308,7 +310,9 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&sat_posZ_m[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast(&sat_velX[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast(&sat_velY[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velZ[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_velZ[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_prg_m[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_dopp_hz[chan][i]), sizeof(double)); } dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); @@ -338,6 +342,8 @@ bool Rtklib_Solver::save_matfile() const auto sat_velX_aux = std::vector(5 * num_epoch); auto sat_velY_aux = std::vector(5 * num_epoch); auto sat_velZ_aux = std::vector(5 * num_epoch); + auto sat_prg_m_aux = std::vector(5 * num_epoch); + auto sat_dopp_hz_aux = std::vector(5 * num_epoch); uint32_t k = 0U; for (int64_t j = 0; j < num_epoch; j++) @@ -350,6 +356,8 @@ bool Rtklib_Solver::save_matfile() const sat_velX_aux[k] = sat_velX[i][j]; sat_velY_aux[k] = sat_velY[i][j]; sat_velZ_aux[k] = sat_velZ[i][j]; + sat_prg_m_aux[k] = sat_prg_m[i][j]; + sat_dopp_hz_aux[k] = sat_dopp_hz[i][j]; k++; } } @@ -428,7 +436,7 @@ bool Rtklib_Solver::save_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("vel_z2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z2.data(), 0); + matvar = Mat_VarCreate("clk_bias_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -451,6 +459,13 @@ bool Rtklib_Solver::save_matfile() const matvar = Mat_VarCreate("sat_velZ", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velZ_aux.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("sat_prg_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_prg_m_aux.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("sat_dopp_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_dopp_hz_aux.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE @@ -1211,9 +1226,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.rx_pvt_var[1] = pvt_sol.qr[1]; vtl_data.rx_pvt_var[2] = pvt_sol.qr[2]; //TODO: get direct estimations for V T variances, instead: - vtl_data.rx_pvt_var[3] = pvt_sol.qr[0] * 0.1; //in general minor than position. - vtl_data.rx_pvt_var[4] = pvt_sol.qr[1] * 0.1; - vtl_data.rx_pvt_var[5] = pvt_sol.qr[2] * 0.1; + vtl_data.rx_pvt_var[3] = pvt_sol.qr[0]; //in general minor than position. + vtl_data.rx_pvt_var[4] = pvt_sol.qr[1]; + vtl_data.rx_pvt_var[5] = pvt_sol.qr[2]; vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler //receiver clock offset and receiver clock drift @@ -1386,7 +1401,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = pvt_sol.qr[5]; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.rr[5]; + tmp_double = vtl_data.rx_dts(0); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); for (int n = 0; n<5; n++) { @@ -1401,7 +1416,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = vtl_data.sat_v(n, 1); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = vtl_data.sat_v(n, 2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = vtl_data.pr_m(n); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = vtl_data.doppler_hz(n); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } // GEO user position Latitude [deg] diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 3c0871b12..25e322509 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -191,7 +191,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) else { dump_vtl_file << "kf_xerr"<< ","< Date: Sun, 4 Dec 2022 23:49:39 +0000 Subject: [PATCH 037/165] ADD: some matlab utils --- src/utils/matlab/vtl/GnssSDR2struct.m | 87 ++++ src/utils/matlab/vtl/SpirentMotion2struct.m | 112 +++++ src/utils/matlab/vtl/SpirentSatData2struct.m | 188 +++++++ src/utils/matlab/vtl/Vtl2struct.m | 93 ++++ src/utils/matlab/vtl/general_raw_plot.m | 198 ++++++++ .../matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m | 466 ++++++++++++++++++ .../vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 344 +++++++++++++ 7 files changed, 1488 insertions(+) create mode 100644 src/utils/matlab/vtl/GnssSDR2struct.m create mode 100644 src/utils/matlab/vtl/SpirentMotion2struct.m create mode 100644 src/utils/matlab/vtl/SpirentSatData2struct.m create mode 100644 src/utils/matlab/vtl/Vtl2struct.m create mode 100644 src/utils/matlab/vtl/general_raw_plot.m create mode 100644 src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m create mode 100644 src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m diff --git a/src/utils/matlab/vtl/GnssSDR2struct.m b/src/utils/matlab/vtl/GnssSDR2struct.m new file mode 100644 index 000000000..170b37f24 --- /dev/null +++ b/src/utils/matlab/vtl/GnssSDR2struct.m @@ -0,0 +1,87 @@ +% GnssSDR2struct Convert GNSS-SDR output .mat file PVT.mat to a struct. +% refSolution = SpirentMotion2struct(path_to_motion_V1_csv) Convert PVT.mat to a struct navSolution +% refSolution has the following fields: + + % navSolution.solution_status=solution_status; + % navSolution.solution_type=solution_type; + % navSolution.valid_sats=valid_sats; + % + % navSolution.RX_time=RX_time; + % navSolution.TOW_at_current_symbol_ms=TOW_at_current_symbol_ms; + % + % navSolution.X=pos_x; + % navSolution.Y=pos_y; + % navSolution.Z=pos_z; + % + % navSolution.latitude=latitude; + % navSolution.longitude=longitude; + % navSolution.height=height; + % + % navSolution.pdop=pdop; + % navSolution.gdop=gdop; + % navSolution.hdop=hdop; + % + % navSolution.vX=vel_x; + % navSolution.vY=vel_y; + % navSolution.vZ=vel_z; + % + % navSolution.vdop=vdop; + % + % navSolution.week=week; +% +% ------------------------------------------------------------------------- +% USE EXAMPLE: navSolution = GnssSDR2struct('PVT_raw.mat') +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% SPDX-FileCopyrightText: 2022 gomezlma(at)inta.es +% SPDX-License-Identifier: GPL-3.0-or-later + + + +function [navSolution] = GnssSDR2struct(PVT_file_Path) + +load(PVT_file_Path, ... +'RX_time' ,'TOW_at_current_symbol_ms' ,'pos_x','pos_y' ,'pos_z',... +'latitude' ,'longitude','height' ,'pdop' ,'gdop' ,'hdop' ,'vel_x' ,... +'vel_y' ,'vel_z' ,'vdop' ,'week','solution_status','solution_type',... +'valid_sats') + +%% GNSS SDR SOLUTION + +navSolution.solution_status=solution_status; +navSolution.solution_type=solution_type; +navSolution.valid_sats=valid_sats; + +navSolution.RX_time=RX_time; +navSolution.TOW_at_current_symbol_ms=TOW_at_current_symbol_ms; + +navSolution.X=pos_x; +navSolution.Y=pos_y; +navSolution.Z=pos_z; + +navSolution.latitude=latitude; +navSolution.longitude=longitude; +navSolution.height=height; + +navSolution.pdop=pdop; +navSolution.gdop=gdop; +navSolution.hdop=hdop; + +navSolution.vX=vel_x; +navSolution.vY=vel_y; +navSolution.vZ=vel_z; + +navSolution.vdop=vdop; + +navSolution.week=week; + +%% clear tmp variables +clearvars -except navSolution +end diff --git a/src/utils/matlab/vtl/SpirentMotion2struct.m b/src/utils/matlab/vtl/SpirentMotion2struct.m new file mode 100644 index 000000000..b5789330b --- /dev/null +++ b/src/utils/matlab/vtl/SpirentMotion2struct.m @@ -0,0 +1,112 @@ +% SpirentMotion2struct Convert CSV file motionV1.csv to a struct. +% refSolution = SpirentMotion2struct(path_to_motion_V1_csv) parse the CSV motionV1.CSV to a struct refSolution +% refSolution has the following fields: +% +% - refSolution.SIM_time: simulation time from 0 (in ms) +% - refSolution.X: UTM referenced position X (in m) +% - refSolution.Y: UTM referenced position Y (in m) +% - refSolution.Z: UTM referenced position Z (in m) +% - refSolution.vX: referenced Velocity X (in m/s) +% - refSolution.vY: referenced Velocity Y (in m/s) +% - refSolution.vZ: referenced Velocity Z (in m/s) +% - refSolution.aX: referenced Aceleration X (in m/s2) +% - refSolution.aY: referenced Aceleration Y (in m/s2) +% - refSolution.aZ: referenced Aceleration Z (in m/s2) +% - refSolution.jX: referenced Jerk X (in m/s3) +% - refSolution.jY: referenced Jerk Y (in m/s3) +% - refSolution.jZ: referenced Jerk Z (in m/s3) +% - refSolution.latitude:reference Latitude(in degrees), +% ellipsoid WGS84? +% - refSolution.longitude:reference Longitude(in degrees), +% ellipsoid WGS84? +% - refSolution.height: referenced Heigh (in m), +% ellipsoid WGS84? +% - refSolution.dop: referenced antenna DOP +% +% ------------------------------------------------------------------------- +% USE EXAMPLE: refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv') +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% SPDX-FileCopyrightText: 2022 gomezlma(at)inta.es +% SPDX-License-Identifier: GPL-3.0-or-later + + + +function [refSolution] = SpirentMotion2struct(path_to_motion_V1_csv) + + % if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file') + % addpath('./libs') + % end + % + % if ~exist('cat2geo.m', 'file') + % addpath('./libs/geoFunctions') + % end + + + %% ===== Import data from text file motion_V1.csv 2 ARRAY ============================ + % Script for importing data from the following text file: + % + % filename: D:\virtualBOX_VM\ubuntu20\ubuntu20\shareFolder\myWork\results\log_spirent\motion_V1.csv + % + % Auto-generated by MATLAB on 20-Nov-2022 12:31:17 + + % Set up the Import Options and import the data + opts = delimitedTextImportOptions("NumVariables", 38); + + % Specify range and delimiter + opts.DataLines = [3, Inf]; + opts.Delimiter = ","; + + % Specify column names and types + opts.VariableNames = ["Time_ms", "Pos_X", "Pos_Y", "Pos_Z", "Vel_X", "Vel_Y", "Vel_Z", "Acc_X", "Acc_Y", "Acc_Z", "Jerk_X", "Jerk_Y", "Jerk_Z", "Lat", "Long", "Height", "Heading", "Elevation", "Bank", "Angvel_X", "Angvel_Y", "Angvel_Z", "Angacc_X", "Angacc_Y", "Angacc_Z", "Ant1_Pos_X", "Ant1_Pos_Y", "Ant1_Pos_Z", "Ant1_Vel_X", "Ant1_Vel_Y", "Ant1_Vel_Z", "Ant1_Acc_X", "Ant1_Acc_Y", "Ant1_Acc_Z", "Ant1_Lat", "Ant1_Long", "Ant1_Height", "Ant1_DOP"]; + opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; + + % Specify file level properties + opts.ExtraColumnsRule = "ignore"; + opts.EmptyLineRule = "read"; + + % Import the data + motionV1 = readtable(path_to_motion_V1_csv, opts); + + % Convert to output type + motionV1 = table2array(motionV1); + + % Clear temporary variables + clear opts + + %% SPIRENT REFERENCE SOLUTION + + refSolution.SIM_time=motionV1(:,1); + + refSolution.X=motionV1(:,2); + refSolution.Y=motionV1(:,3); + refSolution.Z=motionV1(:,4); + + refSolution.vX=motionV1(:,5); + refSolution.vY=motionV1(:,6); + refSolution.vZ=motionV1(:,7); + + refSolution.aX=motionV1(:,8); + refSolution.aY=motionV1(:,9); + refSolution.aZ=motionV1(:,10); + + refSolution.jX=motionV1(:,11); + refSolution.jY=motionV1(:,12); + refSolution.jZ=motionV1(:,13); + + refSolution.latitude=rad2deg(motionV1(:,14)); + refSolution.longitude=rad2deg(motionV1(:,15)); + refSolution.height=motionV1(:,16); + + refSolution.dop=motionV1(:,38); + + % Clear temporary variables + clear motionV1 +end diff --git a/src/utils/matlab/vtl/SpirentSatData2struct.m b/src/utils/matlab/vtl/SpirentSatData2struct.m new file mode 100644 index 000000000..37e930ebd --- /dev/null +++ b/src/utils/matlab/vtl/SpirentSatData2struct.m @@ -0,0 +1,188 @@ +% SpirentSatData2struct Convert CSV file sat_data_V1A1.csv to a struct. +% refSolution = SpirentSatData2struct(path_to_sat_V1_csv) parse the CSV sat_data_V1A1.CSV to a struct refSatData +% refSatData has the following fields: +% +% refSatData.GALILEO.series(i).Sat_ID(j) = GAL(k,3); +% refSatData.GALILEO.series(i).Sat_ID(j) +% refSatData.GALILEO.series(i).Sat_PRN(j) +% refSatData.GALILEO.series(i).sat_X(j) +% refSatData.GALILEO.series(i).sat_Y(j) +% refSatData.GALILEO.series(i).sat_Z(j) +% refSatData.GALILEO.series(i).sat_vX(j) +% refSatData.GALILEO.series(i).sat_vY(j) +% refSatData.GALILEO.series(i).sat_vZ(j) +% refSatData.GALILEO.series(i).azimuth(j) +% refSatData.GALILEO.series(i).elevation(j) +% refSatData.GALILEO.series(i).range(j) +% refSatData.GALILEO.series(i).pr_m(j) +% refSatData.GALILEO.series(i).pr_rate(j) +% refSatData.GALILEO.series(i).iono_delay(j) +% refSatData.GALILEO.series(i).tropo_delay(j) +% refSatData.GALILEO.series(i).pr_error(j) +% refSatData.GALILEO.series(i).signal_dB(j) +% refSatData.GALILEO.series(i).ant_azimuth(j) +% refSatData.GALILEO.series(i).ant_elevation(j) +% refSatData.GALILEO.series(i).range_rate(j) +% refSatData.GALILEO.series(i).pr_Error_rate(j) +% refSatData.GALILEO.series(i).doppler_shift(j) +% refSatData.GALILEO.series(i).delta_range(j) +% refSatData.GALILEO.series(i).sat_Acc_X(j) +% refSatData.GALILEO.series(i).sat_Acc_Y(j) +% refSatData.GALILEO.series(i).sat_Acc_Z(j) +% % +% ------------------------------------------------------------------------- +% USE EXAMPLE: refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv') +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% SPDX-FileCopyrightText: 2022 gomezlma(at)inta.es +% SPDX-License-Identifier: GPL-3.0-or-later + + + +function [refSatData] = SpirentSatData2struct(path_to_sat_V1_csv) + + % if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file') + % addpath('./libs') + % end + % + % if ~exist('cat2geo.m', 'file') + % addpath('./libs/geoFunctions') + % end + + + %% ===== Import data from text file motion_V1.csv 2 ARRAY ============================ + +% Set up the Import Options and import the data +opts = delimitedTextImportOptions("NumVariables", 50); + +% Specify range and delimiter +opts.DataLines = [6, Inf]; +opts.Delimiter = ","; + +% Specify column names and types +opts.VariableNames = ["Time_ms", "Channel", "Sat_type", "Sat_ID", "Sat_PRN", "Echo_Num", "Sat_Pos_X", "Sat_Pos_Y", "Sat_Pos_Z", "Sat_Vel_X", "Sat_Vel_Y", "Sat_Vel_Z", "Azimuth", "Elevation", "Range", "PRangeGroupA", "PRangeGroupB", "PRangeGroupC", "PRangeGroupD", "PRangeGroupE", "PR_rate", "Iono_delayGroupA", "Iono_delayGroupB", "Iono_delayGroupC", "Iono_delayGroupD", "Iono_delayGroupE", "Tropo_delay", "PR_Error", "Signal_dBGroupA", "Signal_dBGroupB", "Signal_dBGroupC", "Signal_dBGroupD", "Signal_dBGroupE", "Ant_azimuth", "Ant_elevation", "Range_rate", "PR_Error_rate", "Doppler_shiftGroupA", "Doppler_shiftGroupB", "Doppler_shiftGroupC", "Doppler_shiftGroupD", "Doppler_shiftGroupE", "Delta_rangeGroupA", "Delta_rangeGroupB", "Delta_rangeGroupC", "Delta_rangeGroupD", "Delta_rangeGroupE", "Sat_Acc_X", "Sat_Acc_Y", "Sat_Acc_Z"]; +opts.VariableTypes = ["double", "double", "char", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; + +% Specify file level properties +opts.ExtraColumnsRule = "ignore"; +opts.EmptyLineRule = "read"; + +% Specify variable properties +opts = setvaropts(opts, "Sat_type", "WhitespaceRule", "preserve"); +opts = setvaropts(opts, "Sat_type", "EmptyFieldRule", "auto"); + +% Import the data +satdataV1A1SPFLD1 = readtable(path_to_sat_V1_csv, opts); + +%% Convert to output type +satdataV1A1SPFLD1 = table2cell(satdataV1A1SPFLD1); +numIdx = cellfun(@(x) ~isnan(str2double(x)), satdataV1A1SPFLD1); +satdataV1A1SPFLD1(numIdx) = cellfun(@(x) {str2double(x)}, satdataV1A1SPFLD1(numIdx)); + +%% Clear temporary variables +clear opts +%% initialize + +refSatData.GPS=[]; +refSatData.GALILEO=[]; +refSatData.GLONASS=[]; +refSatData.BEIDOU=[]; + +%% split by constellations +[indGALILEO,~]= find(strcmp(satdataV1A1SPFLD1, 'GALILEO')); +[indGPS,~]= find(strcmp(satdataV1A1SPFLD1, 'GPS')); + +GAL=satdataV1A1SPFLD1(indGALILEO,:);GAL(:,3)=[]; +GPS=satdataV1A1SPFLD1(indGPS,:); GPS(:,3)=[]; +GAL=cell2mat(GAL); +GPS=cell2mat(GPS); + %% SPIRENT SAT SOLUTION + + refSatData.GALILEO.SIM_time=unique(GAL(:,1),'first'); + refSatData.GPS.SIM_time=unique(GPS(:,1),'first'); + %% + k=0; + for i=1:length(refSatData.GALILEO.SIM_time) + nsats=length(find(GAL(:,1)==refSatData.GALILEO.SIM_time(i))); + refSatData.GALILEO.series(i).nsats=nsats; + + for j=1:nsats + k=k+1; + refSatData.GALILEO.series(i).Sat_ID(j) = GAL(k,3); + refSatData.GALILEO.series(i).Sat_ID(j) = GAL(i,3); + refSatData.GALILEO.series(i).Sat_PRN(j) = GAL(i,4); + refSatData.GALILEO.series(i).sat_X(j) = GAL(k,6); + refSatData.GALILEO.series(i).sat_Y(j) = GAL(k,7); + refSatData.GALILEO.series(i).sat_Z(j) = GAL(k,8); + refSatData.GALILEO.series(i).sat_vX(j) = GAL(k,9); + refSatData.GALILEO.series(i).sat_vY(j) = GAL(k,10); + refSatData.GALILEO.series(i).sat_vZ(j) = GAL(k,11); + refSatData.GALILEO.series(i).azimuth(j) = GAL(k,12); + refSatData.GALILEO.series(i).elevation(j) = GAL(k,13); + refSatData.GALILEO.series(i).range(j) = GAL(k,14); + refSatData.GALILEO.series(i).pr_m(j) = GAL(k,15); + refSatData.GALILEO.series(i).pr_rate(j) = GAL(k,20); + refSatData.GALILEO.series(i).iono_delay(j) = GAL(k,21); + refSatData.GALILEO.series(i).tropo_delay(j) = GAL(k,26);% + refSatData.GALILEO.series(i).pr_error(j) = GAL(k,27); + refSatData.GALILEO.series(i).signal_dB(j) = GAL(k,28); + refSatData.GALILEO.series(i).ant_azimuth(j) = GAL(k,33); + refSatData.GALILEO.series(i).ant_elevation(j) = GAL(k,34); + refSatData.GALILEO.series(i).range_rate(j) = GAL(k,35); + refSatData.GALILEO.series(i).pr_Error_rate(j) = GAL(k,36); + refSatData.GALILEO.series(i).doppler_shift(j) = GAL(k,37); + refSatData.GALILEO.series(i).delta_range(j) = GAL(k,42); + refSatData.GALILEO.series(i).sat_Acc_X(j) = GAL(k,47); + refSatData.GALILEO.series(i).sat_Acc_Y(j) = GAL(k,48); + refSatData.GALILEO.series(i).sat_Acc_Z(j) = GAL(k,49); + end + + end + %% ------------------------------------- + k=0; + for i=1:length(refSatData.GPS.SIM_time) + nsats=length(find(GPS(:,1)==refSatData.GPS.SIM_time(i))); + refSatData.GPS.series(i).nsats=nsats; + + for j=1:nsats + + k=k+1; + refSatData.GPS.series(i).Sat_ID(j) = GPS(k,3); + refSatData.GPS.series(i).Sat_PRN(j) = GPS(k,4); + refSatData.GPS.series(i).sat_X(j) = GPS(k,6); + refSatData.GPS.series(i).sat_Y(j) = GPS(k,7); + refSatData.GPS.series(i).sat_Z(j) = GPS(k,8); + refSatData.GPS.series(i).sat_vX(j) = GPS(k,9); + refSatData.GPS.series(i).sat_vY(j) = GPS(k,10); + refSatData.GPS.series(i).sat_vZ(j) = GPS(k,11); + refSatData.GPS.series(i).azimuth(j) = GPS(k,12); + refSatData.GPS.series(i).elevation(j) = GPS(k,13); + refSatData.GPS.series(i).range(j) = GPS(k,14); + refSatData.GPS.series(i).pr_m(j) = GPS(k,15); + refSatData.GPS.series(i).pr_rate(j) = GPS(k,20); + refSatData.GPS.series(i).iono_delay(j) = GPS(k,21); + refSatData.GPS.series(i).tropo_delay(j) = GPS(k,26);% + refSatData.GPS.series(i).pr_error(j) = GPS(k,27); + refSatData.GPS.series(i).signal_dB(j) = GPS(k,28); + refSatData.GPS.series(i).ant_azimuth(j) = GPS(k,33); + refSatData.GPS.series(i).ant_elevation(j) = GPS(k,34); + refSatData.GPS.series(i).range_rate(j) = GPS(k,35); + refSatData.GPS.series(i).pr_Error_rate(j) = GPS(k,36); + refSatData.GPS.series(i).doppler_shift(j) = GPS(k,37); + refSatData.GPS.series(i).delta_range(j) = GPS(k,42); + refSatData.GPS.series(i).sat_Acc_X(j) = GPS(k,47); + refSatData.GPS.series(i).sat_Acc_Y(j) = GPS(k,48); + refSatData.GPS.series(i).sat_Acc_Z(j) = GPS(k,49); + end + end + + % Clear temporary variables + clear satdataV1A1SPFLD05 GPS GAL +end diff --git a/src/utils/matlab/vtl/Vtl2struct.m b/src/utils/matlab/vtl/Vtl2struct.m new file mode 100644 index 000000000..8c3234e1a --- /dev/null +++ b/src/utils/matlab/vtl/Vtl2struct.m @@ -0,0 +1,93 @@ +%% Import data from text file +% Script for importing data from the following text file: +% +% filename: D:\virtualBOX_VM\ubuntu20\ubuntu20\shareFolder\myWork\results\spirent_usrp_airing\dump_vtl_file.csv +% +% % +% ------------------------------------------------------------------------- +% USE EXAMPLE: vtlSolution = Vtl2struct('dump_vtl_file.csv') +% ------------------------------------------------------------------------- + +% Auto-generated by MATLAB on 24-Nov-2022 17:34:27 +function [vtlSolution] = Vtl2struct(path_to_vtl_csv) +%% Set up the Import Options and import the data +opts = delimitedTextImportOptions("NumVariables", 9); + +% Specify range and delimiter +opts.DataLines = [1, Inf]; +opts.Delimiter = ","; + +% Specify column names and types +opts.VariableNames = ["kf_state", "e06", "VarName3", "e06_1", "VarName5", "VarName6", "VarName7", "VarName8", "VarName9"]; +opts.VariableTypes = ["char", "double", "double", "double", "double", "double", "double", "double", "double"]; + +% Specify file level properties +opts.ExtraColumnsRule = "ignore"; +opts.EmptyLineRule = "read"; + +% Specify variable properties +opts = setvaropts(opts, "kf_state", "WhitespaceRule", "preserve"); +opts = setvaropts(opts, "kf_state", "EmptyFieldRule", "auto"); + +% Import the data +dumpvtlfile = readtable(path_to_vtl_csv, opts); + +%% Convert to output type +dumpvtlfile = table2cell(dumpvtlfile); +numIdx = cellfun(@(x) ~isnan(str2double(x)), dumpvtlfile); +dumpvtlfile(numIdx) = cellfun(@(x) {str2double(x)}, dumpvtlfile(numIdx)); + +%% Clear temporary variables +clear opts +%% + +vtlSolution.kfpvt=[]; +vtlSolution.rtklibpvt=[]; + +%% split by solution type +[indKF,~]= find(strcmp(dumpvtlfile, 'kf_state')); +[indRTKlib,~]= find(strcmp(dumpvtlfile, 'rtklib_state')); +[indkf_err,~]= find(strcmp(dumpvtlfile, 'kf_xerr')); +[ind_LOS,~]= find(strcmp(dumpvtlfile, 'sat_first_LOS')); + +kfpvt=dumpvtlfile(indKF,:);kfpvt(:,1)=[]; +rtklibpvt=dumpvtlfile(indRTKlib,:); rtklibpvt(:,1)=[]; +kferr=dumpvtlfile(indkf_err,:); kferr(:,1)=[]; +LOS=dumpvtlfile(ind_LOS,:); LOS(:,1)=[]; + +kfpvt=cell2mat(kfpvt); +rtklibpvt=cell2mat(rtklibpvt); +kferr=cell2mat(kferr); +LOS=cell2mat(LOS); + +vtlSolution.kfpvt.X=kfpvt(:,1); +vtlSolution.kfpvt.Y=kfpvt(:,2); +vtlSolution.kfpvt.Z=kfpvt(:,3); +vtlSolution.kfpvt.vX=kfpvt(:,4); +vtlSolution.kfpvt.vY=kfpvt(:,5); +vtlSolution.kfpvt.vZ=kfpvt(:,6); +vtlSolution.kfpvt.biasclock=kfpvt(:,7); +vtlSolution.kfpvt.rateblock=kfpvt(:,8); + +vtlSolution.rtklibpvt.X=rtklibpvt(:,1); +vtlSolution.rtklibpvt.Y=rtklibpvt(:,2); +vtlSolution.rtklibpvt.Z=rtklibpvt(:,3); +vtlSolution.rtklibpvt.vX=rtklibpvt(:,4); +vtlSolution.rtklibpvt.vY=rtklibpvt(:,5); +vtlSolution.rtklibpvt.vZ=rtklibpvt(:,6); +vtlSolution.rtklibpvt.biasclock=rtklibpvt(:,7); +vtlSolution.rtklibpvt.rateblock=rtklibpvt(:,8); + +vtlSolution.kferr.X=kferr(:,1); +vtlSolution.kferr.Y=kferr(:,2); +vtlSolution.kferr.Z=kferr(:,3); +vtlSolution.kferr.vX=kferr(:,4); +vtlSolution.kferr.vY=kferr(:,5); +vtlSolution.kferr.vZ=kferr(:,6); +vtlSolution.kferr.biasclock=kferr(:,7); +vtlSolution.kferr.rateblock=kferr(:,8); + +vtlSolution.LOSx=LOS(:,1); +vtlSolution.LOSy=LOS(:,2); +vtlSolution.LOSz=LOS(:,3); +end \ No newline at end of file diff --git a/src/utils/matlab/vtl/general_raw_plot.m b/src/utils/matlab/vtl/general_raw_plot.m new file mode 100644 index 000000000..a9dc27570 --- /dev/null +++ b/src/utils/matlab/vtl/general_raw_plot.m @@ -0,0 +1,198 @@ +%general_raw_plot.m + +%% ====== FILTERING ======================================================= +moving_avg_factor= 500; +LAT_FILT = movmean(navSolution.latitude,moving_avg_factor); +LON_FILT = movmean(navSolution.longitude,moving_avg_factor); +HEIGH_FILT = movmean(navSolution.height,moving_avg_factor); + +X_FILT = movmean(navSolution.X,moving_avg_factor); +Y_FILT = movmean(navSolution.Y,moving_avg_factor); +Z_FILT = movmean(navSolution.Z,moving_avg_factor); + +vX_FILT = movmean(navSolution.vX,moving_avg_factor); +vY_FILT = movmean(navSolution.vY,moving_avg_factor); +vZ_FILT = movmean(navSolution.vZ,moving_avg_factor); + +% navSolution.X(navSolution.X==0) = []; +%% ====== GNSS-SDR Plot all figures ======================================================= +close all +%--- LLH POSITION: GNSS SDR plot on map -------------------------------------- +LLH=figure('Name','LLH system on map'); +geoplot([navSolution.latitude],[navSolution.longitude],'.') +geobasemap satellite +hold on +geoplot(LAT_FILT,LON_FILT,'r.') +title('Position in LLH on map ') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') +% geobasemap streets +% geobasemap topographic +% geobasemap streets-dark + +%---VELOCITY: GNSS SDR plot -------------------------------------- +VEL=figure('Name','velocities and heigh'); +subplot(2,2,1); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vX,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),vX_FILT,'r.'); +ylabel('vX (m/s)') +xlabel('time from First FIX in (seconds)') +title('Subplot 1: vX GOR') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') + +subplot(2,2,2); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),vY_FILT,'r.'); +ylabel('vY (m/s)') +xlabel('time from First FIX in (seconds)') +title('Subplot 2: vY') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') + +subplot(2,2,3); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),vZ_FILT,'r.'); +ylabel('vZ (m/s)') +xlabel('time from First FIX in (seconds)') +title('Subplot 3: vZ') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') + +subplot(2,2,4); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.height,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),HEIGH_FILT,'r.'); +ylabel('HEIGH (m)') +xlabel('time from First FIX in (seconds)') +title('Subplot 4: HEIGH') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') + +sgtitle('velocities and heigh') + +% --- UTM centered POSITION: GNSS SDR plot -------------------------------------- + +POS=figure('Name','UTM COORD CENTERED IN 1^{ST} POSITION'); +subplot(2,2,1); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X-refSolution.X(1),'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),X_FILT-refSolution.X(1),'r.'); +ylabel('X (m)') +xlabel('time from First FIX in (seconds)') +% ylim([-55 100]) +title('Subplot 1: X GOR') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east') + +subplot(2,2,2); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Y-refSolution.Y(1),'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),Y_FILT-refSolution.Y(1),'r.'); +ylabel('Y (m)') +xlabel('time from First FIX in (seconds)') +% ylim([-140 -20]) +title('Subplot 2: Y') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east') + +subplot(2,2,3); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Z-refSolution.Z(1),'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),Z_FILT-refSolution.Z(1),'r.'); +ylabel('Z (m)') +xlabel('time from First FIX in (seconds)') +% ylim([-150 20]) +title('Subplot 3: Z') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east') + +sgtitle('UTM COORD CENTERED IN 1^{ST} POSITION') + +% --- UTM full POSITION: GNSS SDR plot -------------------------------------- + +POS_utm=figure('Name','UTM COORD IN 1^{ST} POSITION'); +subplot(2,2,1); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),X_FILT,'r.'); +ylabel('X (m)') +xlabel('time from First FIX in (seconds)') +% ylim([-55 100]) +title('Subplot 1: X GOR') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east') + +subplot(2,2,2); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Y,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),Y_FILT,'r.'); +ylabel('Y (m)') +xlabel('time from First FIX in (seconds)') +% ylim([-140 -20]) +title('Subplot 2: Y') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east') + +subplot(2,2,3); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Z,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),Z_FILT,'r.'); +ylabel('Z (m)') +xlabel('time from First FIX in (seconds)') +% ylim([-150 20]) +title('Subplot 3: Z') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east') + +sgtitle('UTM COORD FULL IN 1^{ST} POSITION') + +%% ====== SPIRENT Plot all figures ======================================================= +if(plot_reference) + %--- LLH POSITION: SPIRENT plot on map -------------------------------------- + figure(LLH) + geoplot([refSolution.latitude],[refSolution.longitude],'.','DisplayName','reference') + geobasemap satellite + hold off + % geobasemap streets + % geobasemap topographic + % geobasemap streets-dark + + %---VELOCITY: SPIRENT plot -------------------------------------- + figure(VEL) + subplot(2,2,1); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vX,'.','DisplayName','reference'); + hold off;grid on + + subplot(2,2,2); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vY,'.','DisplayName','reference'); + hold on;grid on + + subplot(2,2,3); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vZ,'.','DisplayName','reference'); + hold on;grid on + + subplot(2,2,4); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.height,'.','DisplayName','reference'); + hold on;grid on + + %---UTM centered POSITION: SPIRENT plot -------------------------------------- + figure(POS) + subplot(2,2,1); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.X-refSolution.X(1),'.','DisplayName','reference'); + hold off;grid on + + subplot(2,2,2); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Y-refSolution.Y(1),'.','DisplayName','reference'); + hold on;grid on + + subplot(2,2,3); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Z-refSolution.Z(1),'.','DisplayName','reference'); + hold on;grid on + + %---UTM POSITION: SPIRENT plot -------------------------------------- + figure(POS_utm) + subplot(2,2,1); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.X,'.','DisplayName','reference'); + hold off;grid on + + subplot(2,2,2); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Y,'.','DisplayName','reference'); + hold on;grid on + + subplot(2,2,3); + plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Z,'.','DisplayName','reference'); + hold on;grid on +end \ No newline at end of file diff --git a/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m b/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m new file mode 100644 index 000000000..6198d51f9 --- /dev/null +++ b/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m @@ -0,0 +1,466 @@ +% Read PVG raw dump +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% +%% +clc +close all; +clear all; + +% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file') +% addpath('./libs') +% end +% +% if ~exist('cat2geo.m', 'file') +% addpath('./libs/geoFunctions') +% end + +%% +samplingFreq=25000000; +channels=6; +TTFF_sec=41.48; +path=''; +pvt_raw_log_path=[path 'PVT_raw.dat']; +% GNSS_PVT_raw= gps_l1_ca_read_pvt_raw_dump(channels,pvt_raw_log_path); +GnssSDR2struct('PVT_raw.mat') + +%% ===== Import data from text file motion_V1.csv 2 ARRAY ============================ +% Script for importing data from the following text file: +% +% filename: D:\virtualBOX_VM\ubuntu20\ubuntu20\shareFolder\myWork\results\log_spirent\motion_V1.csv +% +% Auto-generated by MATLAB on 20-Nov-2022 12:31:17 + +% Set up the Import Options and import the data +opts = delimitedTextImportOptions("NumVariables", 38); + +% Specify range and delimiter +opts.DataLines = [3, Inf]; +opts.Delimiter = ","; + +% Specify column names and types +opts.VariableNames = ["Time_ms", "Pos_X", "Pos_Y", "Pos_Z", "Vel_X", "Vel_Y", "Vel_Z", "Acc_X", "Acc_Y", "Acc_Z", "Jerk_X", "Jerk_Y", "Jerk_Z", "Lat", "Long", "Height", "Heading", "Elevation", "Bank", "Angvel_X", "Angvel_Y", "Angvel_Z", "Angacc_X", "Angacc_Y", "Angacc_Z", "Ant1_Pos_X", "Ant1_Pos_Y", "Ant1_Pos_Z", "Ant1_Vel_X", "Ant1_Vel_Y", "Ant1_Vel_Z", "Ant1_Acc_X", "Ant1_Acc_Y", "Ant1_Acc_Z", "Ant1_Lat", "Ant1_Long", "Ant1_Height", "Ant1_DOP"]; +opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; + +% Specify file level properties +opts.ExtraColumnsRule = "ignore"; +opts.EmptyLineRule = "read"; + +% Import the data +motionV1 = readtable("..\log_spirent\motion_V1_SPF_LD_05.csv", opts); + +% Convert to output type +motionV1 = table2array(motionV1); + +% Clear temporary variables +clear opts + +%% ============================ PARSER TO STRUCT ============================ +plot_skyplot=1; +%% GNSS SDR SOLUTION +% navSolution.samplingFreq=25000000; +% navSolution.channels=6; + +navSolution.solution_status=solution_status; +navSolution.solution_type=solution_type; +navSolution.valid_sats=valid_sats; + +navSolution.RX_time=RX_time; +navSolution.TOW_at_current_symbol_ms=TOW_at_current_symbol_ms; + +navSolution.X=pos_x; +navSolution.Y=pos_y; +navSolution.Z=pos_z; + +navSolution.latitude=latitude; +navSolution.longitude=longitude; +navSolution.height=height; + +navSolution.pdop=pdop; +navSolution.gdop=gdop; +navSolution.hdop=hdop; + +navSolution.vX=vel_x; +navSolution.vY=vel_y; +navSolution.vZ=vel_z; + +navSolution.vdop=vdop; + +navSolution.week=week; + +%% SPIRENT REFERENCE SOLUTION + +refSolution.SIM_time=motionV1(:,1); + +refSolution.X=motionV1(:,2); +refSolution.Y=motionV1(:,3); +refSolution.Z=motionV1(:,4); + +refSolution.vX=motionV1(:,5); +refSolution.vY=motionV1(:,6); +refSolution.vZ=motionV1(:,7); + +refSolution.aX=motionV1(:,8); +refSolution.aY=motionV1(:,9); +refSolution.aZ=motionV1(:,10); + +refSolution.jX=motionV1(:,11); +refSolution.jY=motionV1(:,12); +refSolution.jZ=motionV1(:,13); + +refSolution.latitude=rad2deg(motionV1(:,14)); +refSolution.longitude=rad2deg(motionV1(:,15)); +refSolution.height=motionV1(:,16); + +refSolution.dop=motionV1(:,38); + +% Clear temporary variables +clear motionV1 +%% === Convert to UTM coordinate system ============================= + +% Scenario latitude is xx.xxxxxxx N37 49 9.98 +% Scenario longitude is -xx.xxxxxxx W122 28 42.58 +% Scenario elevation is 35 meters. + % lat=[37 49 9.98]; + % long=[-122 -28 -42.58]; + % lat_deg=dms2deg(lat); + % long_deg=dms2deg(long); + % h=35; + + lat_deg=navSolution.latitude(1); + lon_deg=navSolution.longitude(1); + lat=degrees2dms(lat_deg); + lon=degrees2dms(lon_deg); + h=navSolution.height(1); + + +utmstruct = defaultm('utm'); +utmstruct.zone = utmzone(lat_deg, lon_deg); +utmstruct = defaultm(utmstruct); +[utmstruct.latlim,utmstruct.lonlim] = utmzone(utmstruct.zone ); +%Choices i of Reference Ellipsoid +% 1. International Ellipsoid 1924 +% 2. International Ellipsoid 1967 +% 3. World Geodetic System 1972 +% 4. Geodetic Reference System 1980 +% 5. World Geodetic System 1984 + +utmstruct.geoid = wgs84Ellipsoid; + +% [X, Y] = projfwd(utmstruct,lat_deg,lon_deg); +% Z=h; % geographical to cartesian conversion + + +% for k=1:1:length(navSolution.X) +% [navSolution.E(k), ... +% navSolution.N(k), ... +% navSolution.U(k)]=cart2utm(navSolution.X(k), navSolution.Y(k), navSolution.Z(k), utmZone); +% end + +%% ====== FILTERING ======================================================= +moving_avg_factor= 500; +LAT_FILT = movmean(navSolution.latitude,moving_avg_factor); +LON_FILT = movmean(navSolution.longitude,moving_avg_factor); +HEIGH_FILT = movmean(navSolution.height,moving_avg_factor); + +X_FILT = movmean(navSolution.X,moving_avg_factor); +Y_FILT = movmean(navSolution.Y,moving_avg_factor); +Z_FILT = movmean(navSolution.Z,moving_avg_factor); + +vX_FILT = movmean(navSolution.vX,moving_avg_factor); +vY_FILT = movmean(navSolution.vY,moving_avg_factor); +vZ_FILT = movmean(navSolution.vZ,moving_avg_factor); + +%% ====== GNSS-SDR Plot all figures ======================================================= +close all +%--- LLH POSITION: GNSS SDR plot on map -------------------------------------- +LLH=figure('Name','LLH system on map'); +geoplot([navSolution.latitude],[navSolution.longitude],'.') +geobasemap satellite +hold on +geoplot([LAT_FILT],[LON_FILT],'r.') +title('Position in LLH on map ') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') +% geobasemap streets +% geobasemap topographic +% geobasemap streets-dark + +%---VELOCITY: GNSS SDR plot -------------------------------------- +VEL=figure('Name','velocities and heigh'); +subplot(2,2,1); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vX,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),vX_FILT,'r.'); +ylabel('vX (m/s)') +xlabel('time from First FIX in (seconds)') +title('Subplot 1: vX GOR') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') + +subplot(2,2,2); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),vY_FILT,'r.'); +ylabel('vY (m/s)') +xlabel('time from First FIX in (seconds)') +title('Subplot 2: vY') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') + +subplot(2,2,3); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),vZ_FILT,'r.'); +ylabel('vZ (m/s)') +xlabel('time from First FIX in (seconds)') +title('Subplot 3: vZ') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') + +subplot(2,2,4); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.height,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),HEIGH_FILT,'r.'); +ylabel('HEIGH (m)') +xlabel('time from First FIX in (seconds)') +title('Subplot 4: HEIGH') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') + +sgtitle('velocities and heigh') + +% --- UTM centered POSITION: GNSS SDR plot -------------------------------------- + +POS=figure('Name','UTM COORD CENTERED IN 1^{ST} POSITION'); +subplot(2,2,1); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X-refSolution.X(1),'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),X_FILT-refSolution.X(1),'r.'); +ylabel('X (m)') +xlabel('time from First FIX in (seconds)') +title('Subplot 1: X GOR') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east') + +subplot(2,2,2); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Y-refSolution.Y(1),'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),Y_FILT-refSolution.Y(1),'r.'); +ylabel('Y (m)') +xlabel('time from First FIX in (seconds)') +title('Subplot 2: Y') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east') + +subplot(2,2,3); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Z-refSolution.Z(1),'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),Z_FILT-refSolution.Z(1),'r.'); +ylabel('Z (m)') +xlabel('time from First FIX in (seconds)') +title('Subplot 3: Z') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east') + +sgtitle('UTM COORD CENTERED IN 1^{ST} POSITION') +%% ====== SPIRENT Plot all figures ======================================================= + +%--- LLH POSITION: SPIRENT plot on map -------------------------------------- +figure(LLH) +geoplot([refSolution.latitude],[refSolution.longitude],'.','DisplayName','reference') +geobasemap satellite +hold off +% geobasemap streets +% geobasemap topographic +% geobasemap streets-dark + +%---VELOCITY: SPIRENT plot -------------------------------------- +figure(VEL) +subplot(2,2,1); +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vX,'.','DisplayName','reference'); +hold off;grid on + +subplot(2,2,2); +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vY,'.','DisplayName','reference'); +hold on;grid on + +subplot(2,2,3); +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vZ,'.','DisplayName','reference'); +hold on;grid on + +subplot(2,2,4); +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.height,'.','DisplayName','reference'); +hold on;grid on + +%---UTM centered POSITION: SPIRENT plot -------------------------------------- +figure(POS) +subplot(2,2,1); +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.X-refSolution.X(1),'.','DisplayName','reference'); +hold off;grid on + +subplot(2,2,2); +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Y-refSolution.Y(1),'.','DisplayName','reference'); +hold on;grid on + +subplot(2,2,3); +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Z-refSolution.Z(1),'.','DisplayName','reference'); +hold on;grid on + +%% ==== ERRORS Plots Navigation errors ======================================================= + +% figureNumber = 300; +% % The 300 is chosen for more convenient handling of the open +% % figure windows, when many figures are closed and reopened. Figures +% % drawn or opened by the user, will not be "overwritten" by this +% % function if the auto numbering is not used. +% +% %=== Select (or create) and clear the figure ========================== +% figure(figureNumber); +% clf (figureNumber); +% set (figureNumber, 'Name', 'Navigation errors'); +% +% %--- Draw axes -------------------------------------------------------- +% handles(1, 1) = subplot(4, 2, 1 : 4); +% handles(3, 1) = subplot(4, 2, [5, 7]); +% handles(3, 2) = subplot(4, 2, [6, 8]); +% +% +% % +% idx_syn=max(find(TTFF_sec*1000>refSolution.SIM_time)); +% %--- VELOCITY differences ----------------------------- +% plot([(navSolution.vX - refSolution.vX(idx_syn:end))'; ... +% (navSolution.vY - refSolution.vY)';... +% (navSolution.vZ - refSolution.vZ)']); +% +% title (handles(1, 1), 'VELOCITY variations'); +% legend(handles(1, 1), 'vX', 'vY', 'vZ'); +% xlabel(handles(1, 1), ['Measurement period: ', ... +% num2str(424242), 'ms']); +% ylabel(handles(1, 1), 'Variations (m/s)'); +% grid (handles(1, 1)); +% axis (handles(1, 1), 'tight'); + +%% +% plotNavigation(navSolutions,settings,plot_skyplot); +% +% %% Plot results in the necessary data exists ============================== +% if (~isempty(navSolutions)) +% +% %% If reference position is not provided, then set reference position +% %% to the average position +% if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ... +% || isnan(settings.truePosition.U) +% +% %=== Compute mean values ========================================== +% % Remove NaN-s or the output of the function MEAN will be NaN. +% refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E))); +% refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N))); +% refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U))); +% +% %Also convert geodetic coordinates to deg:min:sec vector format +% meanLongitude = dms2mat(deg2dms(... +% mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5); +% meanLatitude = dms2mat(deg2dms(... +% mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5); +% +% LatLong_str=[num2str(meanLatitude(1)), '??', ... +% num2str(meanLatitude(2)), '''', ... +% num2str(meanLatitude(3)), '''''', ... +% ',', ... +% num2str(meanLongitude(1)), '??', ... +% num2str(meanLongitude(2)), '''', ... +% num2str(meanLongitude(3)), ''''''] +% +% +% +% refPointLgText = ['Mean Position\newline Lat: ', ... +% num2str(meanLatitude(1)), '{\circ}', ... +% num2str(meanLatitude(2)), '{\prime}', ... +% num2str(meanLatitude(3)), '{\prime}{\prime}', ... +% '\newline Lng: ', ... +% num2str(meanLongitude(1)), '{\circ}', ... +% num2str(meanLongitude(2)), '{\prime}', ... +% num2str(meanLongitude(3)), '{\prime}{\prime}', ... +% '\newline Hgt: ', ... +% num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')]; +% +% else +% % compute the mean error for static receiver +% mean_position.E = mean(navSolutions.E(~isnan(navSolutions.E))); +% mean_position.N = mean(navSolutions.N(~isnan(navSolutions.N))); +% mean_position.U = mean(navSolutions.U(~isnan(navSolutions.U))); +% refCoord.E = settings.truePosition.E; +% refCoord.N = settings.truePosition.N; +% refCoord.U = settings.truePosition.U; +% +% error_meters=sqrt((mean_position.E-refCoord.E)^2+(mean_position.N-refCoord.N)^2+(mean_position.U-refCoord.U)^2); +% +% refPointLgText = ['Reference Position, Mean 3D error = ' num2str(error_meters) ' [m]']; +% end +% +% figureNumber = 300; +% % The 300 is chosen for more convenient handling of the open +% % figure windows, when many figures are closed and reopened. Figures +% % drawn or opened by the user, will not be "overwritten" by this +% % function if the auto numbering is not used. +% +% %=== Select (or create) and clear the figure ========================== +% figure(figureNumber); +% clf (figureNumber); +% set (figureNumber, 'Name', 'Navigation solutions'); +% +% %--- Draw axes -------------------------------------------------------- +% handles(1, 1) = subplot(4, 2, 1 : 4); +% handles(3, 1) = subplot(4, 2, [5, 7]); +% handles(3, 2) = subplot(4, 2, [6, 8]); + + %% Plot all figures ======================================================= +% +% %--- Coordinate differences in UTM system ----------------------------- +% plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ... +% (navSolutions.N - refCoord.N)',... +% (navSolutions.U - refCoord.U)']); +% +% title (handles(1, 1), 'Coordinates variations in UTM system'); +% legend(handles(1, 1), 'E', 'N', 'U'); +% xlabel(handles(1, 1), ['Measurement period: ', ... +% num2str(settings.navSolPeriod), 'ms']); +% ylabel(handles(1, 1), 'Variations (m)'); +% grid (handles(1, 1)); +% axis (handles(1, 1), 'tight'); +% +% %--- Position plot in UTM system -------------------------------------- +% plot3 (handles(3, 1), navSolutions.E - refCoord.E, ... +% navSolutions.N - refCoord.N, ... +% navSolutions.U - refCoord.U, '+'); +% hold (handles(3, 1), 'on'); +% +% %Plot the reference point +% plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10); +% hold (handles(3, 1), 'off'); +% +% view (handles(3, 1), 0, 90); +% axis (handles(3, 1), 'equal'); +% grid (handles(3, 1), 'minor'); +% +% legend(handles(3, 1), 'Measurements', refPointLgText); +% +% title (handles(3, 1), 'Positions in UTM system (3D plot)'); +% xlabel(handles(3, 1), 'East (m)'); +% ylabel(handles(3, 1), 'North (m)'); +% zlabel(handles(3, 1), 'Upping (m)'); +% +% if (plot_skyplot==1) +% %--- Satellite sky plot ----------------------------------------------- +% skyPlot(handles(3, 2), ... +% navSolution.channel.az, ... +% navSolution.channel.el, ... +% navSolution.channel.PRN(:, 1)); +% +% % title (handles(3, 2), ['Sky plot (mean PDOP: ', ... +% % num2str(mean(navSolution.DOP(2,:))), ')']); +% end +% +% else +% disp('plotNavigation: No navigation data to plot.'); +% end % if (~isempty(navSolutions)) diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m new file mode 100644 index 000000000..8ddf9ff63 --- /dev/null +++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m @@ -0,0 +1,344 @@ +% Read PVG raw dump +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% +%% +clc +close all +clearvars + +% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file') +% addpath('./libs') +% end +% +% if ~exist('cat2geo.m', 'file') +% addpath('./libs/geoFunctions') +% end +SPEED_OF_LIGHT_M_S=299792458.0; +Lambda_GPS_L1=0.1902937; +%% +samplingFreq=5000000; +channels=6; +TTFF_sec=41.48; + +%% ============================ PARSER TO STRUCT ============================ +plot_skyplot=0; +plot_reference=1; +load_observables=1; +advance_vtl_data_available=1; +load_vtl=1; + +navSolution = GnssSDR2struct('PVT_raw.mat'); +refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv'); +%% +if(load_observables) + load observables\observable_raw.mat + refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); + rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05. +end + +if(advance_vtl_data_available) + load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'... + ,'sat_velZ','sat_prg_m','clk_bias_s','sat_dopp_hz') +end + +if(load_vtl) +vtlSolution = Vtl2struct('dump_vtl_file.csv'); +end +%% calculate LOS Rx-sat if advance_vtl_data_available=1 + +if(advance_vtl_data_available) + for chan=1:5 + for t=1:length(navSolution.RX_time) + d(chan)=(sat_posX_m(chan,t)-navSolution.X(t))^2; + d(chan)=d(chan)+(sat_posY_m(chan,t)-navSolution.Y(t))^2; + d(chan)=d(chan)+(sat_posZ_m(chan,t)-navSolution.Z(t))^2; + d(chan)=sqrt(d(chan)); + + pr_m(chan,t)=d(chan)+clk_bias_s(t)*SPEED_OF_LIGHT_M_S; + + a_x(chan,t)=-(sat_posX_m(chan,t)-navSolution.X(t))/d(chan); + a_y(chan,t)=-(sat_posY_m(chan,t)-navSolution.Y(t))/d(chan); + a_z(chan,t)=-(sat_posZ_m(chan,t)-navSolution.Z(t))/d(chan); + end + end +end +%% +% figure;plot([a_x(1,:);a_y(1,:);a_z(1,:)]'); +% figure;plot([vtlSolution.LOSx vtlSolution.LOSy vtlSolution.LOSz]) +%% +if(advance_vtl_data_available) + for chan=1:5 + for t=1:length(navSolution.RX_time) +rhoDot_pri(chan,t)=(sat_velX(chan,t)-navSolution.vX(t))*a_x(chan,t)... + +(sat_velY(chan,t)-navSolution.vY(t))*a_y(chan,t)... + +(sat_velZ(chan,t)-navSolution.vZ(t))*a_z(chan,t); + +kf_yerr(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t); + end + end +end +%% +figure;plot(kf_yerr') +%% +figure;plot(pr_m'-sat_prg_m');hold on +legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside') +%% === Convert to UTM coordinate system ============================= + +% Scenario latitude is xx.xxxxxxx N37 49 9.98 +% Scenario longitude is -xx.xxxxxxx W122 28 42.58 +% Scenario elevation is 35 meters. + % lat=[37 49 9.98]; + % long=[-122 -28 -42.58]; + % lat_deg=dms2deg(lat); + % long_deg=dms2deg(long); + % h=35; + + lat_deg=navSolution.latitude(1); + lon_deg=navSolution.longitude(1); + lat=degrees2dms(lat_deg); + lon=degrees2dms(lon_deg); + h=navSolution.height(1); + + +utmstruct = defaultm('utm'); +utmstruct.zone = utmzone(lat_deg, lon_deg); +utmstruct = defaultm(utmstruct); +[utmstruct.latlim,utmstruct.lonlim] = utmzone(utmstruct.zone ); +%Choices i of Reference Ellipsoid +% 1. International Ellipsoid 1924 +% 2. International Ellipsoid 1967 +% 3. World Geodetic System 1972 +% 4. Geodetic Reference System 1980 +% 5. World Geodetic System 1984 + +utmstruct.geoid = wgs84Ellipsoid; + +% [X, Y] = projfwd(utmstruct,lat_deg,lon_deg); +% Z=h; % geographical to cartesian conversion + + +% for k=1:1:length(navSolution.X) +% [navSolution.E(k), ... +% navSolution.N(k), ... +% navSolution.U(k)]=cart2utm(navSolution.X(k), navSolution.Y(k), navSolution.Z(k), utmZone); +% end + +%% +general_raw_plot +%% + +%---VTL VELOCITY: GNSS SDR plot -------------------------------------- +VTL_VEL=figure('Name','velocities and heigh'); +subplot(2,2,1); +plot(vtlSolution.rtklibpvt.vX,'.'); +hold on;grid on +plot(vtlSolution.kfpvt.vX,'.'); +plot(vtlSolution.kferr.vX,'.'); +ylabel('vX (m/s)') +xlabel('time U.A.') +ylim([-5 5]) +title('Subplot 1: vX ') +legend ('raw RTKlib','raw kf','kferr','Location','east') + +subplot(2,2,2); +plot(vtlSolution.rtklibpvt.vY,'.'); +hold on;grid on +plot(vtlSolution.kfpvt.vY,'.'); +plot(vtlSolution.kferr.vY,'.'); +ylabel('vY (m/s)') +xlabel('time U.A.') +ylim([-5 5]) +title('Subplot 1: vY ') +legend ('raw RTKlib','raw kf','kferr','Location','east') + +subplot(2,2,3); +plot(vtlSolution.rtklibpvt.vZ,'.'); +hold on;grid on +plot(vtlSolution.kfpvt.vZ,'.'); +plot(vtlSolution.kferr.vZ,'.'); +ylabel('vZ (m/s)') +xlabel('time U.A.') +ylim([-5 5]) +title('Subplot 1: vZ ') +legend ('raw RTKlib','raw kf','kferr','Location','east') + +subplot(2,2,4); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.height,'.'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),HEIGH_FILT,'r.'); +ylabel('HEIGH (m)') +xlabel('time from First FIX in (seconds)') +title('Subplot 4: HEIGH') +legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast') + +sgtitle('velocities and heigh') + +% --- VTL UTM centered POSITION: GNSS SDR plot -------------------------------------- + +VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION'); +subplot(2,2,1); +plot(vtlSolution.rtklibpvt.X-vtlSolution.rtklibpvt.X(1),'.'); +hold on;grid on +plot(vtlSolution.kfpvt.X-vtlSolution.kfpvt.X(1),'.'); +plot(vtlSolution.kferr.X,'.'); +ylabel('X (m)') +xlabel('time U.A.') +ylim([-200 800]) +title('Subplot 1: X ') +legend ('raw RTKlib','raw kf','kferr','Location','east') + +subplot(2,2,2); +plot(vtlSolution.rtklibpvt.Y-vtlSolution.rtklibpvt.Y(1),'.'); +hold on;grid on +plot(vtlSolution.kfpvt.Y-vtlSolution.kfpvt.Y(1),'.'); +plot(vtlSolution.kferr.Y,'.'); +ylabel('Y (m)') +xlabel('time U.A.') +ylim([-200 50]) +title('Subplot 1: Y ') +legend ('raw RTKlib','raw kf','kferr','Location','east') + +subplot(2,2,3); +plot(vtlSolution.rtklibpvt.Z-vtlSolution.rtklibpvt.Z(1),'.'); +hold on;grid on +plot(vtlSolution.kfpvt.Z-vtlSolution.kfpvt.Z(1),'.'); +plot(vtlSolution.kferr.Z,'.'); +ylabel('Z (m)') +xlabel('time U.A.') +ylim([-350 50]) +title('Subplot 1: Z ') +legend ('raw RTKlib','raw kf','kferr','Location','east') + +sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION') +%% +close all +% --- 'VTL errPV correction -------------------------------------- + +VTL_errPV=figure('Name','VTL errPV correction'); +subplot(2,3,1); +plot(vtlSolution.rtklibpvt.X-vtlSolution.rtklibpvt.X(1),'.'); +hold on;grid on +plot(vtlSolution.kfpvt.X-vtlSolution.kfpvt.X(1),'.'); +plot(vtlSolution.kferr.X,'.'); +plot(vtlSolution.kfpvt.X-vtlSolution.rtklibpvt.X(1)+vtlSolution.kferr.X,'.'); +ylabel('X (m)') +xlabel('time U.A.') +ylim([-200 800]) +title('Subplot 1: X ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + +subplot(2,3,2); +plot(vtlSolution.rtklibpvt.Y-vtlSolution.rtklibpvt.Y(1),'.'); +hold on;grid on +plot(vtlSolution.kfpvt.Y-vtlSolution.kfpvt.Y(1),'.'); +plot(vtlSolution.kferr.Y,'.'); +plot(vtlSolution.kfpvt.Y-vtlSolution.rtklibpvt.Y(1)+vtlSolution.kferr.Y,'.'); +ylabel('Y (m)') +xlabel('time U.A.') +ylim([-200 50]) +title('Subplot 1: Y ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + +subplot(2,3,3); +plot(vtlSolution.rtklibpvt.Z-vtlSolution.rtklibpvt.Z(1),'.'); +hold on;grid on +plot(vtlSolution.kfpvt.Z-vtlSolution.kfpvt.Z(1),'.'); +plot(vtlSolution.kferr.Z,'.'); +plot(vtlSolution.kfpvt.Z-vtlSolution.rtklibpvt.Z(1)+vtlSolution.kferr.Z,'.'); +ylabel('Z (m)') +xlabel('time U.A.') +ylim([-350 50]) +title('Subplot 1: Z ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + +subplot(2,3,4); +plot(vtlSolution.rtklibpvt.vX,'.'); +hold on;grid on +plot(vtlSolution.kfpvt.vX,'.'); +plot(vtlSolution.kferr.vX,'.'); +plot(vtlSolution.kfpvt.vX+vtlSolution.kferr.vX,'.'); +ylabel('vX (m/s)') +xlabel('time U.A.') +ylim([-5 5]) +title('Subplot 1: vX ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + +subplot(2,3,5); +plot(vtlSolution.rtklibpvt.vY,'.'); +hold on;grid on +plot(vtlSolution.kfpvt.vY,'.'); +plot(vtlSolution.kferr.vY,'.'); +plot(vtlSolution.kfpvt.vY+vtlSolution.kferr.vY,'.'); +ylabel('vY (m/s)') +xlabel('time U.A.') +ylim([-5 5]) +title('Subplot 1: vY ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + +subplot(2,3,6); +plot(vtlSolution.rtklibpvt.vZ,'.'); +hold on;grid on +plot(vtlSolution.kfpvt.vZ,'.'); +plot(vtlSolution.kferr.vZ,'.'); +plot(vtlSolution.kfpvt.vZ+vtlSolution.kferr.vZ,'.'); +ylabel('vZ (m/s)') +xlabel('time U.A.') +ylim([-5 5]) +title('Subplot 1: vZ ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + + +sgtitle('VTL errPV correction') +%% ============================================== ============================================== +time_reference_spirent_obs=129780;%s +if(load_observables) +% figure;plot(Carrier_phase_cycles','.') +% figure;plot(Pseudorange_m','.') + %%% + Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),6); + + for i=1:length(refSatData.GPS.SIM_time) + Carrier_Doppler_hz_sim(i,1)=refSatData.GPS.series(i).doppler_shift(4);%PRN 28 + Carrier_Doppler_hz_sim(i,2)=refSatData.GPS.series(i).doppler_shift(1);%PRN 4 + Carrier_Doppler_hz_sim(i,3)=refSatData.GPS.series(i).doppler_shift(6);%PRN 17 + Carrier_Doppler_hz_sim(i,4)=refSatData.GPS.series(i).doppler_shift(7);%PRN 15 + Carrier_Doppler_hz_sim(i,5)=refSatData.GPS.series(i).doppler_shift(8);%PRN 27 + Carrier_Doppler_hz_sim(i,6)=refSatData.GPS.series(i).doppler_shift(9);%PRN 9 + + end + + + Rx_Dopp_all=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz','.') + xlim([0,140]); + xlabel('') + ylabel('Doppler (Hz)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim','.') + legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside') + hold off + grid on + + Rx_Dopp_one=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','.') + xlim([0,140]); + ylim([-2340,-2220]); + xlabel('') + ylabel('Doppler (Hz)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend('PRN 28 GNSS-SDR','Location','eastoutside') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference') + hold off + grid on +end \ No newline at end of file From 7dcb922b9fcca8f0aaa4e39149c64a98f83ec9e2 Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 8 Dec 2022 13:57:04 +0100 Subject: [PATCH 038/165] fix: buf in kf_H declaration dim --- src/algorithms/PVT/libs/vtl_engine.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 25e322509..94fd7ea06 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -51,8 +51,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_F(6, 6) = 1.0; kf_F(6, 7) = kf_dt; kf_F(7, 7) = 1.0; - kf_H = arma::zeros(8, 2*new_data.sat_number); - kf_x = arma::zeros(8, 1); + kf_H = arma::zeros(2*new_data.sat_number,8); kf_y = arma::zeros(2*new_data.sat_number, 1); kf_yerr = arma::zeros(2*new_data.sat_number, 1); kf_xerr = arma::zeros(8, 1); From 3062b5565dfdea28133fb3eb97bafb97709bf79c Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 8 Dec 2022 13:58:38 +0100 Subject: [PATCH 039/165] MOD: extract filtering from plot routine --- src/utils/matlab/vtl/general_raw_plot.m | 14 -------------- .../matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 12 ++++++++++++ 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/utils/matlab/vtl/general_raw_plot.m b/src/utils/matlab/vtl/general_raw_plot.m index a9dc27570..e295aa543 100644 --- a/src/utils/matlab/vtl/general_raw_plot.m +++ b/src/utils/matlab/vtl/general_raw_plot.m @@ -1,19 +1,5 @@ %general_raw_plot.m -%% ====== FILTERING ======================================================= -moving_avg_factor= 500; -LAT_FILT = movmean(navSolution.latitude,moving_avg_factor); -LON_FILT = movmean(navSolution.longitude,moving_avg_factor); -HEIGH_FILT = movmean(navSolution.height,moving_avg_factor); - -X_FILT = movmean(navSolution.X,moving_avg_factor); -Y_FILT = movmean(navSolution.Y,moving_avg_factor); -Z_FILT = movmean(navSolution.Z,moving_avg_factor); - -vX_FILT = movmean(navSolution.vX,moving_avg_factor); -vY_FILT = movmean(navSolution.vY,moving_avg_factor); -vZ_FILT = movmean(navSolution.vZ,moving_avg_factor); - % navSolution.X(navSolution.X==0) = []; %% ====== GNSS-SDR Plot all figures ======================================================= close all diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m index 8ddf9ff63..c4841e21c 100644 --- a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m +++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m @@ -130,7 +130,19 @@ utmstruct.geoid = wgs84Ellipsoid; % navSolution.N(k), ... % navSolution.U(k)]=cart2utm(navSolution.X(k), navSolution.Y(k), navSolution.Z(k), utmZone); % end +%% ====== FILTERING ======================================================= +moving_avg_factor= 500; +LAT_FILT = movmean(navSolution.latitude,moving_avg_factor); +LON_FILT = movmean(navSolution.longitude,moving_avg_factor); +HEIGH_FILT = movmean(navSolution.height,moving_avg_factor); +X_FILT = movmean(navSolution.X,moving_avg_factor); +Y_FILT = movmean(navSolution.Y,moving_avg_factor); +Z_FILT = movmean(navSolution.Z,moving_avg_factor); + +vX_FILT = movmean(navSolution.vX,moving_avg_factor); +vY_FILT = movmean(navSolution.vY,moving_avg_factor); +vZ_FILT = movmean(navSolution.vZ,moving_avg_factor); %% general_raw_plot %% From ce964eea67a82f68f53e5f29ed6810bea51e2b35 Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 8 Dec 2022 13:58:54 +0100 Subject: [PATCH 040/165] ADD: new matlab routines for vtl --- src/utils/matlab/vtl/kf_prototype.m | 142 ++++++++++++++++++++++ src/utils/matlab/vtl/vtl_general_plot.m | 155 ++++++++++++++++++++++++ src/utils/matlab/vtl/vtl_prototype.m | 149 +++++++++++++++++++++++ 3 files changed, 446 insertions(+) create mode 100644 src/utils/matlab/vtl/kf_prototype.m create mode 100644 src/utils/matlab/vtl/vtl_general_plot.m create mode 100644 src/utils/matlab/vtl/vtl_prototype.m diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m new file mode 100644 index 000000000..a113e4357 --- /dev/null +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -0,0 +1,142 @@ +%% vtl KF +%% +sat_number=5; +%% ################## Kalman filter initialization ###################################### +% covariances (static) +kf_P_x = eye(8)*10.0; %TODO: use a real value. +kf_x = zeros(8, 1); +kf_R = zeros(2*sat_number, 2*sat_number); +kf_dt=0.1; +kf_F = eye(8, 8); +kf_F(1, 4) = kf_dt; +kf_F(2, 5) = kf_dt; +kf_F(3, 6) = kf_dt; +kf_F(7, 8) = kf_dt; + +kf_H = zeros(2*sat_number,8); +kf_y = zeros(2*sat_number, 1); +kf_yerr = zeros(2*sat_number, 1); +% kf_xerr = zeros(8, 1); +kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix + +%% State error Covariance Matrix Q (PVT) +kf_Q = eye(8);%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate. +%% + +% Measurement error Covariance Matrix R assembling +for chan=1:5 %neccesary quantities + for t=1:length(navSolution.RX_time) + % It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) + kf_R(chan, chan) = 40.0; %TODO: fill with real values. + kf_R(chan+sat_number, chan+sat_number) = 10.0; + end +end + +%% ################## Kalman Tracking ###################################### +% receiver solution from rtklib_solver +for t=1:length(navSolution.RX_time) + + if (t Date: Thu, 8 Dec 2022 14:01:27 +0100 Subject: [PATCH 041/165] fix: wrong sign in state error correction --- src/algorithms/PVT/libs/vtl_engine.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 94fd7ea06..b24b3c416 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -80,7 +80,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(3) = new_data.kf_state[3]; kf_x(4) = new_data.kf_state[4]; kf_x(5) = new_data.kf_state[5]; - kf_x(6) = new_data.rx_dts(0); + kf_x(6) = new_data.kf_state[6];//new_data.rx_dts(0); kf_x(7) = new_data.rx_dts(1); kf_P_x=new_data.kf_P; @@ -175,7 +175,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_S = kf_H * kf_P_x* kf_H.t() + kf_R; // innovation covariance matrix (S) kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain kf_xerr = kf_K * (kf_yerr); // Error state estimation - //kf_x = kf_x + kf_xerr; // updated state estimation (a priori + error) + kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix new_data.kf_state=kf_x; //updated state estimation //new_data.kf_state.print("computed kf_state"); From 30233b177b6c0002d6615c343e7bd5bfd6a81084 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 8 Dec 2022 14:35:33 +0000 Subject: [PATCH 042/165] fix: some major issues fiexed. KF not exploding with clock bypassed --- src/algorithms/PVT/libs/vtl_engine.cc | 65 ++++++++++++--------------- 1 file changed, 29 insertions(+), 36 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index b24b3c416..6ebc63fc9 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -35,21 +35,17 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // ################## Kalman filter initialization ###################################### // covariances (static) - kf_P_x = arma::eye(8, 8)*100.0; //TODO: use a real value. + kf_P_x = arma::eye(8, 8); //TODO: use a real value. kf_x = arma::zeros(8, 1); kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); double kf_dt=0.1; - kf_Q = arma::zeros(8, 8); + kf_Q = arma::eye(8, 8); - kf_F = arma::zeros(8, 8); - kf_F(0, 0) = 1.0; kf_F(0, 3) = kf_dt; - kf_F(1, 1) = 1.0; kf_F(1, 4) = kf_dt; - kf_F(2, 2) = 1.0; kf_F(2, 5) = kf_dt; - kf_F(3, 3) = 1.0; - kf_F(4, 4) = 1.0; - kf_F(5, 5) = 1.0; - kf_F(6, 6) = 1.0; kf_F(6, 7) = kf_dt; - kf_F(7, 7) = 1.0; + kf_F = arma::eye(8, 8); + kf_F(0, 3) = kf_dt; + kf_F(1, 4) = kf_dt; + kf_F(2, 5) = kf_dt; + kf_F(6, 7) = kf_dt; kf_H = arma::zeros(2*new_data.sat_number,8); kf_y = arma::zeros(2*new_data.sat_number, 1); @@ -71,7 +67,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(4) = new_data.rx_v(1); kf_x(5) = new_data.rx_v(2); kf_x(6) = new_data.rx_dts(0); - kf_x(7) = new_data.rx_dts(1); + kf_x(7) = 1e-8; + } else { // receiver solution from previous KF step kf_x(0) = new_data.kf_state[0]; @@ -81,16 +78,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(4) = new_data.kf_state[4]; kf_x(5) = new_data.kf_state[5]; kf_x(6) = new_data.kf_state[6];//new_data.rx_dts(0); - kf_x(7) = new_data.rx_dts(1); - + kf_x(7) = 1e-8; kf_P_x=new_data.kf_P; } // State error Covariance Matrix Q (PVT) - for (int32_t i = 0; i < 8; i++) - { - // It is diagonal 8x8 matrix - kf_Q(i, i) = 1.0;//new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate. - } + // for (int32_t i = 0; i < 8; i++) + // { + // // It is diagonal 8x8 matrix + // kf_Q(i, i) = 1.0;//new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate. + // } // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) { @@ -112,8 +108,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) xDot_u=kf_x(3); yDot_u=kf_x(4); zDot_u=kf_x(5); - cdeltat_u=kf_x(6)*SPEED_OF_LIGHT_M_S; - cdeltatDot_u=kf_x(7)*SPEED_OF_LIGHT_M_S; + cdeltat_u=new_data.rx_dts(0)*SPEED_OF_LIGHT_M_S; + cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S; d = arma::zeros(new_data.sat_number, 1); rho_pri = arma::zeros(new_data.sat_number, 1); @@ -139,7 +135,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) new_data.sat_LOS(i,1)=a_y(i); new_data.sat_LOS(i,2)=a_z(i); //compute pseudorange rate estimation - rhoDot_pri(i)=(new_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_data.sat_v(i, 2)-zDot_u)*a_z(i)+cdeltatDot_u; + rhoDot_pri(i)=(new_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_data.sat_v(i, 2)-zDot_u)*a_z(i); } kf_H = arma::zeros(2*new_data.sat_number,8); @@ -155,8 +151,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) { //kf_y(i) = new_data.pr_m(i); // i-Satellite //kf_y(i+new_data.sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; // i-Satellite - kf_yerr(i)=new_data.pr_m(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S; - kf_yerr(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(i); + kf_yerr(i)=rho_pri(i)-new_data.pr_m(i);//-0.000157*SPEED_OF_LIGHT_M_S; + kf_yerr(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1)-rhoDot_pri(i); } @@ -177,8 +173,16 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_xerr = kf_K * (kf_yerr); // Error state estimation kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix + new_data.kf_P=kf_P_x; new_data.kf_state=kf_x; //updated state estimation //new_data.kf_state.print("computed kf_state"); + + // States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) + // kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; + // kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; + + new_data.kf_state(6)=new_data.kf_state(6)/SPEED_OF_LIGHT_M_S; + new_data.kf_state(7)=1e-8/SPEED_OF_LIGHT_M_S; // TODO: compare how KF state diverges from RTKLIB solution! fstream dump_vtl_file; @@ -190,20 +194,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) else { dump_vtl_file << "kf_xerr"<< ","< Date: Thu, 8 Dec 2022 14:41:47 +0000 Subject: [PATCH 043/165] ADD: clk_rate_s to dump and mat files --- src/algorithms/PVT/libs/rtklib_solver.cc | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index b4246cd7a..023f4842f 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -204,7 +204,7 @@ bool Rtklib_Solver::save_matfile() const { // READ DUMP FILE const std::string dump_filename = d_dump_filename; - const int32_t number_of_double_vars = 22; //+1 MAGL + const int32_t number_of_double_vars = 23; //+2 MAGL const int32_t number_of_double_vars_sat = 8; //+ pos/vel/psrange/doppler for sat const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; @@ -256,7 +256,8 @@ bool Rtklib_Solver::save_matfile() const auto cov_yz = std::vector(num_epoch); auto cov_zx = std::vector(num_epoch); - auto vel_z2 = std::vector(num_epoch); + auto clk_bias = std::vector(num_epoch); + auto clk_rate = std::vector(num_epoch); auto sat_posX_m = std::vector>(5, std::vector(num_epoch)); auto sat_posY_m = std::vector>(5, std::vector(num_epoch)); auto sat_posZ_m = std::vector>(5, std::vector(num_epoch)); @@ -302,7 +303,8 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&cov_yz[i]), sizeof(double)); dump_file.read(reinterpret_cast(&cov_zx[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&vel_z2[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&clk_bias[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&clk_rate[i]), sizeof(double)); for (uint32_t chan = 0; chan < 5; chan++) { dump_file.read(reinterpret_cast(&sat_posX_m[chan][i]), sizeof(double)); @@ -436,7 +438,11 @@ bool Rtklib_Solver::save_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("clk_bias_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z2.data(), 0); + matvar = Mat_VarCreate("clk_bias_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_bias.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("clk_rate_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_rate.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -1403,6 +1409,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = vtl_data.rx_dts(0); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = vtl_data.rx_dts(1); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); for (int n = 0; n<5; n++) { tmp_double = vtl_data.sat_p(n, 0); From 4cc75d419f14513520f697c91fdcbf1c31736e42 Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 8 Dec 2022 19:56:25 +0100 Subject: [PATCH 044/165] MOD:vtl2struct LOS vector removed --- src/utils/matlab/vtl/Vtl2struct.m | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/utils/matlab/vtl/Vtl2struct.m b/src/utils/matlab/vtl/Vtl2struct.m index 8c3234e1a..91dac2a8a 100644 --- a/src/utils/matlab/vtl/Vtl2struct.m +++ b/src/utils/matlab/vtl/Vtl2struct.m @@ -48,17 +48,14 @@ vtlSolution.rtklibpvt=[]; [indKF,~]= find(strcmp(dumpvtlfile, 'kf_state')); [indRTKlib,~]= find(strcmp(dumpvtlfile, 'rtklib_state')); [indkf_err,~]= find(strcmp(dumpvtlfile, 'kf_xerr')); -[ind_LOS,~]= find(strcmp(dumpvtlfile, 'sat_first_LOS')); kfpvt=dumpvtlfile(indKF,:);kfpvt(:,1)=[]; rtklibpvt=dumpvtlfile(indRTKlib,:); rtklibpvt(:,1)=[]; kferr=dumpvtlfile(indkf_err,:); kferr(:,1)=[]; -LOS=dumpvtlfile(ind_LOS,:); LOS(:,1)=[]; kfpvt=cell2mat(kfpvt); rtklibpvt=cell2mat(rtklibpvt); kferr=cell2mat(kferr); -LOS=cell2mat(LOS); vtlSolution.kfpvt.X=kfpvt(:,1); vtlSolution.kfpvt.Y=kfpvt(:,2); @@ -87,7 +84,4 @@ vtlSolution.kferr.vZ=kferr(:,6); vtlSolution.kferr.biasclock=kferr(:,7); vtlSolution.kferr.rateblock=kferr(:,8); -vtlSolution.LOSx=LOS(:,1); -vtlSolution.LOSy=LOS(:,2); -vtlSolution.LOSz=LOS(:,3); end \ No newline at end of file From 033a87be4badbd143a41523b5c60f009cea0e8fc Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 8 Dec 2022 19:57:02 +0100 Subject: [PATCH 045/165] MOD: vtl matlab kf changes --- src/utils/matlab/vtl/kf_prototype.m | 108 +++++++++++++++++---------- src/utils/matlab/vtl/vtl_prototype.m | 22 +++++- 2 files changed, 86 insertions(+), 44 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index a113e4357..f6f52a4da 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -3,16 +3,25 @@ sat_number=5; %% ################## Kalman filter initialization ###################################### % covariances (static) -kf_P_x = eye(8)*10.0; %TODO: use a real value. +kf_P_x = eye(8); %TODO: use a real value. kf_x = zeros(8, 1); kf_R = zeros(2*sat_number, 2*sat_number); kf_dt=0.1; -kf_F = eye(8, 8); -kf_F(1, 4) = kf_dt; -kf_F(2, 5) = kf_dt; -kf_F(3, 6) = kf_dt; -kf_F(7, 8) = kf_dt; +% kf_F = eye(8, 8); +% kf_F(1, 4) = kf_dt; +% kf_F(2, 5) = kf_dt; +% kf_F(3, 6) = kf_dt; +% kf_F(7, 8) = kf_dt; +kf_F=[ 1 0 0 kf_dt 0 0 0 0 + 0 1 0 0 kf_dt 0 0 0 + 0 0 1 0 0 kf_dt 0 0 + 0 0 0 1 0 0 0 0 + 0 0 0 0 1 0 0 0 + 0 0 0 0 0 1 0 0 + 0 0 0 0 0 0 1 kf_dt + 0 0 0 0 0 0 0 1]; + kf_H = zeros(2*sat_number,8); kf_y = zeros(2*sat_number, 1); kf_yerr = zeros(2*sat_number, 1); @@ -20,18 +29,41 @@ kf_yerr = zeros(2*sat_number, 1); kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix %% State error Covariance Matrix Q (PVT) -kf_Q = eye(8);%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate. +kf_Q = [ 1 0 0 0 0 0 0 0 + 0 1 0 0 0 0 0 0 + 0 0 1 0 0 0 0 0 + 0 0 0 1 0 0 0 0 + 0 0 0 0 1 0 0 0 + 0 0 0 0 0 1 0 0 + 0 0 0 0 0 0 1e-7 0 + 0 0 0 0 0 0 0 1e-4];%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate. %% -% Measurement error Covariance Matrix R assembling -for chan=1:5 %neccesary quantities - for t=1:length(navSolution.RX_time) - % It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(chan, chan) = 40.0; %TODO: fill with real values. - kf_R(chan+sat_number, chan+sat_number) = 10.0; - end -end +% % Measurement error Covariance Matrix R assembling +% for chan=1:5 %neccesary quantities +% % It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) +% kf_R(chan, chan) = 40.0; %TODO: fill with real values. +% kf_R(chan+sat_number, chan+sat_number) = 10.0; +% end +kf_R=[ 3 0 0 0 0 0 0 0 0 0 + 0 3 0 0 0 0 0 0 0 0 + 0 0 3 0 0 0 0 0 0 0 + 0 0 0 3 0 0 0 0 0 0 + 0 0 0 0 3 0 0 0 0 0 + 0 0 0 0 0 30 0 0 0 0 + 0 0 0 0 0 0 30 0 0 0 + 0 0 0 0 0 0 0 30 0 0 + 0 0 0 0 0 0 0 0 30 0 + 0 0 0 0 0 0 0 0 0 30]; + + d = zeros(sat_number, 1); + rho_pri = zeros(sat_number, 1); + rhoDot_pri = zeros(sat_number, 1); + a_x = zeros(sat_number, 1); + a_y = zeros(sat_number, 1); + a_z = zeros(sat_number, 1); + c_pr_m=zeros(sat_number,length(navSolution.RX_time)); %% ################## Kalman Tracking ###################################### % receiver solution from rtklib_solver for t=1:length(navSolution.RX_time) @@ -44,17 +76,7 @@ for t=1:length(navSolution.RX_time) kf_x(5,t) = navSolution.vY(t); kf_x(6,t) = navSolution.vZ(t); kf_x(7,t) = clk_bias_s(t); - kf_x(8,t) = 1e-6;%new_data.rx_dts(1); - x_u=kf_x(1,t); - y_u=kf_x(2,t); - z_u=kf_x(3,t); - xDot_u=kf_x(4,t); - yDot_u=kf_x(5,t); - zDot_u=kf_x(6,t); - cdeltat_u=kf_x(7,t)*SPEED_OF_LIGHT_M_S; - cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S; - else - kf_x(:,t)=corr_kf_state(:,t-1); + kf_x(8,t) = clk_drift(t);%new_data.rx_dts(1); x_u=kf_x(1,t); y_u=kf_x(2,t); @@ -62,23 +84,27 @@ for t=1:length(navSolution.RX_time) xDot_u=kf_x(4,t); yDot_u=kf_x(5,t); zDot_u=kf_x(6,t); - cdeltat_u=kf_x(7,t)*SPEED_OF_LIGHT_M_S; - cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S; + cdeltat_u(t)=kf_x(7,t)*SPEED_OF_LIGHT_M_S; + cdeltatDot_u=kf_x(8,t)*SPEED_OF_LIGHT_M_S; + kf_P_x = eye(8); %TODO: use a real value. + + else + kf_x(:,t)=corr_kf_state(:,t-1); + x_u=kf_x(1,t); + y_u=kf_x(2,t); + z_u=kf_x(3,t); + xDot_u=kf_x(4,t); + yDot_u=kf_x(5,t); + zDot_u=kf_x(6,t); + cdeltat_u(t)=kf_x(7,t)*SPEED_OF_LIGHT_M_S;% + cdeltatDot_u=kf_x(8,t)*SPEED_OF_LIGHT_M_S;% end % Kalman state prediction (time update) kf_x(:,t) = kf_F * kf_x(:,t); % state prediction kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q; % state error covariance prediction %from error state variables to variables % From state variables definition - % TODO: cast to type properly - - d = zeros(sat_number, 1); - rho_pri = zeros(sat_number, 1); - rhoDot_pri = zeros(sat_number, 1); - a_x = zeros(sat_number, 1); - a_y = zeros(sat_number, 1); - a_z = zeros(sat_number, 1); - c_pr_m=zeros(sat_number,length(navSolution.RX_time)); + % TODO: cast to type properly for chan=1:5 %neccesary quantities d(chan)=(sat_posX_m(chan,t)-kf_x(1,t))^2; @@ -86,7 +112,7 @@ for t=1:length(navSolution.RX_time) d(chan)=d(chan)+(sat_posZ_m(chan,t)-kf_x(3,t))^2; d(chan)=sqrt(d(chan)); - c_pr_m(chan,t)=d(chan)+clk_bias_s(t)*SPEED_OF_LIGHT_M_S; + c_pr_m(chan,t)=d(chan)+cdeltat_u(t); a_x(chan,t)=-(sat_posX_m(chan,t)-kf_x(1,t))/d(chan); a_y(chan,t)=-(sat_posY_m(chan,t)-kf_x(2,t))/d(chan); @@ -94,7 +120,7 @@ for t=1:length(navSolution.RX_time) rhoDot_pri(chan,t)=(sat_velX(chan,t)-kf_x(4,t))*a_x(chan,t)... +(sat_velY(chan,t)-kf_x(5,t))*a_y(chan,t)... - +(sat_velZ(chan,t)-kf_x(6,t))*a_z(chan,t); + +(sat_velZ(chan,t)-kf_x(6,t))*a_z(chan,t)+cdeltatDot_u; end @@ -110,7 +136,7 @@ for t=1:length(navSolution.RX_time) %kf_y(i) = new_data.pr_m(i); % i-Satellite %kf_y(i+sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; % i-Satellite kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);%-0.000157*SPEED_OF_LIGHT_M_S; - kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t); + kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(chan,t); end % DOUBLES DIFFERENCES @@ -138,5 +164,5 @@ for t=1:length(navSolution.RX_time) % kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; % kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; corr_kf_state(7,t)=corr_kf_state(7,t)/SPEED_OF_LIGHT_M_S; - corr_kf_state(8,t)=1e-8/SPEED_OF_LIGHT_M_S; + corr_kf_state(8,t)=corr_kf_state(8,t)/SPEED_OF_LIGHT_M_S;% forced !!!!! end \ No newline at end of file diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m index cba492250..8a07f510c 100644 --- a/src/utils/matlab/vtl/vtl_prototype.m +++ b/src/utils/matlab/vtl/vtl_prototype.m @@ -23,7 +23,7 @@ clearvars % end SPEED_OF_LIGHT_M_S=299792458.0; Lambda_GPS_L1=0.1902937; -point_of_closure=6000; +point_of_closure=5; %% samplingFreq=5000000; channels=6; @@ -40,9 +40,9 @@ load observables\observable_raw.mat % refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05. load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'... - ,'sat_velZ','sat_prg_m','clk_bias_s','sat_dopp_hz') + ,'sat_velZ','sat_prg_m','clk_bias_s','clk_drift','sat_dopp_hz','user_clk_offset') +%% vtlSolution = Vtl2struct('dump_vtl_file.csv'); - %% calculate LOS Rx-sat % for chan=1:5 @@ -102,6 +102,22 @@ legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside') %% %general_raw_plot vtl_general_plot +%% +close all +figure;plot(kf_xerr(7,:),'.');title('clk bias err') +figure;plot(kf_xerr(8,:),'.');title('clk drift err') +figure;plot(kf_x(7,:),'.');title('clk bias state') +figure;plot(kf_x(8,:),'.');title('clk drift state') +figure;plot(corr_kf_state(7,:),'.');title('clk bias corrected state') +figure;plot(corr_kf_state(8,:),'.');title('clk drift corrected state') +%% +% close all +% kferr_pos_all=[vtlSolution.kferr.X vtlSolution.kferr.Y vtlSolution.kferr.Z]; +% kferr_vel_all=[vtlSolution.kferr.vX vtlSolution.kferr.vY vtlSolution.kferr.vZ]; +% figure;plot(kferr_pos_all,'.');title('original pos err') +% figure;plot(kf_xerr(1:3,:)','.');title('calc pos err') +% figure;plot(kferr_vel_all,'.');title('original vel err') +% figure;plot(kf_xerr(4:6,:)','.');title('calc vel err') %% ============================================== ============================================== % time_reference_spirent_obs=129780;%s % if(load_observables) From 594908799281262429c42daeeabc0f5c515e167d Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 8 Dec 2022 19:57:54 +0100 Subject: [PATCH 046/165] MOD: vMATLAB utils PVT mat file clk_drift variable imported --- src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m index c4841e21c..291c7cf4f 100644 --- a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m +++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m @@ -46,7 +46,7 @@ end if(advance_vtl_data_available) load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'... - ,'sat_velZ','sat_prg_m','clk_bias_s','sat_dopp_hz') + ,'sat_velZ','sat_prg_m','clk_bias_s','clk_drift','sat_dopp_hz') end if(load_vtl) @@ -231,7 +231,6 @@ legend ('raw RTKlib','raw kf','kferr','Location','east') sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION') %% -close all % --- 'VTL errPV correction -------------------------------------- VTL_errPV=figure('Name','VTL errPV correction'); From 0b34880fc6f79f0dfe0aa0a747e6b1d49ceba8f5 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 8 Dec 2022 19:02:28 +0000 Subject: [PATCH 047/165] mod: clock rate is clock drift --- src/algorithms/PVT/libs/rtklib_solver.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 023f4842f..481d11757 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -257,7 +257,7 @@ bool Rtklib_Solver::save_matfile() const auto cov_zx = std::vector(num_epoch); auto clk_bias = std::vector(num_epoch); - auto clk_rate = std::vector(num_epoch); + auto clk_drift = std::vector(num_epoch); auto sat_posX_m = std::vector>(5, std::vector(num_epoch)); auto sat_posY_m = std::vector>(5, std::vector(num_epoch)); auto sat_posZ_m = std::vector>(5, std::vector(num_epoch)); @@ -304,7 +304,7 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&cov_zx[i]), sizeof(double)); dump_file.read(reinterpret_cast(&clk_bias[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&clk_rate[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&clk_drift[i]), sizeof(double)); for (uint32_t chan = 0; chan < 5; chan++) { dump_file.read(reinterpret_cast(&sat_posX_m[chan][i]), sizeof(double)); @@ -442,7 +442,7 @@ bool Rtklib_Solver::save_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("clk_rate_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_rate.data(), 0); + matvar = Mat_VarCreate("clk_drift", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_drift.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); From 6b74976a4a6871ba083d51e7cf512a1046bc8ef9 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 8 Dec 2022 19:36:32 +0000 Subject: [PATCH 048/165] fix: state is not clk, is clk times C --- src/algorithms/PVT/libs/vtl_engine.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 6ebc63fc9..1c8fd7244 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -66,8 +66,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(3) = new_data.rx_v(0); kf_x(4) = new_data.rx_v(1); kf_x(5) = new_data.rx_v(2); - kf_x(6) = new_data.rx_dts(0); - kf_x(7) = 1e-8; + kf_x(6) = new_data.rx_dts(0)*SPEED_OF_LIGHT_M_S; + kf_x(7) = new_data.rx_dts(1)*SPEED_OF_LIGHT_M_S; } else { // receiver solution from previous KF step @@ -77,8 +77,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(3) = new_data.kf_state[3]; kf_x(4) = new_data.kf_state[4]; kf_x(5) = new_data.kf_state[5]; - kf_x(6) = new_data.kf_state[6];//new_data.rx_dts(0); - kf_x(7) = 1e-8; + kf_x(6) = new_data.kf_state[6]; + kf_x(7) = new_data.kf_state[7]; kf_P_x=new_data.kf_P; } // State error Covariance Matrix Q (PVT) @@ -108,8 +108,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) xDot_u=kf_x(3); yDot_u=kf_x(4); zDot_u=kf_x(5); - cdeltat_u=new_data.rx_dts(0)*SPEED_OF_LIGHT_M_S; - cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S; + cdeltat_u=kf_x(6); + cdeltatDot_u=kf_x(7); d = arma::zeros(new_data.sat_number, 1); rho_pri = arma::zeros(new_data.sat_number, 1); From 1564af8c111bacb4633e0d2a192b676d94fd34e7 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 8 Dec 2022 19:42:33 +0000 Subject: [PATCH 049/165] fix: kf error s to m ( time C) --- src/algorithms/PVT/libs/vtl_engine.cc | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 1c8fd7244..8f7291f9a 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -176,16 +176,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) new_data.kf_P=kf_P_x; new_data.kf_state=kf_x; //updated state estimation //new_data.kf_state.print("computed kf_state"); - - // States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) - // kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; - // kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; - - new_data.kf_state(6)=new_data.kf_state(6)/SPEED_OF_LIGHT_M_S; - new_data.kf_state(7)=1e-8/SPEED_OF_LIGHT_M_S; - // TODO: compare how KF state diverges from RTKLIB solution! - - fstream dump_vtl_file; + fstream dump_vtl_file; dump_vtl_file.open("dump_vtl_file.csv", ios::out|ios::app); dump_vtl_file.precision(15); if (!dump_vtl_file) { From 9f8284a90932429d90ed5a8496e5aea221d29eff Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Thu, 8 Dec 2022 19:48:14 +0000 Subject: [PATCH 050/165] mod: delete some debug related lines --- src/algorithms/PVT/libs/vtl_engine.cc | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 8f7291f9a..b82025950 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -58,7 +58,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) counter = counter+1; //uint64_t cout << "counter" << counter< Date: Thu, 8 Dec 2022 20:50:56 +0100 Subject: [PATCH 051/165] fix: vtl kf in matlab version working --- src/utils/matlab/vtl/kf_prototype.m | 36 +++++++++++----------------- src/utils/matlab/vtl/vtl_prototype.m | 16 ++++++------- 2 files changed, 22 insertions(+), 30 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index f6f52a4da..78bed097d 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -35,8 +35,8 @@ kf_Q = [ 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 - 0 0 0 0 0 0 1e-7 0 - 0 0 0 0 0 0 0 1e-4];%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate. + 0 0 0 0 0 0 1e-9 0 + 0 0 0 0 0 0 0 1e-7];%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate. %% % % Measurement error Covariance Matrix R assembling @@ -75,8 +75,8 @@ for t=1:length(navSolution.RX_time) kf_x(4,t) = navSolution.vX(t); kf_x(5,t) = navSolution.vY(t); kf_x(6,t) = navSolution.vZ(t); - kf_x(7,t) = clk_bias_s(t); - kf_x(8,t) = clk_drift(t);%new_data.rx_dts(1); + kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S; + kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1); x_u=kf_x(1,t); y_u=kf_x(2,t); @@ -84,9 +84,9 @@ for t=1:length(navSolution.RX_time) xDot_u=kf_x(4,t); yDot_u=kf_x(5,t); zDot_u=kf_x(6,t); - cdeltat_u(t)=kf_x(7,t)*SPEED_OF_LIGHT_M_S; - cdeltatDot_u=kf_x(8,t)*SPEED_OF_LIGHT_M_S; - kf_P_x = eye(8); %TODO: use a real value. + cdeltat_u(t)=kf_x(7,t); + cdeltatDot_u=kf_x(8,t); + kf_P_x = eye(8)*100; %TODO: use a real value. else kf_x(:,t)=corr_kf_state(:,t-1); @@ -96,15 +96,12 @@ for t=1:length(navSolution.RX_time) xDot_u=kf_x(4,t); yDot_u=kf_x(5,t); zDot_u=kf_x(6,t); - cdeltat_u(t)=kf_x(7,t)*SPEED_OF_LIGHT_M_S;% - cdeltatDot_u=kf_x(8,t)*SPEED_OF_LIGHT_M_S;% + cdeltat_u(t)=kf_x(7,t);% + cdeltatDot_u=kf_x(8,t);% end % Kalman state prediction (time update) kf_x(:,t) = kf_F * kf_x(:,t); % state prediction kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q; % state error covariance prediction - %from error state variables to variables - % From state variables definition - % TODO: cast to type properly for chan=1:5 %neccesary quantities d(chan)=(sat_posX_m(chan,t)-kf_x(1,t))^2; @@ -133,8 +130,6 @@ for t=1:length(navSolution.RX_time) % Kalman estimation (measurement update) for chan=1:5 % Measurement matrix H assembling - %kf_y(i) = new_data.pr_m(i); % i-Satellite - %kf_y(i+sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; % i-Satellite kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);%-0.000157*SPEED_OF_LIGHT_M_S; kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(chan,t); end @@ -154,15 +149,12 @@ for t=1:length(navSolution.RX_time) kf_S = kf_H * kf_P_x* kf_H' + kf_R; % innovation covariance matrix (S) kf_K = (kf_P_x * kf_H') * inv(kf_S); % Kalman gain kf_xerr(:,t) = kf_K * (kf_yerr(:,t)); % Error state estimation - %kf_x = kf_x(:,t) - kf_xerr(:,t); % updated state estimation (a priori + error) kf_P_x = (eye(length(kf_P_x)) - kf_K * kf_H) * kf_P_x; % update state estimation error covariance matrix corr_kf_state(:,t)=kf_x(:,t)-kf_xerr(:,t); %updated state estimation - % TODO: compare how KF state diverges from RTKLIB solution! - - + + err_clk_b=kf_xerr(7,t) + err_clk_d=kf_xerr(8,t) % States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) - % kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; - % kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; - corr_kf_state(7,t)=corr_kf_state(7,t)/SPEED_OF_LIGHT_M_S; - corr_kf_state(8,t)=corr_kf_state(8,t)/SPEED_OF_LIGHT_M_S;% forced !!!!! + % kf_x(6) =kf_x(6); + % kf_x(7) =kf_x(7); end \ No newline at end of file diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m index 8a07f510c..8bd495974 100644 --- a/src/utils/matlab/vtl/vtl_prototype.m +++ b/src/utils/matlab/vtl/vtl_prototype.m @@ -23,7 +23,7 @@ clearvars % end SPEED_OF_LIGHT_M_S=299792458.0; Lambda_GPS_L1=0.1902937; -point_of_closure=5; +point_of_closure=6000; %% samplingFreq=5000000; channels=6; @@ -103,13 +103,13 @@ legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside') %general_raw_plot vtl_general_plot %% -close all -figure;plot(kf_xerr(7,:),'.');title('clk bias err') -figure;plot(kf_xerr(8,:),'.');title('clk drift err') -figure;plot(kf_x(7,:),'.');title('clk bias state') -figure;plot(kf_x(8,:),'.');title('clk drift state') -figure;plot(corr_kf_state(7,:),'.');title('clk bias corrected state') -figure;plot(corr_kf_state(8,:),'.');title('clk drift corrected state') +% close all +% figure;plot(kf_xerr(7,:),'.');title('clk bias err') +% figure;plot(kf_xerr(8,:),'.');title('clk drift err') +% figure;plot(kf_x(7,:),'.');title('clk bias state') +% figure;plot(kf_x(8,:),'.');title('clk drift state') +% figure;plot(corr_kf_state(7,:),'.');title('clk bias corrected state') +% figure;plot(corr_kf_state(8,:),'.');title('clk drift corrected state') %% % close all % kferr_pos_all=[vtlSolution.kferr.X vtlSolution.kferr.Y vtlSolution.kferr.Z]; From 74a838339305c534d307face37d8115e34a12793 Mon Sep 17 00:00:00 2001 From: miguekf Date: Sat, 10 Dec 2022 22:01:29 +0100 Subject: [PATCH 052/165] MOD: matlab utils --- src/utils/matlab/vtl/kf_prototype.m | 166 ++++++--- .../vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 321 +++++++++--------- src/utils/matlab/vtl/vtl_general_plot.m | 160 ++++----- src/utils/matlab/vtl/vtl_prototype.m | 128 ++++--- 4 files changed, 416 insertions(+), 359 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index 78bed097d..806c2139b 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -29,14 +29,14 @@ kf_yerr = zeros(2*sat_number, 1); kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix %% State error Covariance Matrix Q (PVT) -kf_Q = [ 1 0 0 0 0 0 0 0 - 0 1 0 0 0 0 0 0 - 0 0 1 0 0 0 0 0 - 0 0 0 1 0 0 0 0 - 0 0 0 0 1 0 0 0 - 0 0 0 0 0 1 0 0 - 0 0 0 0 0 0 1e-9 0 - 0 0 0 0 0 0 0 1e-7];%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate. +kf_Q = [ 100 0 0 0 0 0 0 0 + 0 100 0 0 0 0 0 0 + 0 0 100 0 0 0 0 0 + 0 0 0 10 0 0 0 0 + 0 0 0 0 10 0 0 0 + 0 0 0 0 0 10 0 0 + 0 0 0 0 0 0 500 0 + 0 0 0 0 0 0 0 1500];%careful, values for V and T could not be adecuate. %% % % Measurement error Covariance Matrix R assembling @@ -57,17 +57,24 @@ kf_R=[ 3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 30 0 0 0 0 0 0 0 0 0 0 30]; - d = zeros(sat_number, 1); - rho_pri = zeros(sat_number, 1); - rhoDot_pri = zeros(sat_number, 1); - a_x = zeros(sat_number, 1); - a_y = zeros(sat_number, 1); - a_z = zeros(sat_number, 1); - c_pr_m=zeros(sat_number,length(navSolution.RX_time)); +% pre-allocate for speed +d = zeros(sat_number, 1); +rho_pri = zeros(sat_number, 1); +rhoDot_pri = zeros(sat_number, 1); +a_x = zeros(sat_number, 1); +a_y = zeros(sat_number, 1); +a_z = zeros(sat_number, 1); +c_pr_m=zeros(sat_number,length(navSolution.RX_time)); + +pr_m_filt=zeros(sat_number,length(navSolution.RX_time)); +rhoDot_pri_filt=zeros(sat_number,length(navSolution.RX_time)); +sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time)); + %% ################## Kalman Tracking ###################################### % receiver solution from rtklib_solver -for t=1:length(navSolution.RX_time) - +for t=2:length(navSolution.RX_time) + + clear x_u y_u z_u xDot_u yDot_u zDot_u cdeltatDot_u cdeltatDot_u_g cdeltat_u_g if (t Date: Sat, 10 Dec 2022 22:01:46 +0100 Subject: [PATCH 053/165] ADD: python utils --- src/utils/python/analysis.py | 67 ++++++++++++++++++++++ src/utils/python/vtl_analysis.py | 96 ++++++++++++++++++++++++++++++++ 2 files changed, 163 insertions(+) create mode 100644 src/utils/python/analysis.py create mode 100644 src/utils/python/vtl_analysis.py diff --git a/src/utils/python/analysis.py b/src/utils/python/analysis.py new file mode 100644 index 000000000..c1252451b --- /dev/null +++ b/src/utils/python/analysis.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Wed Nov 23 12:45:37 2022 + +@author: miguel +""" + +import numpy as np +import h5py +import matplotlib.pyplot as plt + +#%% +f = h5py.File('PVT_raw.mat','r') +#data = f.get('rateQualityOutTrim/date') +#data = np.array(data) +print(f.keys()) # gives the name of the variables stored +RX_time = f.get('RX_time') + +vel_x = f.get('vel_x') +vel_y = f.get('vel_y') +vel_z = f.get('vel_z') + +pos_x = f.get('pos_x') +pos_y = f.get('pos_y') +pos_z = f.get('pos_z') + +#kf_state1=f.get('vtl_kf_state_1') + +#%% +for keys in f: + keys=f.get(keys) +#%% +#plt.plot(kf_state1[:,0]-kf_state1[0,0],'o',label='kf_state1') + +#plt.show() +#%% +plt.scatter(RX_time,pos_x) +plt.show() +plt.scatter(RX_time,pos_y-pos_y[0]) +plt.show() +plt.scatter(RX_time,pos_z-pos_z[0]) +plt.show() + +#%% +plt.scatter(RX_time,pos_x-pos_x[0]) +#plt.ylim([4863484, 4863591]) +plt.ylim([-20,110]) +plt.ylabel('X [m]') +plt.show() +plt.scatter(RX_time,pos_y-pos_y[0]) +plt.ylabel('Y [m]') +plt.ylim([-85, 5]) +plt.show() +plt.scatter(RX_time,pos_z-pos_z[0]) +plt.ylabel('Z [m]') +plt.ylim([-110,25]) +plt.show() + +#%% +plt.scatter(RX_time,vel_x) +plt.show() +plt.scatter(RX_time,vel_y-vel_y[0]) +plt.show() +plt.scatter(RX_time,vel_z-vel_z[0]) +plt.show() + diff --git a/src/utils/python/vtl_analysis.py b/src/utils/python/vtl_analysis.py new file mode 100644 index 000000000..b3ed141f4 --- /dev/null +++ b/src/utils/python/vtl_analysis.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Sat Nov 26 22:18:25 2022 + +@author: miguel +""" +#%% +import numpy as np +import matplotlib.pyplot as plt +#%% +with open('dump_vtl_file.csv') as f: + rawdatos=np.genfromtxt(f,dtype=str,delimiter=",") +#%% +idx_kf_err = np.where(rawdatos[:,0]=='kf_xerr') [0]# se indexa +idx_kf_state = np.where(rawdatos[:,0]=='kf_state')[0] +idx_rtklib = np.where(rawdatos[:,0]=='rtklib_state')[0] +#%% +kf_err=rawdatos[idx_kf_err,1:].astype(float) +kf_estate=rawdatos[idx_kf_state,1:].astype(float) +rtklib=rawdatos[idx_rtklib,1:].astype(float) + +#%% +plt.close() +plt.plot(kf_err[:,0],'o',label='kf_err') +plt.plot(kf_estate[:,0]-kf_estate[0,0],'o',label='kf_estate') +plt.plot(rtklib[:,0]-rtklib[0,0],'o',label='rtklib') +plt.xlabel('time U.A.') +plt.ylabel('X [m]') +plt.legend() +plt.show() + +plt.plot(kf_err[:,1],'o',label='kf_err') +plt.plot(kf_estate[:,1]-kf_estate[0,1],'o',label='kf_estate') +plt.plot(rtklib[:,1]-rtklib[0,1],'o',label='rtklib') +plt.xlabel('time U.A.') +plt.ylabel('Y [m]') +plt.legend() +plt.show() + +plt.plot(kf_err[:,2],'o',label='kf_err') +plt.plot(kf_estate[:,2]-kf_estate[0,2],'o',label='kf_estate') +plt.plot(rtklib[:,2]-rtklib[0,2],'o',label='rtklib') +plt.xlabel('time U.A.') +plt.ylabel('Z [m]') +plt.legend() +plt.show() + +#%% +plt.close() +plt.plot(kf_estate[:,0]-kf_estate[0,0]+kf_err[:,0],'o',label='kf_corr_estate') +plt.plot(rtklib[:,0]-rtklib[0,0],'o',label='rtklib') +plt.xlabel('time U.A.') +plt.ylabel('X [m]') +plt.title('X corrected') +plt.legend() +plt.show() + +plt.plot(kf_estate[:,1]-kf_estate[0,1]+kf_err[:,1],'o',label='kf_corr_estate') +plt.plot(rtklib[:,1]-rtklib[0,1],'o',label='rtklib') +plt.xlabel('time U.A.') +plt.ylabel('Y [m]') +plt.title('Y corrected') +plt.legend() +plt.show() + +plt.plot(kf_estate[:,2]-kf_estate[0,2]+kf_err[:,2],'o',label='kf_corr_estate') +plt.plot(rtklib[:,2]-rtklib[0,2],'o',label='rtklib') +plt.xlabel('time U.A.') +plt.ylabel('Z [m]') +plt.title('Z corrected') +plt.legend() +plt.show() + +#%% +plt.close() +plt.plot(kf_err[:,3],'o',label='kf_err') +plt.plot(kf_estate[:,3]-kf_estate[0,3],'o',label='kf_estate') +plt.plot(rtklib[:,3]-rtklib[0,3],'o',label='rtklib') +plt.xlabel('time U.A.') +plt.legend() +plt.show() + +plt.plot(kf_err[:,4],'o',label='kf_err') +plt.plot(kf_estate[:,4]-kf_estate[0,4],'o',label='kf_estate') +plt.plot(rtklib[:,4]-rtklib[0,4],'o',label='rtklib') +plt.xlabel('time U.A.') +plt.legend() +plt.show() + +plt.plot(kf_err[:,5],'o',label='kf_err') +plt.plot(kf_estate[:,5]-kf_estate[0,5],'o',label='kf_estate') +plt.plot(rtklib[:,5]-rtklib[0,5],'o',label='rtklib') +plt.xlabel('time U.A.') +plt.legend() +plt.show() \ No newline at end of file From 3659c291824aa7e28926805cfd7d4e5dd1ecb5f0 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 11 Dec 2022 00:32:21 +0000 Subject: [PATCH 054/165] ADD: cout on msg pvt_to_trk --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index e34bf8fa1..2bce8f9b3 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2153,7 +2153,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; - //std::cout << "msg pvt_to_trk sent.\n"; + std::cout << "msg pvt_to_trk sent.\n"; } else { @@ -2177,7 +2177,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; - //std::cout << "msg pvt_to_trk sent.\n"; + std::cout << "msg pvt_to_trk sent.\n"; } } } From c46a14dd5b56d09a9a4d01ae50277167108cb5c3 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sun, 11 Dec 2022 00:33:45 +0000 Subject: [PATCH 055/165] ADD: vtl_kf obsv output --- src/algorithms/PVT/libs/vtl_engine.cc | 77 +++++++++++++-------------- src/algorithms/PVT/libs/vtl_engine.h | 3 ++ 2 files changed, 40 insertions(+), 40 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index b82025950..9d4b0a36d 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -35,7 +35,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // ################## Kalman filter initialization ###################################### // covariances (static) - kf_P_x = arma::eye(8, 8); //TODO: use a real value. + kf_P_x = arma::eye(8, 8)*1.0; //TODO: use a real value. kf_x = arma::zeros(8, 1); kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); double kf_dt=0.1; @@ -58,7 +58,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) counter = counter+1; //uint64_t cout << "counter" << counter< Date: Sun, 11 Dec 2022 01:01:20 +0000 Subject: [PATCH 056/165] MOD: improve Q weights --- src/algorithms/PVT/libs/vtl_engine.cc | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 9d4b0a36d..6bbc9f957 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -58,7 +58,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) counter = counter+1; //uint64_t cout << "counter" << counter< Date: Sun, 11 Dec 2022 02:11:12 +0100 Subject: [PATCH 057/165] MOD: updated matlab vtl_kf --- src/utils/matlab/vtl/kf_prototype.m | 121 +++++++-------- .../vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 5 +- src/utils/matlab/vtl/vtl_general_plot.m | 134 +++++++++++++++-- src/utils/matlab/vtl/vtl_prototype.m | 138 +++--------------- 4 files changed, 205 insertions(+), 193 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index 806c2139b..6367f121c 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -28,36 +28,7 @@ kf_yerr = zeros(2*sat_number, 1); % kf_xerr = zeros(8, 1); kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix -%% State error Covariance Matrix Q (PVT) -kf_Q = [ 100 0 0 0 0 0 0 0 - 0 100 0 0 0 0 0 0 - 0 0 100 0 0 0 0 0 - 0 0 0 10 0 0 0 0 - 0 0 0 0 10 0 0 0 - 0 0 0 0 0 10 0 0 - 0 0 0 0 0 0 500 0 - 0 0 0 0 0 0 0 1500];%careful, values for V and T could not be adecuate. -%% - -% % Measurement error Covariance Matrix R assembling -% for chan=1:5 %neccesary quantities -% % It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) -% kf_R(chan, chan) = 40.0; %TODO: fill with real values. -% kf_R(chan+sat_number, chan+sat_number) = 10.0; -% end - -kf_R=[ 3 0 0 0 0 0 0 0 0 0 - 0 3 0 0 0 0 0 0 0 0 - 0 0 3 0 0 0 0 0 0 0 - 0 0 0 3 0 0 0 0 0 0 - 0 0 0 0 3 0 0 0 0 0 - 0 0 0 0 0 30 0 0 0 0 - 0 0 0 0 0 0 30 0 0 0 - 0 0 0 0 0 0 0 30 0 0 - 0 0 0 0 0 0 0 0 30 0 - 0 0 0 0 0 0 0 0 0 30]; - -% pre-allocate for speed +%% pre-allocate for speed d = zeros(sat_number, 1); rho_pri = zeros(sat_number, 1); rhoDot_pri = zeros(sat_number, 1); @@ -73,7 +44,21 @@ sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time)); %% ################## Kalman Tracking ###################################### % receiver solution from rtklib_solver for t=2:length(navSolution.RX_time) - + + %% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS) + if (t Date: Sun, 11 Dec 2022 02:11:38 +0100 Subject: [PATCH 058/165] ADD: vtl2struct filtered_doppler_field --- src/utils/matlab/vtl/Vtl2struct.m | 10 ++++ src/utils/matlab/vtl/dopp_filtered_plotting.m | 59 +++++++++++++++++++ 2 files changed, 69 insertions(+) create mode 100644 src/utils/matlab/vtl/dopp_filtered_plotting.m diff --git a/src/utils/matlab/vtl/Vtl2struct.m b/src/utils/matlab/vtl/Vtl2struct.m index 91dac2a8a..d693f23a3 100644 --- a/src/utils/matlab/vtl/Vtl2struct.m +++ b/src/utils/matlab/vtl/Vtl2struct.m @@ -48,14 +48,18 @@ vtlSolution.rtklibpvt=[]; [indKF,~]= find(strcmp(dumpvtlfile, 'kf_state')); [indRTKlib,~]= find(strcmp(dumpvtlfile, 'rtklib_state')); [indkf_err,~]= find(strcmp(dumpvtlfile, 'kf_xerr')); +[ind_filt_dop_sat,~]= find(strcmp(dumpvtlfile, 'filt_dopp_sat')); + kfpvt=dumpvtlfile(indKF,:);kfpvt(:,1)=[]; rtklibpvt=dumpvtlfile(indRTKlib,:); rtklibpvt(:,1)=[]; kferr=dumpvtlfile(indkf_err,:); kferr(:,1)=[]; +filt_dop_sat=dumpvtlfile(ind_filt_dop_sat,:); filt_dop_sat(:,1)=[]; kfpvt=cell2mat(kfpvt); rtklibpvt=cell2mat(rtklibpvt); kferr=cell2mat(kferr); +filt_dop_sat=cell2mat(filt_dop_sat); vtlSolution.kfpvt.X=kfpvt(:,1); vtlSolution.kfpvt.Y=kfpvt(:,2); @@ -84,4 +88,10 @@ vtlSolution.kferr.vZ=kferr(:,6); vtlSolution.kferr.biasclock=kferr(:,7); vtlSolution.kferr.rateblock=kferr(:,8); +vtlSolution.filt_dop_sat(1,:)=filt_dop_sat(:,1); +vtlSolution.filt_dop_sat(2,:)=filt_dop_sat(:,2); +vtlSolution.filt_dop_sat(3,:)=filt_dop_sat(:,3); +vtlSolution.filt_dop_sat(4,:)=filt_dop_sat(:,4); +vtlSolution.filt_dop_sat(5,:)=filt_dop_sat(:,5); + end \ No newline at end of file diff --git a/src/utils/matlab/vtl/dopp_filtered_plotting.m b/src/utils/matlab/vtl/dopp_filtered_plotting.m new file mode 100644 index 000000000..31f75195e --- /dev/null +++ b/src/utils/matlab/vtl/dopp_filtered_plotting.m @@ -0,0 +1,59 @@ + + + +time_reference_spirent_obs=129780;%s +time_vtl_dump_file=linspace(38,157,length(vtlSolution.filt_dop_sat)); +% rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05. +%% + Rx_Dopp_28=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','s') + xlim([0,140]); + ylim([-2340,-2220]); + xlabel('') + ylabel('Doppler (Hz)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend('PRN 28 GNSS-SDR','Location','eastoutside') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference') + plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(1,:),'o','DisplayName','filtered VTL') + hold off;grid minor + + Rx_Dopp_4=figure('Name','RX_Carrier_Doppler_hz'); + plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(2,:)','s') + xlim([0,140]); + ylim([2540,2640]); + xlabel('') + ylabel('Doppler (Hz)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend('PRN 4 GNSS-SDR','Location','eastoutside') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,2)','.','DisplayName','reference') + plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(2,:),'o','DisplayName','filtered VTL') + hold off;grid minor + + Rx_Dopp_17=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(3,:)','s') + xlim([0,140]); + ylim([-1800,-1730]); + xlabel('') + ylabel('Doppler (Hz)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend('PRN 17 GNSS-SDR','Location','eastoutside') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,3)','.','DisplayName','reference') + plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(3,:),'o','DisplayName','filtered VTL') + hold off;grid minor + + Rx_Dopp_15=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(4,:)','s') + xlim([0,140]); + ylim([-2680,-2620]); + xlabel('') + ylabel('Doppler (Hz)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend('PRN 15 GNSS-SDR','Location','eastoutside') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,4)','.','DisplayName','reference') + plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(4,:),'o','DisplayName','filtered VTL') + hold off;grid minor From d85b108ee578b24f93f388c94545c1d94970ebe4 Mon Sep 17 00:00:00 2001 From: miguekf Date: Sun, 11 Dec 2022 17:19:36 +0100 Subject: [PATCH 059/165] MOD: update matlab files --- src/utils/matlab/vtl/dopp_filtered_plotting.m | 105 ++++++++------- src/utils/matlab/vtl/vtl_general_plot.m | 126 ++++++++++-------- 2 files changed, 129 insertions(+), 102 deletions(-) diff --git a/src/utils/matlab/vtl/dopp_filtered_plotting.m b/src/utils/matlab/vtl/dopp_filtered_plotting.m index 31f75195e..0119eec75 100644 --- a/src/utils/matlab/vtl/dopp_filtered_plotting.m +++ b/src/utils/matlab/vtl/dopp_filtered_plotting.m @@ -5,55 +5,62 @@ time_reference_spirent_obs=129780;%s time_vtl_dump_file=linspace(38,157,length(vtlSolution.filt_dop_sat)); % rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05. %% - Rx_Dopp_28=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','s') - xlim([0,140]); - ylim([-2340,-2220]); - xlabel('') - ylabel('Doppler (Hz)') - xlabel('time from simulation init (seconds)') - grid on - hold on - legend('PRN 28 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference') - plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(1,:),'o','DisplayName','filtered VTL') - hold off;grid minor +Rx_Dopp_28=figure('Name','RX_Carrier_Doppler_hz'); +subplot(2,2,1); +plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','s') +xlim([0,140]); +ylim([-2340,-2220]); +xlabel('') +ylabel('Doppler (Hz)') +xlabel('time from simulation init (seconds)') +grid on +hold on +legend('PRN 28 GNSS-SDR','Location','eastoutside') +plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference') +plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(1,:),'o','DisplayName','filtered VTL') +hold off;grid minor - Rx_Dopp_4=figure('Name','RX_Carrier_Doppler_hz'); - plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(2,:)','s') - xlim([0,140]); - ylim([2540,2640]); - xlabel('') - ylabel('Doppler (Hz)') - xlabel('time from simulation init (seconds)') - grid on - hold on - legend('PRN 4 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,2)','.','DisplayName','reference') - plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(2,:),'o','DisplayName','filtered VTL') - hold off;grid minor +% Rx_Dopp_4=figure('Name','RX_Carrier_Doppler_hz'); +subplot(2,2,2); +plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(2,:)','s') +xlim([0,140]); +ylim([2540,2640]); +xlabel('') +ylabel('Doppler (Hz)') +xlabel('time from simulation init (seconds)') +grid on +hold on +legend('PRN 4 GNSS-SDR','Location','eastoutside') +plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,2)','.','DisplayName','reference') +plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(2,:),'o','DisplayName','filtered VTL') +hold off;grid minor - Rx_Dopp_17=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(3,:)','s') - xlim([0,140]); - ylim([-1800,-1730]); - xlabel('') - ylabel('Doppler (Hz)') - xlabel('time from simulation init (seconds)') - grid on - hold on - legend('PRN 17 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,3)','.','DisplayName','reference') - plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(3,:),'o','DisplayName','filtered VTL') - hold off;grid minor +% Rx_Dopp_17=figure('Name','RX_Carrier_Doppler_hz'); +subplot(2,2,3); +plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(3,:)','s') +xlim([0,140]); +ylim([-1800,-1730]); +xlabel('') +ylabel('Doppler (Hz)') +xlabel('time from simulation init (seconds)') +grid on +hold on +legend('PRN 17 GNSS-SDR','Location','eastoutside') +plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,3)','.','DisplayName','reference') +plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(3,:),'o','DisplayName','filtered VTL') +hold off;grid minor - Rx_Dopp_15=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(4,:)','s') - xlim([0,140]); - ylim([-2680,-2620]); - xlabel('') - ylabel('Doppler (Hz)') - xlabel('time from simulation init (seconds)') - grid on - hold on - legend('PRN 15 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,4)','.','DisplayName','reference') - plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(4,:),'o','DisplayName','filtered VTL') - hold off;grid minor +% Rx_Dopp_15=figure('Name','RX_Carrier_Doppler_hz'); +subplot(2,2,4); +plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(4,:)','s') +xlim([0,140]); +ylim([-2680,-2620]); +xlabel('') +ylabel('Doppler (Hz)') +xlabel('time from simulation init (seconds)') +grid on +hold on +legend('PRN 15 GNSS-SDR','Location','eastoutside') +plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,4)','.','DisplayName','reference') +plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(4,:),'o','DisplayName','filtered VTL') +hold off;grid minor diff --git a/src/utils/matlab/vtl/vtl_general_plot.m b/src/utils/matlab/vtl/vtl_general_plot.m index ea760bedc..99b14e391 100644 --- a/src/utils/matlab/vtl/vtl_general_plot.m +++ b/src/utils/matlab/vtl/vtl_general_plot.m @@ -37,7 +37,7 @@ end % legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside') % hold off % grid on -% +% % Rx_pseudo_one=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m(1,:)','s') % xlim([0,140]); % xlabel('') @@ -46,7 +46,7 @@ end % grid on % hold on % legend('PRN 28 GNSS-SDR','Location','eastoutside') -% plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,1)','.','DisplayName','reference') +% plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,1)','.','DisplayName','SPIRENT reference') % plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, pr_m_filt(1,:),'o','DisplayName','filtered VTL') % hold off % grid on @@ -58,13 +58,13 @@ plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vX,'.'); hold on;grid on plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(4,:),'.'); plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vX... - ,'.','DisplayName','reference') + ,'.','DisplayName','SPIRENT reference') % plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(4,:),'.'); ylabel('vX (m/s)') xlabel('time U.A.') ylim([-5 5]) title('Subplot 1: vX ') -legend ('raw navSolution','raw kf state','reference','Location','eastoutside') +legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside') subplot(2,2,2); plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.'); @@ -72,12 +72,12 @@ hold on;grid on plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(5,:),'.'); % plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(5,:),'.'); plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vY... - ,'.','DisplayName','reference') + ,'.','DisplayName','SPIRENT reference') ylabel('vY (m/s)') xlabel('time U.A.') ylim([-5 5]) title('Subplot 1: vY ') -legend ('raw navSolution','raw kf state','reference','Location','eastoutside') +legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside') subplot(2,2,3); plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.'); @@ -85,14 +85,14 @@ hold on;grid on plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(6,:),'.'); % plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(6,:),'.'); plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vZ... - ,'.','DisplayName','reference') + ,'.','DisplayName','SPIRENT reference') ylabel('vZ (m/s)') xlabel('time U.A.') ylim([-5 5]) title('Subplot 1: vZ ') -legend ('raw navSolution','raw kf state','reference','Location','eastoutside') +legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside') -sgtitle('velocities') +sgtitle('velocities') %% --- VTL UTM centered POSITION: GNSS SDR plot -------------------------------------- @@ -130,23 +130,25 @@ ylim([-350 50]) title('Subplot 1: Z ') legend ('raw navSolution','raw kf state','kferr','Location','eastoutside') -sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION') +sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION') %% if(load_observables) -% Rx_Dopp_all=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz','s') -% xlim([0,140]); -% xlabel('') -% ylabel('Doppler (Hz)') -% xlabel('time from simulation init (seconds)') -% grid on -% hold on -% plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim','.') -% plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt,'o') -% legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside') -% hold off -% grid on + % Rx_Dopp_all=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz','s') + % xlim([0,140]); + % xlabel('') + % ylabel('Doppler (Hz)') + % xlabel('time from simulation init (seconds)') + % grid on + % hold on + % plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim','.') + % plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt,'o') + % legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside') + % hold off + % grid on - Rx_Dopp_28=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','s') + Rx_Dopp_28=figure('Name','RX_Carrier_Doppler_hz'); + subplot(2,2,1) + plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','s') xlim([0,140]); ylim([-2340,-2220]); xlabel('') @@ -155,12 +157,14 @@ if(load_observables) grid on hold on legend('PRN 28 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','SPIRENT reference') plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL') hold off;grid minor - Rx_Dopp_4=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(2,:)','s') - xlim([0,140]); + % Rx_Dopp_4=figure('Name','RX_Carrier_Doppler_hz'); + subplot(2,2,2) + plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(2,:)','s') + xlim([0,140]); ylim([2540,2640]); xlabel('') ylabel('Doppler (Hz)') @@ -168,12 +172,14 @@ if(load_observables) grid on hold on legend('PRN 4 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,2)','.','DisplayName','reference') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,2)','.','DisplayName','SPIRENT reference') plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(2,:),'o','DisplayName','filtered VTL') hold off;grid minor - Rx_Dopp_17=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(3,:)','s') - xlim([0,140]); + % Rx_Dopp_17=figure('Name','RX_Carrier_Doppler_hz'); + subplot(2,2,3) + plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(3,:)','s') + xlim([0,140]); ylim([-1800,-1730]); xlabel('') ylabel('Doppler (Hz)') @@ -181,12 +187,14 @@ if(load_observables) grid on hold on legend('PRN 17 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,3)','.','DisplayName','reference') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,3)','.','DisplayName','SPIRENT reference') plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(3,:),'o','DisplayName','filtered VTL') hold off;grid minor - Rx_Dopp_15=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(4,:)','s') - xlim([0,140]); + % Rx_Dopp_15=figure('Name','RX_Carrier_Doppler_hz'); + subplot(2,2,4) + plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(4,:)','s') + xlim([0,140]); ylim([-2680,-2620]); xlabel('') ylabel('Doppler (Hz)') @@ -194,54 +202,66 @@ if(load_observables) grid on hold on legend('PRN 15 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,4)','.','DisplayName','reference') + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,4)','.','DisplayName','SPIRENT reference') plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(4,:),'o','DisplayName','filtered VTL') hold off;grid minor end %% STATE PLOT VTL_STATE=figure('Name','VTL STATE'); subplot(2,2,1); -plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(1:3,:)'-corr_kf_state(1:3,3)','.'); +plot(navSolution.RX_time-navSolution.RX_time(1),[navSolution.X-navSolution.X(1);... + navSolution.Y-navSolution.Y(1) ;navSolution.Z-navSolution.Z(1)],... + 'b.','DisplayName','RTKLIB solution'); hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(1:3,:)'-corr_kf_state(1:3,3)',... + 'k.','DisplayName','filt VTL'); plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.X-refSolution.X(1)... refSolution.Y-refSolution.Y(1) refSolution.Z-refSolution.Z(1)],... - '.','DisplayName','reference'); + 'r.','DisplayName','SPIRENT reference'); +legend('Location','eastoutside'); ylim([-200,200]) xlim([0,120]) ylabel('X Y Z (m)') -xlabel('time U.A.') -title('Subplot 1: POSITION ') +xlabel('time [s]') +title('Subplot 1: POSITION [m]') subplot(2,2,2); -plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(4:6,:)','.'); +plot(navSolution.RX_time-navSolution.RX_time(1),[navSolution.vX;... + navSolution.vY; navSolution.vZ],... + 'b.','DisplayName','RTKLIB solution'); hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(4:6,:)',... + 'k.','DisplayName','filt VTL'); plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.vX... refSolution.vY refSolution.vZ],... - '.','DisplayName','reference'); + 'r.','DisplayName','SPIRENT reference'); + xlim([0,120]) ylabel('vX vY vZ (m/s)') -xlabel('time U.A.') -title('Subplot 1: VELOCITIES ') +xlabel('time [s]') +title('Subplot 1: VELOCITIES [m/s]') subplot(2,2,3); -plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(7,:),'.'); -ylim([3019190, 3019700]) +plot(navSolution.RX_time-navSolution.RX_time(1),clk_bias_s*SPEED_OF_LIGHT_M_S,... + 'b.','DisplayName','RTKLIB solution'); hold on;grid on -plot(navSolution.RX_time-navSolution.RX_time(1),clk_bias_s*SPEED_OF_LIGHT_M_S,'.'); +plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(7,:),... + 'k.','DisplayName','filt VTL'); +ylim([3019190, 3019700]) xlim([0,120]) ylabel('clk bias (m)') -xlabel('time U.A.') -title('Subplot 1: clk bias') -legend('vtl','rtklib') +xlabel('time [s]') +title('Subplot 1: clk bias [m]') subplot(2,2,4); -plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(8,:),'.'); +plot(navSolution.RX_time-navSolution.RX_time(1),clk_drift*SPEED_OF_LIGHT_M_S,... + 'b.','DisplayName','RTKLIB solution'); hold on;grid on -plot(navSolution.RX_time-navSolution.RX_time(1),clk_drift*SPEED_OF_LIGHT_M_S,'.'); +plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(8,:),... + 'k.','DisplayName','filt VTL'); xlim([0,120]) ylabel('clk drift (m/s)') -legend('vtl','rtklib') -xlabel('time U.A.') -title('Subplot 1: clk drift ') +xlabel('time [s]') +title('Subplot 1: clk drift [m/s]') -sgtitle('VTL STATE') \ No newline at end of file +sgtitle('VTL STATE') \ No newline at end of file From c38f9bb81e041180b3815c9feef121d21b99a42e Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 12 Dec 2022 12:29:15 +0100 Subject: [PATCH 060/165] Update vtl pvt to trk message structure --- src/algorithms/PVT/libs/rtklib_solver.h | 3 +- src/algorithms/PVT/libs/vtl_engine.cc | 362 +++++++++++++----------- src/algorithms/libs/trackingcmd.h | 3 +- 3 files changed, 195 insertions(+), 173 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 0c8b3dd26..a7d5bab60 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -123,6 +123,8 @@ public: Beidou_Dnav_Iono beidou_dnav_iono; std::map beidou_dnav_almanac_map; + Vtl_Engine vtl_engine; + private: bool save_matfile() const; @@ -139,7 +141,6 @@ private: bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; bool d_use_e6_for_pvt; - Vtl_Engine vtl_engine; }; diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 6bbc9f957..720991ef4 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -35,10 +35,10 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // ################## Kalman filter initialization ###################################### // covariances (static) - kf_P_x = arma::eye(8, 8)*1.0; //TODO: use a real value. - kf_x = arma::zeros(8, 1); - kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); - double kf_dt=0.1; + kf_P_x = arma::eye(8, 8) * 1.0; //TODO: use a real value. + kf_x = arma::zeros(8, 1); + kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); + double kf_dt = 0.1; kf_Q = arma::eye(8, 8); kf_F = arma::eye(8, 8); @@ -47,215 +47,235 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_F(2, 5) = kf_dt; kf_F(6, 7) = kf_dt; - kf_H = arma::zeros(2*new_data.sat_number,8); - kf_y = arma::zeros(2*new_data.sat_number, 1); - kf_yerr = arma::zeros(2*new_data.sat_number, 1); + kf_H = arma::zeros(2 * new_data.sat_number, 8); + kf_y = arma::zeros(2 * new_data.sat_number, 1); + kf_yerr = arma::zeros(2 * new_data.sat_number, 1); kf_xerr = arma::zeros(8, 1); - kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix - + kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix + // ################## Kalman Tracking ###################################### - static uint32_t counter=0; //counter - counter = counter+1; //uint64_t - cout << "counter" << counter< Date: Mon, 12 Dec 2022 15:11:45 +0100 Subject: [PATCH 061/165] VTL Engine now sends messages to tracking loops --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 86 ++++++++++++------- 1 file changed, 53 insertions(+), 33 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 2bce8f9b3..fb60ade51 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2139,48 +2139,68 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (d_close_vtl_loop == true) { std::map::const_iterator gnss_observables_iter; + int idx = 0; for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system { - // test complete loop - if (gnss_observables_iter->second.last_vtl_cmd_sample_counter == 0) + try { - // send new tracking command - const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); - trk_cmd_test->carrier_freq_hz = 12345.4; - trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; - trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; - this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); - d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; - std::cout << "msg pvt_to_trk sent.\n"; + d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx).channel_id = gnss_observables_iter->second.Channel_ID; + //todo: VTL loop CAN NOT run every PVT epoch because it is required to wait for the corrections to be applied to the tracking KF. + //currently the VTL runs every PVT epoch which will create inestabilities. + this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx))); + idx++; } - else + catch (std::exception& ex) { - // std::cout << "CH " << gnss_observables_iter->second.Channel_ID - // << " T_RX: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs) - // << " T_last_vtl_trk: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast(gnss_observables_iter->second.fs) - // << " T_map: " << static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast(gnss_observables_iter->second.fs) - // << " T2: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) - // << " T3: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) << "\n"; - - // To.Do: check if satellite change, check if there is a possibility to not find the last cmd timestamp in the map... - if (gnss_observables_iter->second.last_vtl_cmd_sample_counter >= d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) - { - std::cout << "CH " << gnss_observables_iter->second.Channel_ID << " processed VTL cmd, total loop time is " - << ((static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast(gnss_observables_iter->second.fs)) * 1000.0 - << " [ms]!\n"; - // send new tracking command - const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); - trk_cmd_test->carrier_freq_hz = 12345.4; - trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; - trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; - this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); - d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; - std::cout << "msg pvt_to_trk sent.\n"; - } + std::cout << "VTL Engine problem: " << ex.what() << "\n"; } } + //Dummy messages for evaluation of msg latency + // std::map::const_iterator gnss_observables_iter; + // for (gnss_observables_iter = d_gnss_observables_map.cbegin(); + // gnss_observables_iter != d_gnss_observables_map.cend(); + // ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system + // { + // // test complete loop + // if (gnss_observables_iter->second.last_vtl_cmd_sample_counter == 0) + // { + // // send new tracking command + // const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); + // trk_cmd_test->carrier_freq_hz = 12345.4; + // trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; + // trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; + // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); + // d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; + // std::cout << "msg pvt_to_trk sent.\n"; + // } + // else + // { + // // std::cout << "CH " << gnss_observables_iter->second.Channel_ID + // // << " T_RX: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs) + // // << " T_last_vtl_trk: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast(gnss_observables_iter->second.fs) + // // << " T_map: " << static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast(gnss_observables_iter->second.fs) + // // << " T2: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) + // // << " T3: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) << "\n"; + // + // // To.Do: check if satellite change, check if there is a possibility to not find the last cmd timestamp in the map... + // if (gnss_observables_iter->second.last_vtl_cmd_sample_counter >= d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) + // { + // std::cout << "CH " << gnss_observables_iter->second.Channel_ID << " processed VTL cmd, total loop time is " + // << ((static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast(gnss_observables_iter->second.fs)) * 1000.0 + // << " [ms]!\n"; + // // send new tracking command + // const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); + // trk_cmd_test->carrier_freq_hz = 12345.4; + // trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; + // trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; + // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); + // d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; + // std::cout << "msg pvt_to_trk sent.\n"; + // } + // } + // } } // ***************************** From ea15ca0db01cd2100ddb13eff170af044bb9332a Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 12 Dec 2022 15:15:55 +0100 Subject: [PATCH 062/165] Fix wrong status vector in KF tracking --- .../tracking/gnuradio_blocks/kf_tracking.cc | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 9d3d2377a..58ac6c391 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -636,13 +636,17 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) { gr::thread::scoped_lock lock(d_setlock); //To.Do: apply VTL corrections to the KF states - // code - // d_code_error_kf_chips; - // d_code_freq_kf_chips_s; - // // carrier - // d_carrier_phase_kf_rad; - // d_carrier_doppler_kf_hz; - // d_carrier_doppler_rate_kf_hz_s; + double delta_t_s = static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in; + arma::mat F_tmp; + F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0}, + {0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)}, + {0.0, 0.0, 1.0, delta_t_s}, + {0.0, 0.0, 0.0, 1.0}}; + arma::vec x_tmp; + // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s + x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; + d_x_old_old = F_tmp * x_tmp; + // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; d_vtl_cmd_samplestamp = cmd->sample_counter; From 86c805177a09b14aad53b05ddfe02194c91ff307 Mon Sep 17 00:00:00 2001 From: miguekf Date: Mon, 12 Dec 2022 23:44:36 +0100 Subject: [PATCH 063/165] MOD: matlab improve plot --- src/utils/matlab/vtl/vtl_general_plot.m | 32 ++++++++++++------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/utils/matlab/vtl/vtl_general_plot.m b/src/utils/matlab/vtl/vtl_general_plot.m index 99b14e391..5b70e24bc 100644 --- a/src/utils/matlab/vtl/vtl_general_plot.m +++ b/src/utils/matlab/vtl/vtl_general_plot.m @@ -98,39 +98,39 @@ sgtitle('velocities') VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION'); subplot(2,2,1); -plot(navSolution.X-navSolution.X(1),'.'); +plot(navSolution.X,'.'); hold on;grid on -plot(corr_kf_state(1,3:end)-corr_kf_state(1,3)) -plot(kf_xerr(1,:),'.'); +plot(corr_kf_state(1,3:end)) +% plot(refSolution.X,'.') ylabel('X (m)') xlabel('time U.A.') -ylim([-200 800]) +% ylim([-200 800]) title('Subplot 1: X ') legend ('raw navSolution','raw kf state','kferr','Location','eastoutside') subplot(2,2,2); -plot(navSolution.Y-navSolution.Y(1),'.'); +plot(navSolution.Y,'.'); hold on;grid on -plot(corr_kf_state(2,3:end)-corr_kf_state(2,3)) -plot(kf_xerr(2,:),'.'); +plot(corr_kf_state(2,3:end)) +% plot(refSolution.Y,'.') ylabel('Y (m)') xlabel('time U.A.') -ylim([-200 50]) +% ylim([-200 50]) title('Subplot 1: Y ') legend ('raw navSolution','raw kf state','kferr','Location','eastoutside') subplot(2,2,3); -plot(navSolution.Z-navSolution.Z(1),'.'); +plot(navSolution.Z,'.'); hold on;grid on -plot(corr_kf_state(3,3:end)-corr_kf_state(3,3)) -plot(kf_xerr(3,:),'.'); +plot(corr_kf_state(3,3:end)) +% plot(refSolution.Z,'.') ylabel('Z (m)') xlabel('time U.A.') -ylim([-350 50]) +% ylim([-350 50]) title('Subplot 1: Z ') -legend ('raw navSolution','raw kf state','kferr','Location','eastoutside') +legend ('raw navSolution','raw kf state','SPIRENT ref','Location','eastoutside') -sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION') +sgtitle('VTL UTM COORD') %% if(load_observables) % Rx_Dopp_all=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz','s') @@ -215,8 +215,8 @@ plot(navSolution.RX_time-navSolution.RX_time(1),[navSolution.X-navSolution.X(1); hold on;grid on plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(1:3,:)'-corr_kf_state(1:3,3)',... 'k.','DisplayName','filt VTL'); -plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.X-refSolution.X(1)... - refSolution.Y-refSolution.Y(1) refSolution.Z-refSolution.Z(1)],... +plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.X-refSolution.X(spirent_index_TTFF)... + refSolution.Y-refSolution.Y(spirent_index_TTFF) refSolution.Z-refSolution.Z(spirent_index_TTFF)],... 'r.','DisplayName','SPIRENT reference'); legend('Location','eastoutside'); ylim([-200,200]) From 233343a5b7080c9377b8f755dd4ea11484ac9148 Mon Sep 17 00:00:00 2001 From: miguekf Date: Mon, 12 Dec 2022 23:45:02 +0100 Subject: [PATCH 064/165] ADD: cdeltaDot_u in measurements (both sides) --- src/utils/matlab/vtl/kf_prototype.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index 6367f121c..bb46a5e3d 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -112,7 +112,7 @@ for t=2:length(navSolution.RX_time) rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... - +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); + +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t)+cdeltatDot_u; end @@ -131,8 +131,8 @@ for t=2:length(navSolution.RX_time) % Kalman estimation (measurement update) for chan=1:5 % Measurement matrix H assembling - kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);%-0.000157*SPEED_OF_LIGHT_M_S; - kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t); + kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t); + kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(chan,t); end % DOUBLES DIFFERENCES From ea10a529989029a4176dc251aea8e999ca860ccc Mon Sep 17 00:00:00 2001 From: miguekf Date: Mon, 12 Dec 2022 23:45:46 +0100 Subject: [PATCH 065/165] FIX: matlab bug in plot: spirent synchro index --- src/utils/matlab/vtl/vtl_prototype.m | 1 + 1 file changed, 1 insertion(+) diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m index 19b6ec5ce..64a4c7179 100644 --- a/src/utils/matlab/vtl/vtl_prototype.m +++ b/src/utils/matlab/vtl/vtl_prototype.m @@ -44,6 +44,7 @@ pr_dot_var=30;%m^2/s^2 samplingFreq=5000000; channels=6; TTFF_sec=41.48; +spirent_index_TTFF=416; plot_skyplot=0; plot_reference=1; From b2d5705a89c6d26a81f9962261df32ebe5d9bfcc Mon Sep 17 00:00:00 2001 From: miguekf Date: Tue, 13 Dec 2022 00:07:42 +0100 Subject: [PATCH 066/165] ADD: matlab file output for trk_cmd proto --- src/utils/matlab/vtl/kf_prototype.m | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index bb46a5e3d..733ff584f 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -115,8 +115,6 @@ for t=2:length(navSolution.RX_time) +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t)+cdeltatDot_u; end - - for chan=1:5 % Measurement matrix H assembling % It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0; @@ -202,6 +200,15 @@ for t=2:length(navSolution.RX_time) rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t); %convert rhoDot_pri to doppler shift! sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t))/Lambda_GPS_L1; - + +% carrier_phase_rads = 0; + carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t); +% carrier_freq_rate_hz_s = 0; +% code_phase_chips = 0; end + +% carrier_phase_rads = 0; +% carrier_freq_hz = 0; +% carrier_freq_rate_hz_s = 0; +% code_phase_chips = 0; end \ No newline at end of file From 5a7b2e81843c5b76bc8cf34ede225707696ce07f Mon Sep 17 00:00:00 2001 From: miguekf Date: Tue, 13 Dec 2022 00:07:55 +0100 Subject: [PATCH 067/165] MOD: improve matlab plot --- src/utils/matlab/vtl/vtl_general_plot.m | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/utils/matlab/vtl/vtl_general_plot.m b/src/utils/matlab/vtl/vtl_general_plot.m index 5b70e24bc..b6f964ae1 100644 --- a/src/utils/matlab/vtl/vtl_general_plot.m +++ b/src/utils/matlab/vtl/vtl_general_plot.m @@ -98,10 +98,10 @@ sgtitle('velocities') VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION'); subplot(2,2,1); -plot(navSolution.X,'.'); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X,'.'); hold on;grid on -plot(corr_kf_state(1,3:end)) -% plot(refSolution.X,'.') +plot(navSolution.RX_time(3:end)-navSolution.RX_time(1),corr_kf_state(1,3:end)) +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.X,'.') ylabel('X (m)') xlabel('time U.A.') % ylim([-200 800]) @@ -109,10 +109,10 @@ title('Subplot 1: X ') legend ('raw navSolution','raw kf state','kferr','Location','eastoutside') subplot(2,2,2); -plot(navSolution.Y,'.'); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Y,'.'); hold on;grid on -plot(corr_kf_state(2,3:end)) -% plot(refSolution.Y,'.') +plot(navSolution.RX_time(3:end)-navSolution.RX_time(1),corr_kf_state(2,3:end)) +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Y,'.') ylabel('Y (m)') xlabel('time U.A.') % ylim([-200 50]) @@ -120,10 +120,10 @@ title('Subplot 1: Y ') legend ('raw navSolution','raw kf state','kferr','Location','eastoutside') subplot(2,2,3); -plot(navSolution.Z,'.'); +plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Z,'.'); hold on;grid on -plot(corr_kf_state(3,3:end)) -% plot(refSolution.Z,'.') +plot(navSolution.RX_time(3:end)-navSolution.RX_time(1),corr_kf_state(3,3:end)) +plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Z,'.') ylabel('Z (m)') xlabel('time U.A.') % ylim([-350 50]) From 3f87e598a9f1a0de588e444ccc076eb4eaf95a15 Mon Sep 17 00:00:00 2001 From: miguel Date: Thu, 15 Dec 2022 12:21:01 +0100 Subject: [PATCH 068/165] ADD: trk_cmd carrier freq Hz --- src/algorithms/PVT/libs/vtl_engine.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 720991ef4..fa7d53ea9 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -244,8 +244,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // Notice: keep the same satellite order as in the Vtl_Data matrices // sample code TrackingCmd trk_cmd; - trk_cmd.carrier_phase_rads = 0; - trk_cmd.carrier_freq_hz = 0; + trk_cmd.carrier_phase_rads = 0; // difficult of calculation + trk_cmd.carrier_freq_hz = doppler_hz_filt(i) + kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_rate_hz_s = 0; trk_cmd.code_phase_chips = 0; trk_cmd.enable_carrier_nco_cmd = true; From 6390f4c2162077664758709cc9614f2c3993aabd Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 15 Dec 2022 18:09:22 +0100 Subject: [PATCH 069/165] ADD: matlab tracking file for plots --- src/utils/matlab/vtl/vtl_tracking.m | 64 +++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 src/utils/matlab/vtl/vtl_tracking.m diff --git a/src/utils/matlab/vtl/vtl_tracking.m b/src/utils/matlab/vtl/vtl_tracking.m new file mode 100644 index 000000000..fca25566f --- /dev/null +++ b/src/utils/matlab/vtl/vtl_tracking.m @@ -0,0 +1,64 @@ +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% +%% +clc +close all +clearvars + +% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file') +% addpath('./libs') +% end +% +% if ~exist('cat2geo.m', 'file') +% addpath('./libs/geoFunctions') +% end +SPEED_OF_LIGHT_M_S=299792458.0; +Lambda_GPS_L1=0.1902937; +%% + +for chan=0:4 +load(['tracking\tracking_raw' num2str(chan) '.mat']); +tracking_channel(chan+1).PRN=PRN; +tracking_channel(chan+1).CN0_SNV_dB_Hz=CN0_SNV_dB_Hz; +tracking_channel(chan+1).carrier_doppler_hz=carrier_doppler_hz; +tracking_channel(chan+1).carrier_doppler_rate_hz=carrier_doppler_rate_hz; +tracking_channel(chan+1).code_error_chips=code_error_chips; +tracking_channel(chan+1).code_freq_chips=code_freq_chips; +tracking_channel(chan+1).code_freq_rate_chips=code_freq_rate_chips; +tracking_channel(chan+1).acc_carrier_phase_rad=acc_carrier_phase_rad; + +clearvars -except tracking_channel chan +end +%% +figure +for chan=1:5 +subplot(2,3,chan); +plot(tracking_channel(chan).carrier_doppler_hz); +grid minor +end +sgtitle('carrier doppler hz channel') +%% +figure +for chan=1:5 +subplot(2,3,chan); +plot(tracking_channel(chan).CN0_SNV_dB_Hz); +grid minor +end +sgtitle('CN0 SNV dB Hz channel') + +%% +figure +for chan=1:5 +subplot(2,3,chan); +plot(tracking_channel(chan).acc_carrier_phase_rad); +grid minor +end +sgtitle('acc carrier phase rad') \ No newline at end of file From c53bd9387cb4ad8f3800e9932113ba002eb0f215 Mon Sep 17 00:00:00 2001 From: miguekf Date: Fri, 16 Dec 2022 17:35:57 +0100 Subject: [PATCH 070/165] ADD: spirent_usrp_vtl_no_kf_tracking.conf --- conf/vtl/spirent_usrp_vtl_no_kf_tracking.conf | 107 ++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 conf/vtl/spirent_usrp_vtl_no_kf_tracking.conf diff --git a/conf/vtl/spirent_usrp_vtl_no_kf_tracking.conf b/conf/vtl/spirent_usrp_vtl_no_kf_tracking.conf new file mode 100644 index 000000000..8f6fe7e79 --- /dev/null +++ b/conf/vtl/spirent_usrp_vtl_no_kf_tracking.conf @@ -0,0 +1,107 @@ +; This is a GNSS-SDR configuration file +; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/ +; SPDX-License-Identifier: GPL-3.0-or-later +; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors) + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +GNSS-SDR.internal_fs_sps=5000000 +GNSS-SDR.telecommand_enabled=false +GNSS-SDR.telecommand_tcp_port=3333 + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=File_Signal_Source +SignalSource.filename=/media/sf_shareFolder/Spirent/SPF-LD-05-12H03_CH0.iq +SignalSource.item_type=ishort +SignalSource.sampling_frequency=5000000 +SignalSource.samples=0 + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner +DataTypeAdapter.implementation=Ishort_To_Complex +InputFilter.implementation=Pass_Through +InputFilter.item_type=gr_complex +Resampler.implementation=Direct_Resampler +Resampler.sample_freq_in=5000000 +Resampler.sample_freq_out=5000000 +Resampler.item_type=gr_complex + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1C.count=6 +Channels.in_acquisition=6 +Channel.signal=1C + +Channel0.satellite=28 +Channel1.satellite=4 +Channel2.satellite=17 +Channel3.satellite=15 +Channel4.satellite=27 +Channel5.satellite=9 + +;######### ACQUISITION GLOBAL CONFIG ############ +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.item_type=gr_complex +Acquisition_1C.pfa=0.01 +Acquisition_1C.doppler_max=5000 +Acquisition_1C.doppler_step=250 +Acquisition_1C.blocking=true + +;######### TRACKING GLOBAL CONFIG ############ +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.pll_bw_hz=40.0; +Tracking_1C.dll_bw_hz=4.0; + +Tracking_1C.dump=true +Tracking_1C.dump_filename=./tracking/tracking_raw.dat +Tracking_1C.dump_mat=true + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder + +;######### OBSERVABLES CONFIG ############ +Observables.implementation=Hybrid_Observables +Observables.dump=true +Observables.dump_mat=true +Observables.dump_filename=./observables/observables_raw.dat + + +;######### PVT CONFIG ############ +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.enable_rx_clock_correction=false +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.output_rate_ms=100; +PVT.rinexobs_rate_ms=100; +PVT.display_rate_ms=100; +PVT.elevation_mask=15; +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 + +PVT.dump=true +PVT.dump_mat=true +PVT.dump_filename=./PVT_raw + +PVT.gpx_output_enabled=false +PVT.geojson_output_enabled=false +PVT.kml_output_enabled=false +PVT.xml_output_enabled=false +PVT.rinex_output_enabled=false +PVT.nmea_output_file_enabled=false + +PVT.enable_vtl=true +PVT.close_vtl_loop=true + +;######### PVT MONITOR CONFIG ############ +PVT.enable_monitor=false +PVT.monitor_client_addresses=127.0.0.1 +PVT.monitor_udp_port=1111 + +;######### MONITOR CONFIG ############ +Monitor.enable_monitor=false +Monitor.decimation_factor=1 +Monitor.client_addresses=127.0.0.1 +Monitor.udp_port=1112 From 4ab537ab31f8d924928ffade16025c7ca8cec359 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 16 Dec 2022 16:54:25 +0000 Subject: [PATCH 071/165] MOD: Q and R values --- src/algorithms/PVT/libs/vtl_engine.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index fa7d53ea9..a0ac10e00 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -100,8 +100,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) for (int32_t i = 0; i < new_data.sat_number; i++) { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 10.0; //TODO: fill with real values. - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 30.0; + kf_R(i, i) = 20.0; //TODO: fill with real values. + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 2.0; } // Kalman state prediction (time update) From e2af50f569e018369085daa236f0b94b8fda4fd8 Mon Sep 17 00:00:00 2001 From: miguekf Date: Fri, 16 Dec 2022 18:03:31 +0100 Subject: [PATCH 072/165] ADD: kf_tracking VTL conf file --- conf/vtl/spirent_usrp_vtl.conf | 121 +++++++++++++++++++++++++++++++++ 1 file changed, 121 insertions(+) create mode 100644 conf/vtl/spirent_usrp_vtl.conf diff --git a/conf/vtl/spirent_usrp_vtl.conf b/conf/vtl/spirent_usrp_vtl.conf new file mode 100644 index 000000000..2243669a5 --- /dev/null +++ b/conf/vtl/spirent_usrp_vtl.conf @@ -0,0 +1,121 @@ +; This is a GNSS-SDR configuration file +; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/ +; SPDX-License-Identifier: GPL-3.0-or-later +; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors) + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +GNSS-SDR.internal_fs_sps=5000000 +GNSS-SDR.telecommand_enabled=false +GNSS-SDR.telecommand_tcp_port=3333 + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=File_Signal_Source +SignalSource.filename=/media/sf_shareFolder/Spirent/SPF-LD-05-12H03_CH0.iq +SignalSource.item_type=ishort +SignalSource.sampling_frequency=5000000 +SignalSource.samples=0 + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner +DataTypeAdapter.implementation=Ishort_To_Complex +InputFilter.implementation=Pass_Through +InputFilter.item_type=gr_complex +Resampler.implementation=Direct_Resampler +Resampler.sample_freq_in=5000000 +Resampler.sample_freq_out=5000000 +Resampler.item_type=gr_complex + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1C.count=6 +Channels.in_acquisition=6 +Channel.signal=1C + +Channel0.satellite=28 +Channel1.satellite=4 +Channel2.satellite=17 +Channel3.satellite=15 +Channel4.satellite=27 +Channel5.satellite=9 + +;######### ACQUISITION GLOBAL CONFIG ############ +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.item_type=gr_complex +Acquisition_1C.pfa=0.01 +Acquisition_1C.doppler_max=5000 +Acquisition_1C.doppler_step=250 +Acquisition_1C.blocking=true + +;######### TRACKING GLOBAL CONFIG ############ +Tracking_1C.implementation=GPS_L1_CA_KF_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.extend_correlation_symbols=1; +Tracking_1C.early_late_space_chips=0.5; +Tracking_1C.early_late_space_narrow_chips=0.15 + +;Tracking_1C.code_disc_sd_chips=0.2; // Initial R +;Tracking_1C.carrier_disc_sd_rads=0.3; // Initial R + +;Tracking_1C.init_code_phase_sd_chips=0.5; // Initial P_0_0 +;Tracking_1C.init_carrier_phase_sd_rad=0.7; +;Tracking_1C.init_carrier_freq_sd_hz=5; +;Tracking_1C.init_carrier_freq_rate_sd_hz_s=1; + +;Tracking_1C.code_phase_sd_chips=0.15; // Initial Q +;Tracking_1C.carrier_phase_sd_rad=0.25; +;Tracking_1C.carrier_freq_sd_hz=0.6; +;Tracking_1C.carrier_freq_rate_sd_hz_s=0.01; + +Tracking_1C.dump=true +Tracking_1C.dump_filename=./tracking/tracking_raw.dat +Tracking_1C.dump_mat=true + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder + +;######### OBSERVABLES CONFIG ############ +Observables.implementation=Hybrid_Observables +Observables.dump=true +Observables.dump_mat=true +Observables.dump_filename=./observables/observables_raw.dat + + +;######### PVT CONFIG ############ +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.enable_rx_clock_correction=false +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.output_rate_ms=100; +PVT.rinexobs_rate_ms=100; +PVT.display_rate_ms=100; +PVT.elevation_mask=15; +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 + +PVT.dump=true +PVT.dump_mat=true +PVT.dump_filename=./PVT_raw + +PVT.gpx_output_enabled=false +PVT.geojson_output_enabled=false +PVT.kml_output_enabled=false +PVT.xml_output_enabled=false +PVT.rinex_output_enabled=false +PVT.nmea_output_file_enabled=false + +PVT.enable_vtl=true +PVT.close_vtl_loop=true + +;######### PVT MONITOR CONFIG ############ +PVT.enable_monitor=false +PVT.monitor_client_addresses=127.0.0.1 +PVT.monitor_udp_port=1111 + +;######### MONITOR CONFIG ############ +Monitor.enable_monitor=false +Monitor.decimation_factor=1 +Monitor.client_addresses=127.0.0.1 +Monitor.udp_port=1112 From c0c1c20eb6bafa26e2b41229f1f97465e33183da Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 16 Dec 2022 19:05:10 +0100 Subject: [PATCH 073/165] Fix hash code does not match bug in pvt to trk vtl msg. Clode vtl still does not work --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 9 +++++++-- .../tracking/gnuradio_blocks/kf_tracking.cc | 11 +++++++++-- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index fb60ade51..69f08ee21 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2136,7 +2136,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false, d_enable_vtl, d_close_vtl_loop)) { // ****** experimental VTL tests - if (d_close_vtl_loop == true) + if (d_close_vtl_loop == true and d_enable_vtl == true) { std::map::const_iterator gnss_observables_iter; int idx = 0; @@ -2149,7 +2149,8 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx).channel_id = gnss_observables_iter->second.Channel_ID; //todo: VTL loop CAN NOT run every PVT epoch because it is required to wait for the corrections to be applied to the tracking KF. //currently the VTL runs every PVT epoch which will create inestabilities. - this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx))); + const std::shared_ptr trk_cmd_test = std::make_shared(d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx)); + this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); idx++; } catch (std::exception& ex) @@ -2202,6 +2203,10 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // } // } } + else + { + std::cout << "Loop open!\n"; + } // ***************************** d_pvt_errors_counter = 0; // Reset consecutive PVT error counter diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 58ac6c391..fa9c9d058 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -638,6 +638,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) //To.Do: apply VTL corrections to the KF states double delta_t_s = static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in; arma::mat F_tmp; + //ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0}, {0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)}, {0.0, 0.0, 1.0, delta_t_s}, @@ -645,13 +646,19 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) arma::vec x_tmp; // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; - d_x_old_old = F_tmp * x_tmp; + // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) + arma::vec tmp_x = F_tmp * x_tmp; + double old_doppler = d_x_old_old(2); + d_x_old_old(2) = tmp_x(2); //replace only the Carrier Frequency state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; d_vtl_cmd_samplestamp = cmd->sample_counter; std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " - << static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n"; + << static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in + << " SampleCounter origin: " << cmd->sample_counter + << " Doppler new state: " << tmp_x(2) << " vs. trk state: " << old_doppler << " [Hz]" + << " [s]\n"; } } else From 54c3d6f964cb099c540277e6558a0a19e1845ed0 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 17 Dec 2022 12:46:22 +0000 Subject: [PATCH 074/165] MOD: debug in trk_cmd update --- src/algorithms/PVT/libs/vtl_engine.cc | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index a0ac10e00..d9055f7f6 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -232,20 +232,20 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_yerr = kf_H * kf_xerr; // Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): - for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector + for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector { - rho_pri_filt(i) = new_data.pr_m(i) + kf_yerr(i); // now filtered - rhoDot_pri_filt(i) = (new_data.doppler_hz(i) * Lambda_GPS_L1 + kf_x(7)) - kf_yerr(i + new_data.sat_number); // now filtered + rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered + rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1 + kf_x(7)) - kf_yerr(channel + new_data.sat_number); // now filtered // TO DO: convert rhoDot_pri to doppler shift! // Doppler shift defined as pseudorange rate measurement divided by the negative of carrier wavelength. - doppler_hz_filt(i) = (rhoDot_pri_filt(i) - kf_x(7)) / Lambda_GPS_L1; + doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1; //TODO: Fill the tracking commands outputs // Notice: keep the same satellite order as in the Vtl_Data matrices // sample code TrackingCmd trk_cmd; trk_cmd.carrier_phase_rads = 0; // difficult of calculation - trk_cmd.carrier_freq_hz = doppler_hz_filt(i) + kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction + trk_cmd.carrier_freq_hz = doppler_hz_filt(channel) ;//+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_rate_hz_s = 0; trk_cmd.code_phase_chips = 0; trk_cmd.enable_carrier_nco_cmd = true; @@ -253,6 +253,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) trk_cmd.sample_counter = new_data.sample_counter; trk_cmd.channel_id = 0; trk_cmd_outs.push_back(trk_cmd); + if(channel==0) + { + std::cout << "CH " << channel + << " Doppler vtl commanded: " << doppler_hz_filt(channel) << " [Hz]" + << " \n"; + } } From e3a62735e36dc04c76d7d41d2dab9775a148c0b2 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 17 Dec 2022 12:46:40 +0000 Subject: [PATCH 075/165] MOD: debug in kf_tracking --- .../tracking/gnuradio_blocks/kf_tracking.cc | 32 ++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index fa9c9d058..5e0f4d2e8 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -57,6 +57,8 @@ #include #include #include +#include "iostream" +#include #if HAS_GENERIC_LAMBDA #else @@ -647,18 +649,40 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) - arma::vec tmp_x = F_tmp * x_tmp; + //arma::vec tmp_x = F_tmp * x_tmp; double old_doppler = d_x_old_old(2); - d_x_old_old(2) = tmp_x(2); //replace only the Carrier Frequency state + d_x_old_old(2) = x_tmp(2); //replace only the Carrier Frequency state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; d_vtl_cmd_samplestamp = cmd->sample_counter; - std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " + // std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " + // << static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in + // << " SampleCounter origin: " << cmd->sample_counter + // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" + // << " [s]\n"; + if(this->d_channel ==0) + { + std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " << static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in << " SampleCounter origin: " << cmd->sample_counter - << " Doppler new state: " << tmp_x(2) << " vs. trk state: " << old_doppler << " [Hz]" + << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" << " [s]\n"; + } + + std::fstream dump_tracking_file; + dump_tracking_file.open("dump_trk_file.csv", std::ios::out | std::ios::app); + dump_tracking_file.precision(15); + if (!dump_tracking_file) + { + std::cout << "dump_tracking_file not created!"; + } + else + { + dump_tracking_file << "doppler_corr" + << ","<< this->d_channel << "," << x_tmp(2) << "," << old_doppler << "\n"; + dump_tracking_file.close(); + } } } else From 7d7745055a2d1ea70beff21616ac9335f82bbf6f Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 17 Dec 2022 12:58:48 +0000 Subject: [PATCH 076/165] MOD: not updating tracking state for debugging --- src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 5e0f4d2e8..627ab5089 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -651,7 +651,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) //arma::vec tmp_x = F_tmp * x_tmp; double old_doppler = d_x_old_old(2); - d_x_old_old(2) = x_tmp(2); //replace only the Carrier Frequency state + //d_x_old_old(2) = x_tmp(2); //replace only the Carrier Frequency state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; From 4d5a92b5ad52910909bc15baf2d25ac6562da1fb Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 17 Dec 2022 13:06:30 +0000 Subject: [PATCH 077/165] MOD: trk_cmd not corrected by clk_drift --- src/algorithms/PVT/libs/vtl_engine.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index d9055f7f6..458ae1684 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -245,7 +245,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // sample code TrackingCmd trk_cmd; trk_cmd.carrier_phase_rads = 0; // difficult of calculation - trk_cmd.carrier_freq_hz = doppler_hz_filt(channel) ;//+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction + trk_cmd.carrier_freq_hz = doppler_hz_filt(channel) + kf_x(7)/ Lambda_GPS_L1;//+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_rate_hz_s = 0; trk_cmd.code_phase_chips = 0; trk_cmd.enable_carrier_nco_cmd = true; From f466687b23fc1bedf42aa05b3247c98a49e552d8 Mon Sep 17 00:00:00 2001 From: miguekf Date: Sat, 17 Dec 2022 14:24:55 +0100 Subject: [PATCH 078/165] MOD: matlab trk_cmd files updated --- .../vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 23 +++++- src/utils/matlab/vtl/trk2struct.m | 56 +++++++++++++ src/utils/matlab/vtl/vtl_prototype.m | 11 +-- src/utils/matlab/vtl/vtl_tracking.m | 82 +++++++++++-------- 4 files changed, 133 insertions(+), 39 deletions(-) create mode 100644 src/utils/matlab/vtl/trk2struct.m diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m index 42626a591..cf743c75a 100644 --- a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m +++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m @@ -23,6 +23,7 @@ clearvars % end SPEED_OF_LIGHT_M_S=299792458.0; Lambda_GPS_L1=0.1902937; +GPS_L1_freq_hz=1575.42e6; %% samplingFreq=5000000; channels=6; @@ -34,12 +35,13 @@ plot_reference=1; load_observables=1; advance_vtl_data_available=1; load_vtl=1; +load_tfk_cmd=1; navSolution = GnssSDR2struct('PVT_raw.mat'); refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv'); %% if(load_observables) - load observables\observable_raw.mat + load observables\observables_raw.mat refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05. end @@ -53,6 +55,25 @@ if(load_vtl) vtlSolution = Vtl2struct('dump_vtl_file.csv'); end +if(load_tfk_cmd) + trkSolution=trk2struct('dump_trk_file.csv'); + + %% split by solution type + figure;sgtitle('real doppler') + for chan=0:4 + eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.real==chan);']) + eval(['Dopp_real_CH' num2str(chan) '=trkSolution.dopp.real(indCH' num2str(chan) ',2);']) + eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_real_CH' num2str(chan) ')']) + end + figure;sgtitle('cmd doppler') + for chan=0:4 + eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.cmd==chan);']) + eval(['Dopp_cmd_CH' num2str(chan) '=trkSolution.dopp.cmd(indCH' num2str(chan) ',2);']) + eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_cmd_CH' num2str(chan) ')']) + end + +end + %% calculate LOS Rx-sat if advance_vtl_data_available=1 if(advance_vtl_data_available) diff --git a/src/utils/matlab/vtl/trk2struct.m b/src/utils/matlab/vtl/trk2struct.m new file mode 100644 index 000000000..b0532148f --- /dev/null +++ b/src/utils/matlab/vtl/trk2struct.m @@ -0,0 +1,56 @@ +%% Import data from text file +% Script for importing data from the following text file: +% +% filename: D:\virtualBOX_VM\ubuntu20\ubuntu20\shareFolder\myWork\results\spirent_usrp_airing\dump_vtl_file.csv +% +% % +% ------------------------------------------------------------------------- +% USE EXAMPLE: vtlSolution = Vtl2struct('dump_vtl_file.csv') +% ------------------------------------------------------------------------- + +% Auto-generated by MATLAB on 24-Nov-2022 17:34:27 +function [trkSolution] = trk2struct(path_to_trk_csv) +%% Set up the Import Options and import the data +opts = delimitedTextImportOptions("NumVariables", 4); + +% Specify range and delimiter +opts.DataLines = [1, Inf]; +opts.Delimiter = ","; + +% Specify column names and types +opts.VariableNames = ["doppler_corr", "VarName2", "VarName3", "VarName4"]; +opts.VariableTypes = ["char", "double", "double", "double"]; + +% Specify file level properties +opts.ExtraColumnsRule = "ignore"; +opts.EmptyLineRule = "read"; + +% Specify variable properties +opts = setvaropts(opts, "doppler_corr", "WhitespaceRule", "preserve"); +opts = setvaropts(opts, "doppler_corr", "EmptyFieldRule", "auto"); + +% Import the data +dumptrackingfile = readtable(path_to_trk_csv, opts); + +%% Convert to output type +dumptrackingfile = table2cell(dumptrackingfile); +numIdx = cellfun(@(x) ~isnan(str2double(x)), dumptrackingfile); +dumptrackingfile(numIdx) = cellfun(@(x) {str2double(x)}, dumptrackingfile(numIdx)); + +%% Clear temporary variables +clear numIdx opts + +trkSolution.dopp=[]; + +%% split by solution type +[indDopp,~]= find(strcmp(dumptrackingfile, 'doppler_corr')); + +trk_dopp=dumptrackingfile(indDopp,:);trk_dopp(:,1)=[]; + +trk_dopp=cell2mat(trk_dopp); + +for i=1:length(trk_dopp) + trkSolution.dopp.real=[trk_dopp(:,1) trk_dopp(:,3)]; + trkSolution.dopp.cmd=[trk_dopp(:,1) trk_dopp(:,2)]; +end +end \ No newline at end of file diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m index 64a4c7179..7977138b4 100644 --- a/src/utils/matlab/vtl/vtl_prototype.m +++ b/src/utils/matlab/vtl/vtl_prototype.m @@ -23,15 +23,16 @@ clearvars % end SPEED_OF_LIGHT_M_S=299792458.0; Lambda_GPS_L1=0.1902937; -point_of_closure=6000; +GPS_L1_freq_hz=1575.42e6; +point_of_closure=3400; %% ==================== VARIANCES ============================= pos_var=100;%m^2 vel_var=10;%m^2/s^2 clk_bias_var=40;%m^2 clk_drift_var=1500;%m^2/s^2 -pr_var=10;%m^2 -pr_dot_var=30;%m^2/s^2 +pr_var=20;%m^2 +pr_dot_var=2;%m^2/s^2 % CARLES PAPER LTE GNSS VTL % pos_var=2;%m^2 @@ -55,14 +56,14 @@ load_observables=1; navSolution = GnssSDR2struct('PVT_raw.mat'); refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv'); % -load observables\observable_raw.mat +load observables\observables_raw.mat % refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05. load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'... ,'sat_velZ','sat_prg_m','clk_bias_s','clk_drift','sat_dopp_hz','user_clk_offset') if(load_observables) - load observables\observable_raw.mat + load observables\observables_raw.mat refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); end %% diff --git a/src/utils/matlab/vtl/vtl_tracking.m b/src/utils/matlab/vtl/vtl_tracking.m index fca25566f..c78a70db6 100644 --- a/src/utils/matlab/vtl/vtl_tracking.m +++ b/src/utils/matlab/vtl/vtl_tracking.m @@ -23,42 +23,58 @@ clearvars SPEED_OF_LIGHT_M_S=299792458.0; Lambda_GPS_L1=0.1902937; %% +trkSolution=trk2struct('dump_trk_file.csv'); +%% split by solution type +figure;sgtitle('real doppler') for chan=0:4 -load(['tracking\tracking_raw' num2str(chan) '.mat']); -tracking_channel(chan+1).PRN=PRN; -tracking_channel(chan+1).CN0_SNV_dB_Hz=CN0_SNV_dB_Hz; -tracking_channel(chan+1).carrier_doppler_hz=carrier_doppler_hz; -tracking_channel(chan+1).carrier_doppler_rate_hz=carrier_doppler_rate_hz; -tracking_channel(chan+1).code_error_chips=code_error_chips; -tracking_channel(chan+1).code_freq_chips=code_freq_chips; -tracking_channel(chan+1).code_freq_rate_chips=code_freq_rate_chips; -tracking_channel(chan+1).acc_carrier_phase_rad=acc_carrier_phase_rad; - -clearvars -except tracking_channel chan +eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.real==chan);']) +eval(['Dopp_real_CH' num2str(chan) '=trkSolution.dopp.real(indCH' num2str(chan) ',2);']) +eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_real_CH' num2str(chan) ')']) end -%% -figure -for chan=1:5 -subplot(2,3,chan); -plot(tracking_channel(chan).carrier_doppler_hz); -grid minor +figure;sgtitle('cmd doppler') +for chan=0:4 +eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.cmd==chan);']) +eval(['Dopp_cmd_CH' num2str(chan) '=trkSolution.dopp.cmd(indCH' num2str(chan) ',2);']) +eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_cmd_CH' num2str(chan) ')']) end -sgtitle('carrier doppler hz channel') -%% -figure -for chan=1:5 -subplot(2,3,chan); -plot(tracking_channel(chan).CN0_SNV_dB_Hz); -grid minor -end -sgtitle('CN0 SNV dB Hz channel') %% -figure -for chan=1:5 -subplot(2,3,chan); -plot(tracking_channel(chan).acc_carrier_phase_rad); -grid minor -end -sgtitle('acc carrier phase rad') \ No newline at end of file +% for chan=0:4 +% load(['tracking\tracking_raw' num2str(chan) '.mat']); +% tracking_channel(chan+1).PRN=PRN; +% tracking_channel(chan+1).CN0_SNV_dB_Hz=CN0_SNV_dB_Hz; +% tracking_channel(chan+1).carrier_doppler_hz=carrier_doppler_hz; +% tracking_channel(chan+1).carrier_doppler_rate_hz=carrier_doppler_rate_hz; +% tracking_channel(chan+1).code_error_chips=code_error_chips; +% tracking_channel(chan+1).code_freq_chips=code_freq_chips; +% tracking_channel(chan+1).code_freq_rate_chips=code_freq_rate_chips; +% tracking_channel(chan+1).acc_carrier_phase_rad=acc_carrier_phase_rad; +% +% clearvars -except tracking_channel chan +% end +% %% +% figure +% for chan=1:5 +% subplot(2,3,chan); +% plot(tracking_channel(chan).carrier_doppler_hz); +% grid minor +% end +% sgtitle('carrier doppler hz channel') +% %% +% figure +% for chan=1:5 +% subplot(2,3,chan); +% plot(tracking_channel(chan).CN0_SNV_dB_Hz); +% grid minor +% end +% sgtitle('CN0 SNV dB Hz channel') +% +% %% +% figure +% for chan=1:5 +% subplot(2,3,chan); +% plot(tracking_channel(chan).acc_carrier_phase_rad); +% grid minor +% end +% sgtitle('acc carrier phase rad') \ No newline at end of file From 3646be2a72c49d31db086b8aaa582e596565962f Mon Sep 17 00:00:00 2001 From: miguekf Date: Sat, 17 Dec 2022 16:35:49 +0100 Subject: [PATCH 079/165] MOD:path to file in config file --- conf/vtl/spirent_usrp_vtl.conf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/vtl/spirent_usrp_vtl.conf b/conf/vtl/spirent_usrp_vtl.conf index 2243669a5..bda484561 100644 --- a/conf/vtl/spirent_usrp_vtl.conf +++ b/conf/vtl/spirent_usrp_vtl.conf @@ -12,7 +12,7 @@ GNSS-SDR.telecommand_tcp_port=3333 ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=File_Signal_Source -SignalSource.filename=/media/sf_shareFolder/Spirent/SPF-LD-05-12H03_CH0.iq +SignalSource.filename=/media/sf_sharedFolder/myWork/data/AIRING/SPF-5-LD-12H03_CH0.iq SignalSource.item_type=ishort SignalSource.sampling_frequency=5000000 SignalSource.samples=0 From 2a182bfbad837c357b710e1af063e2bb6ed60e32 Mon Sep 17 00:00:00 2001 From: miguekf Date: Sat, 17 Dec 2022 16:42:00 +0100 Subject: [PATCH 080/165] FIX: typo in config file path --- conf/vtl/spirent_usrp_vtl.conf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/vtl/spirent_usrp_vtl.conf b/conf/vtl/spirent_usrp_vtl.conf index bda484561..2d790edae 100644 --- a/conf/vtl/spirent_usrp_vtl.conf +++ b/conf/vtl/spirent_usrp_vtl.conf @@ -12,7 +12,7 @@ GNSS-SDR.telecommand_tcp_port=3333 ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=File_Signal_Source -SignalSource.filename=/media/sf_sharedFolder/myWork/data/AIRING/SPF-5-LD-12H03_CH0.iq +SignalSource.filename=/media/sf_shareFolder/myWork/data/AIRING/SPF-LD-05-12H03_CH0.iq SignalSource.item_type=ishort SignalSource.sampling_frequency=5000000 SignalSource.samples=0 From aa9608b11a22f48ece3e0f0dc8861f40e72b6bf9 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 17 Dec 2022 18:47:24 +0000 Subject: [PATCH 081/165] MOD: debug kf_tracking --- src/algorithms/PVT/libs/vtl_engine.cc | 3 ++- .../tracking/gnuradio_blocks/kf_tracking.cc | 20 +++++++++++-------- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 458ae1684..8aeaa0abc 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -232,6 +232,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_yerr = kf_H * kf_xerr; // Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): + TrackingCmd trk_cmd; + for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector { rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered @@ -243,7 +245,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) //TODO: Fill the tracking commands outputs // Notice: keep the same satellite order as in the Vtl_Data matrices // sample code - TrackingCmd trk_cmd; trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_freq_hz = doppler_hz_filt(channel) + kf_x(7)/ Lambda_GPS_L1;//+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_rate_hz_s = 0; diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 627ab5089..4e80beb2f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -634,20 +634,23 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr).hash_code()) { const auto cmd = wht::any_cast>(pmt::any_ref(msg)); + //std::cout<< "test cast CH "<sample_counter <<"\n"; if (cmd->channel_id == this->d_channel) { + + arma::vec x_tmp; + arma::mat F_tmp; + gr::thread::scoped_lock lock(d_setlock); //To.Do: apply VTL corrections to the KF states double delta_t_s = static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in; - arma::mat F_tmp; + // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s + x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; //ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0}, {0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)}, {0.0, 0.0, 1.0, delta_t_s}, {0.0, 0.0, 0.0, 1.0}}; - arma::vec x_tmp; - // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s - x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) //arma::vec tmp_x = F_tmp * x_tmp; double old_doppler = d_x_old_old(2); @@ -661,13 +664,14 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // << " SampleCounter origin: " << cmd->sample_counter // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" // << " [s]\n"; - if(this->d_channel ==0) + if(cmd->channel_id ==0) { - std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " - << static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in + std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " + << delta_t_s << "[s]" << " SampleCounter origin: " << cmd->sample_counter << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" - << " [s]\n"; + << "\n"; + std::cout << "use count " < Date: Mon, 19 Dec 2022 16:05:22 +0100 Subject: [PATCH 082/165] Fix vtl cmd vector flush --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 1 + src/algorithms/PVT/libs/vtl_engine.cc | 18 +++++++++--------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 69f08ee21..33f244a2b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2158,6 +2158,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::cout << "VTL Engine problem: " << ex.what() << "\n"; } } + d_internal_pvt_solver->vtl_engine.trk_cmd_outs.clear(); //Dummy messages for evaluation of msg latency // std::map::const_iterator gnss_observables_iter; // for (gnss_observables_iter = d_gnss_observables_map.cbegin(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 8aeaa0abc..4706d87da 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -233,7 +233,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): TrackingCmd trk_cmd; - + for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector { rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered @@ -245,8 +245,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) //TODO: Fill the tracking commands outputs // Notice: keep the same satellite order as in the Vtl_Data matrices // sample code - trk_cmd.carrier_phase_rads = 0; // difficult of calculation - trk_cmd.carrier_freq_hz = doppler_hz_filt(channel) + kf_x(7)/ Lambda_GPS_L1;//+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction + trk_cmd.carrier_phase_rads = 0; // difficult of calculation + trk_cmd.carrier_freq_hz = doppler_hz_filt(channel) + kf_x(7) / Lambda_GPS_L1; //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_rate_hz_s = 0; trk_cmd.code_phase_chips = 0; trk_cmd.enable_carrier_nco_cmd = true; @@ -254,12 +254,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) trk_cmd.sample_counter = new_data.sample_counter; trk_cmd.channel_id = 0; trk_cmd_outs.push_back(trk_cmd); - if(channel==0) - { - std::cout << "CH " << channel - << " Doppler vtl commanded: " << doppler_hz_filt(channel) << " [Hz]" - << " \n"; - } + if (channel == 0) + { + std::cout << "[" << trk_cmd.sample_counter << "] CH " << channel + << " Doppler vtl commanded: " << doppler_hz_filt(channel) << " [Hz]" + << " \n"; + } } From 5864791a309518b6660fbfdcf1759b6c3a659f55 Mon Sep 17 00:00:00 2001 From: miguel Date: Mon, 19 Dec 2022 18:28:41 +0100 Subject: [PATCH 083/165] MOD: numeric derivate correction (temporal) --- src/algorithms/PVT/libs/vtl_engine.cc | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 4706d87da..e90075f6f 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -58,7 +58,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) counter = counter + 1; //uint64_t cout << "counter" << counter << endl; //new_data.kf_state.print("new_data kf initial"); - if (counter < 1000) + uint32_t closure_point=10; + + if (counter < closure_point) { // // receiver solution from rtklib_solver kf_x(0) = new_data.rx_p(0); @@ -182,8 +184,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix new_data.kf_P = kf_P_x; - new_data.kf_state = kf_x; //updated state estimation - // // ################## Geometric Transformation ###################################### // // x_u=kf_x(0); @@ -240,13 +240,20 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1 + kf_x(7)) - kf_yerr(channel + new_data.sat_number); // now filtered // TO DO: convert rhoDot_pri to doppler shift! // Doppler shift defined as pseudorange rate measurement divided by the negative of carrier wavelength. + double d_dt_clk_drift; + d_dt_clk_drift=(kf_x(7)-new_data.kf_state(7)); + if(counter < closure_point) + { doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1; + }else{ + doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)-d_dt_clk_drift) / Lambda_GPS_L1; + } //TODO: Fill the tracking commands outputs // Notice: keep the same satellite order as in the Vtl_Data matrices // sample code trk_cmd.carrier_phase_rads = 0; // difficult of calculation - trk_cmd.carrier_freq_hz = doppler_hz_filt(channel) + kf_x(7) / Lambda_GPS_L1; //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction + trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_rate_hz_s = 0; trk_cmd.code_phase_chips = 0; trk_cmd.enable_carrier_nco_cmd = true; @@ -262,6 +269,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) } } + new_data.kf_state = kf_x; //updated state estimation fstream dump_vtl_file; dump_vtl_file.open("dump_vtl_file.csv", ios::out | ios::app); From 6a2271d07ea7783fdd54eab61991806f9b000fa2 Mon Sep 17 00:00:00 2001 From: miguel Date: Mon, 19 Dec 2022 18:29:20 +0100 Subject: [PATCH 084/165] MOD: correction without time update for debug --- src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 4e80beb2f..cdb3c6c60 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -654,7 +654,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) //arma::vec tmp_x = F_tmp * x_tmp; double old_doppler = d_x_old_old(2); - //d_x_old_old(2) = x_tmp(2); //replace only the Carrier Frequency state + d_x_old_old(2) = x_tmp(2); //replace only the Carrier Frequency state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; From 06f6d6be7b37d58fdb867431a804c757ebd741d4 Mon Sep 17 00:00:00 2001 From: miguel Date: Mon, 19 Dec 2022 18:34:01 +0100 Subject: [PATCH 085/165] MOD: time update implemented --- src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index cdb3c6c60..ec85d9881 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -652,9 +652,9 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) {0.0, 0.0, 1.0, delta_t_s}, {0.0, 0.0, 0.0, 1.0}}; // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) - //arma::vec tmp_x = F_tmp * x_tmp; + arma::vec tmp_x = F_tmp * x_tmp; double old_doppler = d_x_old_old(2); - d_x_old_old(2) = x_tmp(2); //replace only the Carrier Frequency state + d_x_old_old(2) = tmp_x(2); //replace only the Carrier Frequency state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; From 82ef7cd6e52171ead0c92ff2d2feb48c287c07a4 Mon Sep 17 00:00:00 2001 From: miguel Date: Mon, 19 Dec 2022 18:35:31 +0100 Subject: [PATCH 086/165] MOD: closure loop in 5 steps --- src/algorithms/PVT/libs/vtl_engine.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index e90075f6f..246be994f 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -58,7 +58,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) counter = counter + 1; //uint64_t cout << "counter" << counter << endl; //new_data.kf_state.print("new_data kf initial"); - uint32_t closure_point=10; + uint32_t closure_point=5; if (counter < closure_point) { // From 9b9b195509d10566e01b6be92c4b91c6513b2093 Mon Sep 17 00:00:00 2001 From: miguekf Date: Mon, 19 Dec 2022 18:46:54 +0100 Subject: [PATCH 087/165] MOD: MATLAB file doppler correction with time derivative of clk_drift --- src/utils/matlab/vtl/kf_prototype.m | 8 +++++++- src/utils/matlab/vtl/vtl_prototype.m | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index 733ff584f..0c89c9451 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -199,7 +199,13 @@ for t=2:length(navSolution.RX_time) pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t); %convert rhoDot_pri to doppler shift! - sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t))/Lambda_GPS_L1; + d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1)); + + if (t Date: Mon, 19 Dec 2022 18:56:45 +0100 Subject: [PATCH 088/165] MOD: matlab file kf_vtl parameter for state number --- src/utils/matlab/vtl/kf_prototype.m | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index 0c89c9451..422f0ff26 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -2,17 +2,27 @@ %% sat_number=5; %% ################## Kalman filter initialization ###################################### +st_nmbr=8; % covariances (static) -kf_P_x = eye(8); %TODO: use a real value. -kf_x = zeros(8, 1); +kf_P_x = eye(st_nmbr); %TODO: use a real value. +kf_x = zeros(st_nmbr, 1); kf_R = zeros(2*sat_number, 2*sat_number); kf_dt=0.1; -% kf_F = eye(8, 8); +% kf_F = eye(st_nmbr, st_nmbr); % kf_F(1, 4) = kf_dt; % kf_F(2, 5) = kf_dt; % kf_F(3, 6) = kf_dt; % kf_F(7, 8) = kf_dt; +% kf_F=[ 1 0 0 kf_dt 0 0 0 0 0 +% 0 1 0 0 kf_dt 0 0 0 0 +% 0 0 1 0 0 kf_dt 0 0 0 +% 0 0 0 1 0 0 0 0 0 +% 0 0 0 0 1 0 0 0 0 +% 0 0 0 0 0 1 0 0 0 +% 0 0 0 0 0 0 1 kf_dt kf_dt^2/2 +% 0 0 0 0 0 0 0 kf_dt 1]; + kf_F=[ 1 0 0 kf_dt 0 0 0 0 0 1 0 0 kf_dt 0 0 0 0 0 1 0 0 kf_dt 0 0 @@ -22,7 +32,7 @@ kf_F=[ 1 0 0 kf_dt 0 0 0 0 0 0 0 0 0 0 1 kf_dt 0 0 0 0 0 0 0 1]; -kf_H = zeros(2*sat_number,8); +kf_H = zeros(2*sat_number,st_nmbr); kf_y = zeros(2*sat_number, 1); kf_yerr = zeros(2*sat_number, 1); % kf_xerr = zeros(8, 1); @@ -48,7 +58,7 @@ for t=2:length(navSolution.RX_time) %% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS) if (t Date: Tue, 20 Dec 2022 19:21:00 +0100 Subject: [PATCH 089/165] ADD: matlab VTL models with d_clk_drift --- src/utils/matlab/vtl/kf_prototype_clk_d.m | 238 ++++++++++++++++++ .../matlab/vtl/vtl_prototype_clk_d_drift.m | 102 ++++++++ 2 files changed, 340 insertions(+) create mode 100644 src/utils/matlab/vtl/kf_prototype_clk_d.m create mode 100644 src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m diff --git a/src/utils/matlab/vtl/kf_prototype_clk_d.m b/src/utils/matlab/vtl/kf_prototype_clk_d.m new file mode 100644 index 000000000..3b8d53eb9 --- /dev/null +++ b/src/utils/matlab/vtl/kf_prototype_clk_d.m @@ -0,0 +1,238 @@ +%% vtl KF +%% +sat_number=5; +%% ################## Kalman filter initialization ###################################### +st_nmbr=9; +% covariances (static) +kf_P_x = eye(st_nmbr); %TODO: use a real value. +kf_x = zeros(st_nmbr, 1); +kf_R = zeros(2*sat_number, 2*sat_number); +kf_dt=0.1; +% kf_F = eye(st_nmbr, st_nmbr); +% kf_F(1, 4) = kf_dt; +% kf_F(2, 5) = kf_dt; +% kf_F(3, 6) = kf_dt; +% kf_F(7, 8) = kf_dt; + +kf_F=[ 1 0 0 kf_dt 0 0 0 0 0 + 0 1 0 0 kf_dt 0 0 0 0 + 0 0 1 0 0 kf_dt 0 0 0 + 0 0 0 1 0 0 0 0 0 + 0 0 0 0 1 0 0 0 0 + 0 0 0 0 0 1 0 0 0 + 0 0 0 0 0 0 1 kf_dt kf_dt^2/2 + 0 0 0 0 0 0 0 1 kf_dt + 0 0 0 0 0 0 0 0 1]; + +% kf_F=[ 1 0 0 kf_dt 0 0 0 0 +% 0 1 0 0 kf_dt 0 0 0 +% 0 0 1 0 0 kf_dt 0 0 +% 0 0 0 1 0 0 0 0 +% 0 0 0 0 1 0 0 0 +% 0 0 0 0 0 1 0 0 +% 0 0 0 0 0 0 1 kf_dt +% 0 0 0 0 0 0 0 1]; + +kf_H = zeros(2*sat_number,st_nmbr); +kf_y = zeros(2*sat_number, 1); +kf_yerr = zeros(2*sat_number, 1); +% kf_xerr = zeros(8, 1); +kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix + +%% pre-allocate for speed +d = zeros(sat_number, 1); +rho_pri = zeros(sat_number, 1); +rhoDot_pri = zeros(sat_number, 1); +a_x = zeros(sat_number, 1); +a_y = zeros(sat_number, 1); +a_z = zeros(sat_number, 1); +c_pr_m=zeros(sat_number,length(navSolution.RX_time)); + +pr_m_filt=zeros(sat_number,length(navSolution.RX_time)); +rhoDot_pri_filt=zeros(sat_number,length(navSolution.RX_time)); +sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time)); + +%% ################## Kalman Tracking ###################################### +% receiver solution from rtklib_solver +for t=2:length(navSolution.RX_time) + + %% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS) + if (t<3) + %% State error Covariance Matrix Q (PVT) + kf_Q = eye(st_nmbr);%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate. + %% + + % Measurement error Covariance Matrix R assembling + kf_R = blkdiag(eye(sat_number)*40,eye(sat_number)*10); + + else + kf_Q = blkdiag(eye(3)*pos_var,eye(3)*vel_var,clk_bias_var,clk_drift_var,clk_d_drift_var); + kf_R = blkdiag(eye(sat_number)*pr_var,eye(sat_number)*pr_dot_var); + end + + clear x_u y_u z_u xDot_u yDot_u zDot_u cdeltatDot_u cdeltatDot_u_g cdeltat_u_g + if (t<3) + kf_x(1,t) = navSolution.X(t); + kf_x(2,t) = navSolution.Y(t); + kf_x(3,t) = navSolution.Z(t); + kf_x(4,t) = navSolution.vX(t); + kf_x(5,t) = navSolution.vY(t); + kf_x(6,t) = navSolution.vZ(t); + kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S; + kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1); + kf_x(9,t) = 0;%new_data.rx_dts(1); + + x_u=kf_x(1,t); + y_u=kf_x(2,t); + z_u=kf_x(3,t); + xDot_u=kf_x(4,t); + yDot_u=kf_x(5,t); + zDot_u=kf_x(6,t); + cdeltat_u(t)=kf_x(7,t); + cdeltatDot_u(t)=kf_x(8,t); + d_cdeltatDot_u(t)=kf_x(9,t); + + % Kalman state prediction (time update) + kf_P_x = eye(st_nmbr)*100; %TODO: use a real value. + kf_xpri(:,t) = kf_F * (kf_x(:,t)-kf_x(:,t-1));% state prediction + kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction + else + x_u=corr_kf_state(1,t-1); + y_u=corr_kf_state(2,t-1); + z_u=corr_kf_state(3,t-1); + xDot_u=corr_kf_state(4,t-1); + yDot_u=corr_kf_state(5,t-1); + zDot_u=corr_kf_state(6,t-1); + cdeltat_u(t)=corr_kf_state(7,t-1); + cdeltatDot_u(t)=corr_kf_state(8,t-1); + d_cdeltatDot_u(t)=corr_kf_state(9,t-1); + + % Kalman state prediction (time update) + kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction + end + + + for chan=1:5 %neccesary quantities + d(chan)=(sat_posX_m(chan,t)-x_u)^2; + d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2; + d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2; + d(chan)=sqrt(d(chan)); + + c_pr_m(chan,t)=d(chan)+cdeltat_u(t); + + a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan); + a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan); + a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan); + + rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... + +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... + +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); + end + + for chan=1:5 % Measurement matrix H assembling + % It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) + kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0; + kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0; + kf_H(chan+sat_number, 9) = kf_dt; + end + + % unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H)); + % !!!! Limitaciones + % obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad + % no se recomienda para las pruebas de observabilidad. Ob será numéricamente singular para la mayoría de los + % sistemas con más de unos cuantos estados. Este hecho está bien documentado en la sección III de [1]. + + % Kalman estimation (measurement update) + for chan=1:5 % Measurement matrix H assembling + kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t); + kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u(t)+kf_dt*d_cdeltatDot_u(t))-rhoDot_pri(chan,t); + end + + % DOUBLES DIFFERENCES + % kf_yerr = zeros(2*sat_number, 1); + % for (int32_t i = 1; i < sat_number; i++) % Measurement vector + % { + % kf_y(i)=new_data.pr_m(i)-new_data.pr_m(i-1); + % kf_yerr(i)=kf_y(i)-(rho_pri(i)+rho_pri(i-1)); + % kf_y(i+sat_number)=(rhoDot_pri(i)-rhoDot_pri(i-1))/Lambda_GPS_L1; + % kf_yerr(i+sat_number)=kf_y(i+sat_number)-(new_data.doppler_hz(i)-new_data.doppler_hz(i-1)); + % } + % kf_yerr.print("DOUBLES DIFFERENCES"); + + % Kalman filter update step + kf_S = kf_H * kf_P_x* kf_H' + kf_R; % innovation covariance matrix (S) + kf_K = (kf_P_x * kf_H') * inv(kf_S); % Kalman gain + kf_xerr(:,t) = kf_K * (kf_yerr(:,t)); % Error state estimation + kf_P_x = (eye(length(kf_P_x)) - kf_K * kf_H) * kf_P_x; % update state estimation error covariance matrix + + if (t<3) +% kf_xerr(9,t)=15; + corr_kf_state(:,t)=kf_xpri(:,t)-kf_xerr(:,t)+kf_x(:,t); %updated state estimation + else + corr_kf_state(:,t)=corr_kf_state(:,t-1)-kf_xerr(:,t); %updated state estimation + end + + %% ################## Geometric Transformation ###################################### + + x_u=corr_kf_state(1,t); + y_u=corr_kf_state(2,t); + z_u=corr_kf_state(3,t); + xDot_u=corr_kf_state(4,t); + yDot_u=corr_kf_state(5,t); + zDot_u=corr_kf_state(6,t); + cdeltat_u_g=corr_kf_state(7,t); + cdeltatDot_u_g=corr_kf_state(8,t); + d_cdeltatDot_u_g=corr_kf_state(9,t); + +% for chan=1:5 %neccesary quantities +% d(chan)=(sat_posX_m(chan,t)-x_u)^2; +% d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2; +% d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2; +% d(chan)=sqrt(d(chan)); +% +% c_pr_m(chan,t)=d(chan)+cdeltat_u_g; +% +% a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan); +% a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan); +% a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan); +% +% rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... +% +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... +% +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); +% end + + kf_H = zeros(2*sat_number,st_nmbr); + + for chan=1:5 % Measurement matrix H assembling + % It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) + kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0; + kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0; + kf_H(chan+sat_number, 9) = kf_dt; + end + + % Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x + kf_yerr_g=kf_H*kf_xerr; + % Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): + for chan=1:5 % Measurement vector + pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered + rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t)+kf_dt*corr_kf_state(9,t))-kf_yerr_g(chan+sat_number,t); + %convert rhoDot_pri to doppler shift! +% d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1)); + + if (t<3) + sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t)-corr_kf_state(9,t)*kf_dt)/Lambda_GPS_L1; + else + sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t)-corr_kf_state(9,t)*kf_dt)/Lambda_GPS_L1; + end + +% carrier_phase_rads = 0; + carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t); +% carrier_freq_rate_hz_s = 0; +% code_phase_chips = 0; + end + +% carrier_phase_rads = 0; +% carrier_freq_hz = 0; +% carrier_freq_rate_hz_s = 0; +% code_phase_chips = 0; +end \ No newline at end of file diff --git a/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m b/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m new file mode 100644 index 000000000..21b2746ac --- /dev/null +++ b/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m @@ -0,0 +1,102 @@ +% VTL prototype +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% +%% +clc +close all +clearvars + +% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file') +% addpath('./libs') +% end +% +% if ~exist('cat2geo.m', 'file') +% addpath('./libs/geoFunctions') +% end +SPEED_OF_LIGHT_M_S=299792458.0; +Lambda_GPS_L1=0.1902937; +GPS_L1_freq_hz=1575.42e6; +% point_of_closure=4118;%4060; + +%% ==================== VARIANCES ============================= +pos_var=100;%m^2 +vel_var=10;%m^2/s^2 +clk_bias_var=100;%m^2 +clk_drift_var=40;%m^2/s^2 +clk_d_drift_var=1; + +pr_var=20;%m^2 +pr_dot_var=2;%m^2/s^2 +% CARLES PAPER LTE GNSS VTL +% pos_var=2;%m^2 +% vel_var=0.2;%m^2/s^2 +% clk_bias_var=1e-7;%m^2 +% clk_drift_var=1e-4;%m^2/s^2 +% pr_var=20;%m^2 +% pr_dot_var=3;%m^2/s^2 +%% ============================================================ +samplingFreq=5000000; +channels=6; +TTFF_sec=41.48; +spirent_index_TTFF=416; + +plot_skyplot=0; +plot_reference=1; +load_observables=1; + +%% ============================ PARSER TO STRUCT ============================ + +navSolution = GnssSDR2struct('PVT_raw.mat'); +refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv'); +% +load observables\observables_raw.mat +% refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); +rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05. +load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'... + ,'sat_velZ','sat_prg_m','clk_bias_s','clk_drift','sat_dopp_hz','user_clk_offset') + +if(load_observables) + load observables\observables_raw.mat + refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); +end +%% +% vtlSolution = Vtl2struct('dump_vtl_file.csv'); +%% + +kf_prototype_clk_d + +%% ====== FILTERING ======================================================= +% moving_avg_factor= 500; +% LAT_FILT = movmean(navSolution.latitude,moving_avg_factor); +% LON_FILT = movmean(navSolution.longitude,moving_avg_factor); +% HEIGH_FILT = movmean(navSolution.height,moving_avg_factor); +% +% X_FILT = movmean(navSolution.X,moving_avg_factor); +% Y_FILT = movmean(navSolution.Y,moving_avg_factor); +% Z_FILT = movmean(navSolution.Z,moving_avg_factor); +% +% vX_FILT = movmean(navSolution.vX,moving_avg_factor); +% vY_FILT = movmean(navSolution.vY,moving_avg_factor); +% vZ_FILT = movmean(navSolution.vZ,moving_avg_factor); +% +%% +%general_raw_plot +vtl_general_plot + +%% ============================================== ============================================== +%% +figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_yerr(1:5,:)');title('c pr m-error');xlabel('t U.A');ylabel('pr m [m]');grid minor +legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside') +figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_yerr(6:10,:)');title('c pr m DOT-error');xlabel('t U.A');ylabel('pr m dot [m/s]');grid minor +legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside') + +%% +figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(9,:)');title('kf 9 xerr');xlabel('t U.A');ylabel('kf 9 xerr');grid minor From 6039d2471b955c655f485107c5fa5433331df4b4 Mon Sep 17 00:00:00 2001 From: miguekf Date: Wed, 21 Dec 2022 22:32:02 +0100 Subject: [PATCH 090/165] MOD: matlab vtl with 2 order clk model --- src/utils/matlab/vtl/kf_prototype_clk_d.m | 44 +++++++---------------- 1 file changed, 12 insertions(+), 32 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype_clk_d.m b/src/utils/matlab/vtl/kf_prototype_clk_d.m index 3b8d53eb9..2ee938376 100644 --- a/src/utils/matlab/vtl/kf_prototype_clk_d.m +++ b/src/utils/matlab/vtl/kf_prototype_clk_d.m @@ -126,17 +126,17 @@ for t=2:length(navSolution.RX_time) rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... - +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); + +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t)+cdeltatDot_u(t); end for chan=1:5 % Measurement matrix H assembling % It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0; kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0; - kf_H(chan+sat_number, 9) = kf_dt; + kf_H(chan+sat_number, 9) = 0.0; end - % unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H)); + unobsv(t) = st_nmbr - rank(obsv(kf_F,kf_H)); % !!!! Limitaciones % obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad % no se recomienda para las pruebas de observabilidad. Ob será numéricamente singular para la mayoría de los @@ -145,7 +145,7 @@ for t=2:length(navSolution.RX_time) % Kalman estimation (measurement update) for chan=1:5 % Measurement matrix H assembling kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t); - kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u(t)+kf_dt*d_cdeltatDot_u(t))-rhoDot_pri(chan,t); + kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u(t)+d_cdeltatDot_u(t-1)+d_cdeltatDot_u(t)*kf_dt)-rhoDot_pri(chan,t); end % DOUBLES DIFFERENCES @@ -184,23 +184,6 @@ for t=2:length(navSolution.RX_time) cdeltatDot_u_g=corr_kf_state(8,t); d_cdeltatDot_u_g=corr_kf_state(9,t); -% for chan=1:5 %neccesary quantities -% d(chan)=(sat_posX_m(chan,t)-x_u)^2; -% d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2; -% d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2; -% d(chan)=sqrt(d(chan)); -% -% c_pr_m(chan,t)=d(chan)+cdeltat_u_g; -% -% a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan); -% a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan); -% a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan); -% -% rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... -% +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... -% +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); -% end - kf_H = zeros(2*sat_number,st_nmbr); for chan=1:5 % Measurement matrix H assembling @@ -215,20 +198,17 @@ for t=2:length(navSolution.RX_time) % Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): for chan=1:5 % Measurement vector pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered - rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t)+kf_dt*corr_kf_state(9,t))-kf_yerr_g(chan+sat_number,t); + rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t); %convert rhoDot_pri to doppler shift! -% d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1)); + % d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1)); - if (t<3) - sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t)-corr_kf_state(9,t)*kf_dt)/Lambda_GPS_L1; - else - sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t)-corr_kf_state(9,t)*kf_dt)/Lambda_GPS_L1; - end - -% carrier_phase_rads = 0; + sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t)+corr_kf_state(9,t-1)+corr_kf_state(9,t)*kf_dt)/Lambda_GPS_L1; + + + % carrier_phase_rads = 0; carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t); -% carrier_freq_rate_hz_s = 0; -% code_phase_chips = 0; + % carrier_freq_rate_hz_s = 0; + % code_phase_chips = 0; end % carrier_phase_rads = 0; From 63b777ecbd3900f58697e7c5827e69eded778dd6 Mon Sep 17 00:00:00 2001 From: miguekf Date: Wed, 21 Dec 2022 23:45:25 +0100 Subject: [PATCH 091/165] MOD: modify matlab kf_prototype clk_drift error compensation --- src/utils/matlab/vtl/kf_prototype.m | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index 422f0ff26..b5c04ca0e 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -122,7 +122,7 @@ for t=2:length(navSolution.RX_time) rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... - +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t)+cdeltatDot_u; + +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); end for chan=1:5 % Measurement matrix H assembling @@ -140,7 +140,12 @@ for t=2:length(navSolution.RX_time) % Kalman estimation (measurement update) for chan=1:5 % Measurement matrix H assembling kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t); - kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(chan,t); + + if (t Date: Wed, 21 Dec 2022 23:47:57 +0100 Subject: [PATCH 092/165] MOD: modify matlab kf_prototype clk_drift error compensation --- src/utils/matlab/vtl/kf_prototype.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index b5c04ca0e..836e8ba9c 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -122,7 +122,7 @@ for t=2:length(navSolution.RX_time) rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... - +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); + +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t)+cdeltatDot_u; end for chan=1:5 % Measurement matrix H assembling @@ -142,9 +142,9 @@ for t=2:length(navSolution.RX_time) kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t); if (t Date: Wed, 21 Dec 2022 23:53:24 +0100 Subject: [PATCH 093/165] FIX: matlab kf_prototype clk_drift error compensation --- src/utils/matlab/vtl/kf_prototype.m | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index 836e8ba9c..f00e1ca05 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -122,7 +122,7 @@ for t=2:length(navSolution.RX_time) rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... - +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t)+cdeltatDot_u; + +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); end for chan=1:5 % Measurement matrix H assembling @@ -142,9 +142,9 @@ for t=2:length(navSolution.RX_time) kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t); if (t Date: Sun, 25 Dec 2022 02:23:54 +0100 Subject: [PATCH 094/165] ADD: clk drift derivative. 9 state kf --- src/algorithms/PVT/libs/rtklib_solver.cc | 4 +- src/algorithms/PVT/libs/vtl_engine.cc | 49 ++++++++++++++---------- 2 files changed, 30 insertions(+), 23 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 613ef0421..49cf7e34a 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1213,8 +1213,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) - vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]+SPEED_OF_LIGHT_M_S * dts[n * 2]; - vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; + vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n] + SPEED_OF_LIGHT_M_S * dts[n * 2]; + vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0] - SPEED_OF_LIGHT_M_S *dts[1 + 2 * n] / Lambda_GPS_L1; vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; vtl_data.pr_res(n) = pr_residual_vec[n]; } diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 246be994f..61225b28f 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -35,22 +35,22 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // ################## Kalman filter initialization ###################################### // covariances (static) - kf_P_x = arma::eye(8, 8) * 1.0; //TODO: use a real value. - kf_x = arma::zeros(8, 1); + kf_P_x = arma::eye(9, 9) * 1.0; //TODO: use a real value. + kf_x = arma::zeros(9, 1); kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); double kf_dt = 0.1; - kf_Q = arma::eye(8, 8); + kf_Q = arma::eye(9, 9); - kf_F = arma::eye(8, 8); + kf_F = arma::eye(9, 9); kf_F(0, 3) = kf_dt; kf_F(1, 4) = kf_dt; kf_F(2, 5) = kf_dt; kf_F(6, 7) = kf_dt; - kf_H = arma::zeros(2 * new_data.sat_number, 8); + kf_H = arma::zeros(2 * new_data.sat_number, 9); kf_y = arma::zeros(2 * new_data.sat_number, 1); kf_yerr = arma::zeros(2 * new_data.sat_number, 1); - kf_xerr = arma::zeros(8, 1); + kf_xerr = arma::zeros(9, 1); kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix // ################## Kalman Tracking ###################################### @@ -61,7 +61,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) uint32_t closure_point=5; if (counter < closure_point) - { // + { // receiver solution from rtklib_solver kf_x(0) = new_data.rx_p(0); kf_x(1) = new_data.rx_p(1); @@ -71,6 +71,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(5) = new_data.rx_v(2); kf_x(6) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; kf_x(7) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; + kf_x(8) = 1.0; kf_x = kf_F * kf_x; // state prediction } @@ -85,6 +86,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(5) = new_data.kf_state[5]; kf_x(6) = new_data.kf_state[6]; kf_x(7) = new_data.kf_state[7]; + kf_x(8) = new_data.kf_state[8]; kf_P_x = new_data.kf_P; } // State error Covariance Matrix Q (PVT) @@ -95,8 +97,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_Q(3, 3) = 10.0; kf_Q(4, 4) = 10.0; kf_Q(5, 5) = 10.0; - kf_Q(6, 6) = 40.0; - kf_Q(7, 7) = 1500.0; + kf_Q(6, 6) = 100.0; + kf_Q(7, 7) = 40.0; + kf_Q(8, 8) = 1.0; // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) @@ -154,11 +157,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) rhoDot_pri(i) = (new_data.sat_v(i, 0) - xDot_u) * a_x(i) + (new_data.sat_v(i, 1) - yDot_u) * a_y(i) + (new_data.sat_v(i, 2) - zDot_u) * a_z(i); } - kf_H = arma::zeros(2 * new_data.sat_number, 8); + kf_H = arma::zeros(2 * new_data.sat_number, 9); for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling { - // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) + // It has 9 columns (9 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); @@ -171,10 +174,14 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // Kalman estimation (measurement update) for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector { - //kf_y(i) = new_data.pr_m(i); // i-Satellite - //kf_y(i+new_data.sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; // i-Satellite kf_yerr(i) = rho_pri(i) - new_data.pr_m(i); - kf_yerr(i + new_data.sat_number) = (new_data.doppler_hz(i) * Lambda_GPS_L1) - rhoDot_pri(i); + if (counter < closure_point) + { + kf_yerr(i + new_data.sat_number) = (new_data.doppler_hz(i) * Lambda_GPS_L1) - rhoDot_pri(i); + }else{ + kf_yerr(i + new_data.sat_number) = (new_data.doppler_hz(i) * Lambda_GPS_L1+cdeltatDot_u) - rhoDot_pri(i); + + } } // Kalman filter update step @@ -210,14 +217,14 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) a_y(i) = -(new_data.sat_p(i, 1) - kf_x(1)) / d(i); a_z(i) = -(new_data.sat_p(i, 2) - kf_x(2)) / d(i); //compute pseudorange rate estimation - rhoDot_pri(i) = (new_data.sat_v(i, 0) - kf_x(3)) * a_x(i) + (new_data.sat_v(i, 1) - kf_x(4)) * a_y(i) + (new_data.sat_v(i, 2) - kf_x(5)) * a_z(i) + kf_x(7); + rhoDot_pri(i) = (new_data.sat_v(i, 0) - kf_x(3)) * a_x(i) + (new_data.sat_v(i, 1) - kf_x(4)) * a_y(i) + (new_data.sat_v(i, 2) - kf_x(5)) * a_z(i); } - kf_H = arma::zeros(2 * new_data.sat_number, 8); + kf_H = arma::zeros(2 * new_data.sat_number, 9); for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling { - // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) + // It has 9 columns (9 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); @@ -240,13 +247,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1 + kf_x(7)) - kf_yerr(channel + new_data.sat_number); // now filtered // TO DO: convert rhoDot_pri to doppler shift! // Doppler shift defined as pseudorange rate measurement divided by the negative of carrier wavelength. - double d_dt_clk_drift; - d_dt_clk_drift=(kf_x(7)-new_data.kf_state(7)); + //double d_dt_clk_drift; + //d_dt_clk_drift=(kf_x(7)-new_data.kf_state(7)); if(counter < closure_point) { doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1; }else{ - doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)-d_dt_clk_drift) / Lambda_GPS_L1; + doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1; } //TODO: Fill the tracking commands outputs @@ -283,7 +290,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) dump_vtl_file << "kf_xerr" << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) << "," << kf_xerr(6) << "," << kf_xerr(7) << endl; dump_vtl_file << "kf_state" - << "," << new_data.kf_state(0) << "," << new_data.kf_state(1) << "," << new_data.kf_state(2) << "," << new_data.kf_state(3) << "," << new_data.kf_state(4) << "," << new_data.kf_state(5) << "," << new_data.kf_state(6) << "," << new_data.kf_state(7) << endl; + << "," << new_data.kf_state(0) << "," << new_data.kf_state(1) << "," << new_data.kf_state(2) << "," << new_data.kf_state(3) << "," << new_data.kf_state(4) << "," << new_data.kf_state(5) << "," << new_data.kf_state(6) << "," << new_data.kf_state(7)<< new_data.kf_state(8) << endl; dump_vtl_file << "rtklib_state" << "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << endl; dump_vtl_file << "filt_dopp_sat" From 3f199e85f3eb1e61fc72e06b006e78524c6fd27b Mon Sep 17 00:00:00 2001 From: miguekf Date: Tue, 27 Dec 2022 17:13:23 +0100 Subject: [PATCH 095/165] ADD: matlab vtl_kf with 9 states and carrier phase error --- src/utils/matlab/vtl/kf_prototype4acc.m | 239 ++++++++++++++++++++++++ 1 file changed, 239 insertions(+) create mode 100644 src/utils/matlab/vtl/kf_prototype4acc.m diff --git a/src/utils/matlab/vtl/kf_prototype4acc.m b/src/utils/matlab/vtl/kf_prototype4acc.m new file mode 100644 index 000000000..126b46917 --- /dev/null +++ b/src/utils/matlab/vtl/kf_prototype4acc.m @@ -0,0 +1,239 @@ +%% vtl KF +%% +sat_number=5; +%% ################## Kalman filter initialization ###################################### +st_nmbr=9; +% covariances (static) +kf_P_x = eye(st_nmbr); %TODO: use a real value. +kf_x = zeros(st_nmbr, 1); +kf_R = zeros(2*sat_number, 2*sat_number); +kf_dt=0.1; +% kf_F = eye(st_nmbr, st_nmbr); +% kf_F(1, 4) = kf_dt; +% kf_F(2, 5) = kf_dt; +% kf_F(3, 6) = kf_dt; +% kf_F(7, 8) = kf_dt; + +kf_F=[ 1 0 0 kf_dt 0 0 0 0 0 + 0 1 0 0 kf_dt 0 0 0 0 + 0 0 1 0 0 kf_dt 0 0 0 + 0 0 0 1 0 0 0 0 0 + 0 0 0 0 1 0 0 0 0 + 0 0 0 0 0 1 0 0 0 + 0 0 0 0 0 0 1 kf_dt kf_dt^2/2 + 0 0 0 0 0 0 0 1 kf_dt + 0 0 0 0 0 0 0 0 1]; + +% kf_F=[ 1 0 0 kf_dt 0 0 0 0 +% 0 1 0 0 kf_dt 0 0 0 +% 0 0 1 0 0 kf_dt 0 0 +% 0 0 0 1 0 0 0 0 +% 0 0 0 0 1 0 0 0 +% 0 0 0 0 0 1 0 0 +% 0 0 0 0 0 0 1 kf_dt +% 0 0 0 0 0 0 0 1]; +% +kf_H = zeros(2*sat_number,st_nmbr); +kf_y = zeros(2*sat_number, 1); +kf_yerr = zeros(2*sat_number, 1); +% kf_xerr = zeros(8, 1); +kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix + +%% pre-allocate for speed +d = zeros(sat_number, 1); +rho_pri = zeros(sat_number, 1); +rhoDot_pri = zeros(sat_number, 1); +a_x = zeros(sat_number, 1); +a_y = zeros(sat_number, 1); +a_z = zeros(sat_number, 1); +c_pr_m=zeros(sat_number,length(navSolution.RX_time)); + +pr_m_filt=zeros(sat_number,length(navSolution.RX_time)); +rhoDot_pri_filt=zeros(sat_number,length(navSolution.RX_time)); +sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time)); + +%% ################## Kalman Tracking ###################################### +% receiver solution from rtklib_solver +for t=2:length(navSolution.RX_time) + + %% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS) + if (t Date: Tue, 27 Dec 2022 17:14:46 +0100 Subject: [PATCH 096/165] MOD: matlab code phase error plots --- .../matlab/vtl/vtl_prototype_clk_d_drift.m | 57 +++++++++++++++++-- 1 file changed, 53 insertions(+), 4 deletions(-) diff --git a/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m b/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m index 21b2746ac..b6e106f01 100644 --- a/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m +++ b/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m @@ -24,14 +24,14 @@ clearvars SPEED_OF_LIGHT_M_S=299792458.0; Lambda_GPS_L1=0.1902937; GPS_L1_freq_hz=1575.42e6; -% point_of_closure=4118;%4060; +point_of_closure=4060;%4060; %% ==================== VARIANCES ============================= pos_var=100;%m^2 vel_var=10;%m^2/s^2 clk_bias_var=100;%m^2 -clk_drift_var=40;%m^2/s^2 -clk_d_drift_var=1; +clk_drift_var=100;%m^2/s^2 +clk_d_drift_var=10; pr_var=20;%m^2 pr_dot_var=2;%m^2/s^2 @@ -51,6 +51,7 @@ spirent_index_TTFF=416; plot_skyplot=0; plot_reference=1; load_observables=1; +load_tfk_cmd=1; %% ============================ PARSER TO STRUCT ============================ @@ -68,10 +69,36 @@ if(load_observables) refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); end %% +if(load_tfk_cmd) + trkSolution=trk2struct('dump_trk_file.csv'); + + % split by solution type + figure;sgtitle('real doppler') + for chan=0:4 + eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.real==chan);']) + eval(['Dopp_real_CH' num2str(chan) '=trkSolution.dopp.real(indCH' num2str(chan) ',2);']) + eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_real_CH' num2str(chan) ')']) + end + figure;sgtitle('cmd doppler') + for chan=0:4 + eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.cmd==chan);']) + eval(['Dopp_cmd_CH' num2str(chan) '=trkSolution.dopp.cmd(indCH' num2str(chan) ',2);']) + eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_cmd_CH' num2str(chan) ')']) + end + + figure;sgtitle('real phase') + for chan=0:4 + eval(['[indCH' num2str(chan) ',~]= find(trkSolution.phase.real==chan);']) + eval(['Phase_real_CH' num2str(chan) '=trkSolution.phase.real(indCH' num2str(chan) ',2);']) + eval(['subplot(2,3,' num2str(chan+1) ');plot(Phase_real_CH' num2str(chan) ')']) + end + +end +%% % vtlSolution = Vtl2struct('dump_vtl_file.csv'); %% -kf_prototype_clk_d +kf_prototype4acc %% ====== FILTERING ======================================================= % moving_avg_factor= 500; @@ -100,3 +127,25 @@ legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside') %% figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(9,:)');title('kf 9 xerr');xlabel('t U.A');ylabel('kf 9 xerr');grid minor +%% + +figure;sgtitle(' error in phase') +for chan=0:4 + eval(['subplot(2,3,' num2str(chan+1) ');plot(err_carrier_phase_rads_filt(' num2str(chan+1) ',:))']) + hold on; grid minor; +end + +%% +% figure;sgtitle('real vs cmd phase') +% for chan=0:4 +% eval(['subplot(2,3,' num2str(chan+1) ');plot(Phase_real_CH' num2str(chan) ')']) +% hold on; grid minor; +% eval(['subplot(2,3,' num2str(chan+1) ');plot(carrier_phase_rads_filt(' num2str(chan+1) ',1:3425)+Phase_real_CH' num2str(chan) '(1)' ')']) +% legend('real','cmd') +% end +% %% +% figure;sgtitle(' error in cmd to actual phase') +% for chan=0:4 +% eval(['subplot(2,3,' num2str(chan+1) ');plot(Phase_real_CH' num2str(chan) '-transpose(carrier_phase_rads_filt(' num2str(chan+1) ',1:3425))+Phase_real_CH' num2str(chan) '(1)' ')']) +% grid minor; +% end \ No newline at end of file From 87b3dfcd02e915772a020eae2c3c6f92f0de5b40 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 30 Dec 2022 17:40:22 +0000 Subject: [PATCH 097/165] MOD: kf_tracking doppler shift file dump --- .../tracking/gnuradio_blocks/kf_tracking.cc | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index ec85d9881..6da782672 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -654,7 +654,10 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) arma::vec tmp_x = F_tmp * x_tmp; double old_doppler = d_x_old_old(2); - d_x_old_old(2) = tmp_x(2); //replace only the Carrier Frequency state + double old_doppler_shift = d_x_old_old(3); + double old_code_phase_chips = d_x_old_old(0)*SPEED_OF_LIGHT_M_S; + d_x_old_old(2) = tmp_x(2); //replace the Carrier Frequency state + //d_x_old_old(0) = tmp_x(0); //replace the Code Phase state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; @@ -664,15 +667,15 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // << " SampleCounter origin: " << cmd->sample_counter // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" // << " [s]\n"; - if(cmd->channel_id ==0) - { - std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " - << delta_t_s << "[s]" - << " SampleCounter origin: " << cmd->sample_counter - << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" - << "\n"; - std::cout << "use count " <channel_id ==0) + // { + // std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " + // << delta_t_s << "[s]" + // << " SampleCounter origin: " << cmd->sample_counter + // << " code phase new state: " << x_tmp(0) << " vs. trk state: " << old_code_phase_chips << " [chips]" + // << "\n"; + // std::cout << "use count " <d_channel << "," << x_tmp(2) << "," << old_doppler << "\n"; + << ","<< this->d_channel << "," << x_tmp(2) << "," << old_doppler << "," << old_doppler_shift << "," << old_code_phase_chips << "\n"; dump_tracking_file.close(); } } From 4a578a5e7bd3f685e1a1322de1f7247c98dd4d66 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 30 Dec 2022 17:42:01 +0000 Subject: [PATCH 098/165] MOD: reduce closure steps from 4 to 2 (minimum) --- src/algorithms/PVT/libs/vtl_engine.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 61225b28f..ac119908a 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -58,7 +58,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) counter = counter + 1; //uint64_t cout << "counter" << counter << endl; //new_data.kf_state.print("new_data kf initial"); - uint32_t closure_point=5; + uint32_t closure_point=3; if (counter < closure_point) { From d8fc40e2090be05c519545a0b1f9c6ac95e1ddd6 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 30 Dec 2022 17:42:28 +0000 Subject: [PATCH 099/165] FORMAT: clean finished TODO coments --- src/algorithms/PVT/libs/vtl_engine.cc | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index ac119908a..802e7a8a5 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -245,10 +245,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) { rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1 + kf_x(7)) - kf_yerr(channel + new_data.sat_number); // now filtered - // TO DO: convert rhoDot_pri to doppler shift! - // Doppler shift defined as pseudorange rate measurement divided by the negative of carrier wavelength. - //double d_dt_clk_drift; - //d_dt_clk_drift=(kf_x(7)-new_data.kf_state(7)); + if(counter < closure_point) { doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1; From 4bdc6b71107a53f3713cba6f2c56079a3362904f Mon Sep 17 00:00:00 2001 From: miguekf Date: Fri, 30 Dec 2022 18:47:49 +0100 Subject: [PATCH 100/165] MOD: matlab change carrier phase dump for doppler shift dump and add code phase dump --- src/utils/matlab/vtl/trk2struct.m | 9 ++++-- .../matlab/vtl/vtl_prototype_clk_d_drift.m | 28 ++++++++++++++----- 2 files changed, 27 insertions(+), 10 deletions(-) diff --git a/src/utils/matlab/vtl/trk2struct.m b/src/utils/matlab/vtl/trk2struct.m index b0532148f..efee9865b 100644 --- a/src/utils/matlab/vtl/trk2struct.m +++ b/src/utils/matlab/vtl/trk2struct.m @@ -11,15 +11,15 @@ % Auto-generated by MATLAB on 24-Nov-2022 17:34:27 function [trkSolution] = trk2struct(path_to_trk_csv) %% Set up the Import Options and import the data -opts = delimitedTextImportOptions("NumVariables", 4); +opts = delimitedTextImportOptions("NumVariables", 5); % Specify range and delimiter opts.DataLines = [1, Inf]; opts.Delimiter = ","; % Specify column names and types -opts.VariableNames = ["doppler_corr", "VarName2", "VarName3", "VarName4"]; -opts.VariableTypes = ["char", "double", "double", "double"]; +opts.VariableNames = ["doppler_corr", "VarName2", "VarName3", "VarName4","VarName5","VarName5"]; +opts.VariableTypes = ["char", "double", "double", "double", "double", "double"]; % Specify file level properties opts.ExtraColumnsRule = "ignore"; @@ -52,5 +52,8 @@ trk_dopp=cell2mat(trk_dopp); for i=1:length(trk_dopp) trkSolution.dopp.real=[trk_dopp(:,1) trk_dopp(:,3)]; trkSolution.dopp.cmd=[trk_dopp(:,1) trk_dopp(:,2)]; + + trkSolution.doppShift.real=[trk_dopp(:,1) trk_dopp(:,4)]; + trkSolution.tao.real=[trk_dopp(:,1) trk_dopp(:,5)]; end end \ No newline at end of file diff --git a/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m b/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m index b6e106f01..e42484acb 100644 --- a/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m +++ b/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m @@ -86,11 +86,19 @@ if(load_tfk_cmd) eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_cmd_CH' num2str(chan) ')']) end - figure;sgtitle('real phase') + figure;sgtitle('real doppler shift') for chan=0:4 - eval(['[indCH' num2str(chan) ',~]= find(trkSolution.phase.real==chan);']) - eval(['Phase_real_CH' num2str(chan) '=trkSolution.phase.real(indCH' num2str(chan) ',2);']) - eval(['subplot(2,3,' num2str(chan+1) ');plot(Phase_real_CH' num2str(chan) ')']) + eval(['[indCH' num2str(chan) ',~]= find(trkSolution.doppShift.real==chan);']) + eval(['doppShift_CH' num2str(chan) '=trkSolution.doppShift.real(indCH' num2str(chan) ',2);']) + eval(['subplot(2,3,' num2str(chan+1) ');plot(doppShift_CH' num2str(chan) ')']) + grid on + end + + figure;sgtitle('real code phase') + for chan=0:4 + eval(['[indCH' num2str(chan) ',~]= find(trkSolution.tao.real==chan);']) + eval(['tao_real_CH' num2str(chan) '=trkSolution.tao.real(indCH' num2str(chan) ',2);']) + eval(['subplot(2,3,' num2str(chan+1) ');plot(tao_real_CH' num2str(chan) ')']) end end @@ -127,14 +135,20 @@ legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside') %% figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(9,:)');title('kf 9 xerr');xlabel('t U.A');ylabel('kf 9 xerr');grid minor +% %% +% +% figure;sgtitle(' error in phase') +% for chan=0:4 +% eval(['subplot(2,3,' num2str(chan+1) ');plot(err_carrier_phase_rads_filt(' num2str(chan+1) ',:))']) +% hold on; grid minor; +% end %% -figure;sgtitle(' error in phase') +figure;sgtitle(' error in code phase') for chan=0:4 - eval(['subplot(2,3,' num2str(chan+1) ');plot(err_carrier_phase_rads_filt(' num2str(chan+1) ',:))']) + eval(['subplot(2,3,' num2str(chan+1) ');plot(err_code_phase_chips(' num2str(chan+1) ',:))']) hold on; grid minor; end - %% % figure;sgtitle('real vs cmd phase') % for chan=0:4 From 6de8fdab323f13978f7520a7f60b365561eb5a24 Mon Sep 17 00:00:00 2001 From: miguekf Date: Fri, 30 Dec 2022 18:53:02 +0100 Subject: [PATCH 101/165] FORMAT: matlab vtl_kf file --- src/utils/matlab/vtl/kf_prototype4acc.m | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype4acc.m b/src/utils/matlab/vtl/kf_prototype4acc.m index 126b46917..26e87e0af 100644 --- a/src/utils/matlab/vtl/kf_prototype4acc.m +++ b/src/utils/matlab/vtl/kf_prototype4acc.m @@ -217,7 +217,7 @@ for t=2:length(navSolution.RX_time) pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t); %convert rhoDot_pri to doppler shift! - % d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1)); + % d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1)); % d_dt_clk_drift=0; if (t Date: Fri, 30 Dec 2022 18:53:54 +0100 Subject: [PATCH 102/165] ADD_matlab: code phase error chips estimation --- src/utils/matlab/vtl/kf_prototype4acc.m | 1 + 1 file changed, 1 insertion(+) diff --git a/src/utils/matlab/vtl/kf_prototype4acc.m b/src/utils/matlab/vtl/kf_prototype4acc.m index 26e87e0af..11e4300ab 100644 --- a/src/utils/matlab/vtl/kf_prototype4acc.m +++ b/src/utils/matlab/vtl/kf_prototype4acc.m @@ -229,6 +229,7 @@ for t=2:length(navSolution.RX_time) err_carrier_phase_rads_filt(chan,t) = trapz(kf_yerr_g(chan+sat_number,1:t)/Lambda_GPS_L1)*2*kf_dt; carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t); % carrier_freq_rate_hz_s = 0; + err_code_phase_chips(chan,t) = (kf_yerr_g(chan,t))/SPEED_OF_LIGHT_M_S*1023e3; end end \ No newline at end of file From 86859d13922a7d80b62e6e021c530e21f949cf7e Mon Sep 17 00:00:00 2001 From: miguel Date: Mon, 2 Jan 2023 15:38:10 +0100 Subject: [PATCH 103/165] ADD: vtl code phase correction --- src/algorithms/PVT/libs/vtl_engine.cc | 2 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 24 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 802e7a8a5..e9b3dce68 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -259,7 +259,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_rate_hz_s = 0; - trk_cmd.code_phase_chips = 0; + trk_cmd.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; trk_cmd.enable_carrier_nco_cmd = true; trk_cmd.enable_code_nco_cmd = true; trk_cmd.sample_counter = new_data.sample_counter; diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 6da782672..3661fd84f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -655,9 +655,9 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) arma::vec tmp_x = F_tmp * x_tmp; double old_doppler = d_x_old_old(2); double old_doppler_shift = d_x_old_old(3); - double old_code_phase_chips = d_x_old_old(0)*SPEED_OF_LIGHT_M_S; + double old_code_phase_chips = d_x_old_old(0); d_x_old_old(2) = tmp_x(2); //replace the Carrier Frequency state - //d_x_old_old(0) = tmp_x(0); //replace the Code Phase state + d_x_old_old(0) = tmp_x(0); //replace the Code Phase state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; @@ -667,15 +667,15 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // << " SampleCounter origin: " << cmd->sample_counter // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" // << " [s]\n"; - // if(cmd->channel_id ==0) - // { - // std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " - // << delta_t_s << "[s]" - // << " SampleCounter origin: " << cmd->sample_counter - // << " code phase new state: " << x_tmp(0) << " vs. trk state: " << old_code_phase_chips << " [chips]" - // << "\n"; - // std::cout << "use count " <channel_id ==0) + { + std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " + << delta_t_s << "[s]" + << " SampleCounter origin: " << cmd->sample_counter + << " code phase new state: " << x_tmp(0) << " vs. trk state: " << old_code_phase_chips << " [chips]" + << "\n"; + std::cout << "use count " < Date: Mon, 2 Jan 2023 15:51:33 +0100 Subject: [PATCH 104/165] MOD: comment kf_tracking screen output --- .../tracking/gnuradio_blocks/kf_tracking.cc | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 3661fd84f..a0379c43d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -667,15 +667,15 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // << " SampleCounter origin: " << cmd->sample_counter // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" // << " [s]\n"; - if(cmd->channel_id ==0) - { - std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " - << delta_t_s << "[s]" - << " SampleCounter origin: " << cmd->sample_counter - << " code phase new state: " << x_tmp(0) << " vs. trk state: " << old_code_phase_chips << " [chips]" - << "\n"; - std::cout << "use count " <channel_id ==0) + // { + // std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " + // << delta_t_s << "[s]" + // << " SampleCounter origin: " << cmd->sample_counter + // << " code phase new state: " << x_tmp(0) << " vs. trk state: " << old_code_phase_chips << " [chips]" + // << "\n"; + // std::cout << "use count " < Date: Tue, 3 Jan 2023 11:51:29 +0100 Subject: [PATCH 105/165] FIX: code error reset bug --- src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index a0379c43d..4ab28257d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -1216,7 +1216,7 @@ void kf_tracking::run_Kf() // Kalman loop // Prediction - d_x_old_old(0)=0; // reset error estimation because the NCO corrects the code phase + //d_x_old_old(0)=0; // reset error estimation because the NCO corrects the code phase d_x_new_old = d_F * d_x_old_old; d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q; @@ -1231,7 +1231,7 @@ void kf_tracking::run_Kf() // new code phase estimation d_code_error_kf_chips = d_x_new_new(0); - + d_x_new_new(0)=0; // reset error estimation because the NCO corrects the code phase // new carrier phase estimation d_carrier_phase_kf_rad = d_x_new_new(1); From 1b0a3f6b378acac06810392550abd9ad459cea98 Mon Sep 17 00:00:00 2001 From: miguekf Date: Tue, 3 Jan 2023 13:40:58 +0100 Subject: [PATCH 106/165] ADD: matlab skyplot implemented --- .../vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 32 +++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m index cf743c75a..dc933909b 100644 --- a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m +++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m @@ -30,7 +30,7 @@ channels=6; TTFF_sec=41.48; %% ============================ PARSER TO STRUCT ============================ -plot_skyplot=0; % not yet implemented +plot_skyplot=1; plot_reference=1; load_observables=1; advance_vtl_data_available=1; @@ -371,4 +371,32 @@ if(load_observables) grid on end %% -dopp_filtered_plotting \ No newline at end of file +dopp_filtered_plotting +%% +labels = ["G1", "G22", "E17", "E11"]; %inizialitation neccesary +if(plot_skyplot) + + for i=1:length(refSatData.GPS.SIM_time) + for j=1:length(refSatData.GPS.series(i).Sat_PRN) + + labels{i,j} = ['G' num2str(refSatData.GPS.series(i).Sat_PRN(j))]; + + if(refSatData.GPS.series(i).azimuth(j)<0) + az(i,j) = rad2deg(refSatData.GPS.series(i).azimuth(j))+360; + else + az(i,j) = rad2deg(refSatData.GPS.series(i).azimuth(j)); + end + el(i,j) = rad2deg(refSatData.GPS.series(i).elevation(j)); + end + groups(i,:) = categorical([0 0 1 1], [0 1], ["GPS", "Galileo"]); + end + % skyplot(az(i,:), el(i,:), labels, "GroupData", groups) + figure; + %skyplot(az(500,:), el(500,:), labels(500,:)) + sp = skyplot(az(1,:),el(1,:),labels(1,:)); + for idx = 1:length(az) + set(sp,AzimuthData=az(1:idx,:),ElevationData=el(1:idx,:)); + drawnow limitrate + end + legend('GPS') +end \ No newline at end of file From 247eeb2bbb784296b8fed78793e4624e4d17d6cc Mon Sep 17 00:00:00 2001 From: miguekf Date: Tue, 3 Jan 2023 18:51:13 +0100 Subject: [PATCH 107/165] FIX: matlab spirent import bug when a constellation is not present. --- src/utils/matlab/vtl/SpirentSatData2struct.m | 22 +++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/src/utils/matlab/vtl/SpirentSatData2struct.m b/src/utils/matlab/vtl/SpirentSatData2struct.m index 37e930ebd..640e5ad38 100644 --- a/src/utils/matlab/vtl/SpirentSatData2struct.m +++ b/src/utils/matlab/vtl/SpirentSatData2struct.m @@ -99,15 +99,18 @@ refSatData.BEIDOU=[]; [indGALILEO,~]= find(strcmp(satdataV1A1SPFLD1, 'GALILEO')); [indGPS,~]= find(strcmp(satdataV1A1SPFLD1, 'GPS')); -GAL=satdataV1A1SPFLD1(indGALILEO,:);GAL(:,3)=[]; -GPS=satdataV1A1SPFLD1(indGPS,:); GPS(:,3)=[]; -GAL=cell2mat(GAL); -GPS=cell2mat(GPS); - %% SPIRENT SAT SOLUTION +if(~isempty(indGALILEO)) + GAL=satdataV1A1SPFLD1(indGALILEO,:);GAL(:,3)=[]; + GAL=cell2mat(GAL); +end +if(~isempty(indGPS)) + GPS=satdataV1A1SPFLD1(indGPS,:); GPS(:,3)=[]; + GPS=cell2mat(GPS); +end + +if(~isempty(indGALILEO)) refSatData.GALILEO.SIM_time=unique(GAL(:,1),'first'); - refSatData.GPS.SIM_time=unique(GPS(:,1),'first'); - %% k=0; for i=1:length(refSatData.GALILEO.SIM_time) nsats=length(find(GAL(:,1)==refSatData.GALILEO.SIM_time(i))); @@ -145,7 +148,10 @@ GPS=cell2mat(GPS); end end +end %% ------------------------------------- +if(~isempty(indGPS)) + refSatData.GPS.SIM_time=unique(GPS(:,1),'first'); k=0; for i=1:length(refSatData.GPS.SIM_time) nsats=length(find(GPS(:,1)==refSatData.GPS.SIM_time(i))); @@ -182,7 +188,7 @@ GPS=cell2mat(GPS); refSatData.GPS.series(i).sat_Acc_Z(j) = GPS(k,49); end end - +end % Clear temporary variables clear satdataV1A1SPFLD05 GPS GAL end From aaa38386b5272e6899715ac6b61d33135bb6abe1 Mon Sep 17 00:00:00 2001 From: miguekf Date: Tue, 3 Jan 2023 18:53:11 +0100 Subject: [PATCH 108/165] FIX: matlab plot filtered doppler only if VTL data is present --- src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m index dc933909b..07c0a20d0 100644 --- a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m +++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m @@ -371,7 +371,9 @@ if(load_observables) grid on end %% -dopp_filtered_plotting +if(load_vtl) + dopp_filtered_plotting +end %% labels = ["G1", "G22", "E17", "E11"]; %inizialitation neccesary if(plot_skyplot) From 1a5f2b42a8f96e2b04f0a65332f5c5db1b41635e Mon Sep 17 00:00:00 2001 From: miguekf Date: Tue, 3 Jan 2023 18:54:05 +0100 Subject: [PATCH 109/165] FIX: matlab only kf_yerr plot if advanced vtl data is present --- src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m index 07c0a20d0..77a0fb415 100644 --- a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m +++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m @@ -106,12 +106,14 @@ rhoDot_pri(chan,t)=(sat_velX(chan,t)-navSolution.vX(t))*a_x(chan,t)... kf_yerr(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t); end end -end -%% figure;plot(kf_yerr') -%% + figure;plot(pr_m'-sat_prg_m');hold on legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside') +end + +%% + %% === Convert to UTM coordinate system ============================= % Scenario latitude is xx.xxxxxxx N37 49 9.98 From 400310df585357848ffd173e5e478d4da492fbac Mon Sep 17 00:00:00 2001 From: miguekf Date: Wed, 4 Jan 2023 19:21:38 +0100 Subject: [PATCH 110/165] ADD: matlab CN0 sat data reading --- src/utils/matlab/vtl/Vtl2struct.m | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/utils/matlab/vtl/Vtl2struct.m b/src/utils/matlab/vtl/Vtl2struct.m index d693f23a3..6ffc62e85 100644 --- a/src/utils/matlab/vtl/Vtl2struct.m +++ b/src/utils/matlab/vtl/Vtl2struct.m @@ -49,17 +49,20 @@ vtlSolution.rtklibpvt=[]; [indRTKlib,~]= find(strcmp(dumpvtlfile, 'rtklib_state')); [indkf_err,~]= find(strcmp(dumpvtlfile, 'kf_xerr')); [ind_filt_dop_sat,~]= find(strcmp(dumpvtlfile, 'filt_dopp_sat')); +[ind_CN0_sat,~]= find(strcmp(dumpvtlfile, 'CN0_sat')); kfpvt=dumpvtlfile(indKF,:);kfpvt(:,1)=[]; rtklibpvt=dumpvtlfile(indRTKlib,:); rtklibpvt(:,1)=[]; kferr=dumpvtlfile(indkf_err,:); kferr(:,1)=[]; filt_dop_sat=dumpvtlfile(ind_filt_dop_sat,:); filt_dop_sat(:,1)=[]; +CN0_sat=dumpvtlfile(ind_CN0_sat,:); CN0_sat(:,1)=[]; kfpvt=cell2mat(kfpvt); rtklibpvt=cell2mat(rtklibpvt); kferr=cell2mat(kferr); filt_dop_sat=cell2mat(filt_dop_sat); +CN0_sat=cell2mat(CN0_sat); vtlSolution.kfpvt.X=kfpvt(:,1); vtlSolution.kfpvt.Y=kfpvt(:,2); @@ -94,4 +97,10 @@ vtlSolution.filt_dop_sat(3,:)=filt_dop_sat(:,3); vtlSolution.filt_dop_sat(4,:)=filt_dop_sat(:,4); vtlSolution.filt_dop_sat(5,:)=filt_dop_sat(:,5); +vtlSolution.CN0_sat(1,:)=CN0_sat(:,1); +vtlSolution.CN0_sat(2,:)=CN0_sat(:,2); +vtlSolution.CN0_sat(3,:)=CN0_sat(:,3); +vtlSolution.CN0_sat(4,:)=CN0_sat(:,4); +vtlSolution.CN0_sat(5,:)=CN0_sat(:,5); + end \ No newline at end of file From a882822dd9859106be437a86699fb7433bc620a7 Mon Sep 17 00:00:00 2001 From: miguekf Date: Wed, 4 Jan 2023 19:21:55 +0100 Subject: [PATCH 111/165] ADD: matlab file for CN0 sat plotting --- src/utils/matlab/vtl/CN0_sat_plotting.m | 65 +++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 src/utils/matlab/vtl/CN0_sat_plotting.m diff --git a/src/utils/matlab/vtl/CN0_sat_plotting.m b/src/utils/matlab/vtl/CN0_sat_plotting.m new file mode 100644 index 000000000..8101a3c80 --- /dev/null +++ b/src/utils/matlab/vtl/CN0_sat_plotting.m @@ -0,0 +1,65 @@ + + + +time_reference_spirent_obs=129780;%s +time_vtl_dump_file=linspace(38,157,length(vtlSolution.CN0_sat)); +%% +%-------------------------------------------------------- +Rx_Dopp_30=figure('Name','RX_Carrier_Doppler_hz'); +subplot(2,3,1); +xlim([0,160]); +xlabel('') +ylabel('CN0 sat (dB-Hz)') +xlabel('time from simulation init (seconds)') +grid on +hold on +title('PRN 30 GNSS-SDR') +plot(time_vtl_dump_file,vtlSolution.CN0_sat(1,:),'o','DisplayName','filtered VTL') +hold off;grid minor + +%-------------------------------------------------------- +subplot(2,3,2); +xlim([0,160]); +xlabel('') +ylabel('CN0 sat (dB-Hz)') +xlabel('time from simulation init (seconds)') +grid on +hold on +title('PRN 29 GNSS-SDR') +plot(time_vtl_dump_file,vtlSolution.CN0_sat(2,:),'o','DisplayName','filtered VTL') +hold off;grid minor +%-------------------------------------------------------- +subplot(2,3,3); +xlim([0,160]); +xlabel('') +ylabel('CN0 sat (dB-Hz)') +xlabel('time from simulation init (seconds)') +grid on +hold on +% legend('PRN 17 GNSS-SDR','Location','eastoutside') +title('PRN 24 GNSS-SDR') +plot(time_vtl_dump_file,vtlSolution.CN0_sat(3,:),'o','DisplayName','filtered VTL') +hold off;grid minor +%-------------------------------------------------------- +subplot(2,3,4); +xlim([0,160]); +xlabel('') +ylabel('CN0 sat (dB-Hz)') +xlabel('time from simulation init (seconds)') +grid on +hold on +title('PRN 12 GNSS-SDR') +plot(time_vtl_dump_file,vtlSolution.CN0_sat(4,:),'o','DisplayName','filtered VTL') +hold off;grid minor +%-------------------------------------------------------- + +subplot(2,3,5); +xlim([0,160]); +xlabel('') +ylabel('CN0 sat (dB-Hz)') +xlabel('time from simulation init (seconds)') +grid on +hold on +title('PRN 10 GNSS-SDR') +plot(time_vtl_dump_file,vtlSolution.CN0_sat(5,:),'o','DisplayName','filtered VTL') +hold off;grid minor \ No newline at end of file From 96b1dae7e6f7e1168636fc4adaeea9498917b830 Mon Sep 17 00:00:00 2001 From: miguekf Date: Wed, 4 Jan 2023 19:23:43 +0100 Subject: [PATCH 112/165] ADD: VTL sat CN0 dump to file --- src/algorithms/PVT/libs/vtl_engine.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index e9b3dce68..22a8e1d57 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -292,6 +292,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) << "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << endl; dump_vtl_file << "filt_dopp_sat" << "," << doppler_hz_filt(0) << "," << doppler_hz_filt(1) << "," << doppler_hz_filt(2) << "," << doppler_hz_filt(3) << "," << doppler_hz_filt(4) << endl; + dump_vtl_file << "CN0_sat" + << "," << new_data.sat_CN0_dB_hz(0) << "," << new_data.sat_CN0_dB_hz(1)<< "," << new_data.sat_CN0_dB_hz(2)<< "," << new_data.sat_CN0_dB_hz(3)<< "," << new_data.sat_CN0_dB_hz(4) << endl; dump_vtl_file.close(); } From ea240f0d7b65effb5a7d6c1d867bb147002a9a50 Mon Sep 17 00:00:00 2001 From: miguel Date: Thu, 5 Jan 2023 12:41:16 +0100 Subject: [PATCH 113/165] ADD: sat CN0 to PVT.mat output --- src/algorithms/PVT/libs/rtklib_solver.cc | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 49cf7e34a..a551687bb 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -205,7 +205,7 @@ bool Rtklib_Solver::save_matfile() const // READ DUMP FILE const std::string dump_filename = d_dump_filename; const int32_t number_of_double_vars = 23; //+2 MAGL - const int32_t number_of_double_vars_sat = 8; //+ pos/vel/psrange/doppler for sat + const int32_t number_of_double_vars_sat = 9; //+ pos(3)/vel(3)/psrange/doppler/CN0 for sat const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; @@ -266,6 +266,7 @@ bool Rtklib_Solver::save_matfile() const auto sat_velZ = std::vector>(5, std::vector(num_epoch)); auto sat_prg_m = std::vector>(5, std::vector(num_epoch)); auto sat_dopp_hz = std::vector>(5, std::vector(num_epoch)); + auto sat_CN0_dBhz = std::vector>(5, std::vector(num_epoch)); auto latitude = std::vector(num_epoch); auto longitude = std::vector(num_epoch); @@ -314,7 +315,8 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&sat_velY[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast(&sat_velZ[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast(&sat_prg_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_dopp_hz[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_dopp_hz[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&sat_CN0_dBhz[chan][i]), sizeof(double)); } dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); @@ -346,6 +348,7 @@ bool Rtklib_Solver::save_matfile() const auto sat_velZ_aux = std::vector(5 * num_epoch); auto sat_prg_m_aux = std::vector(5 * num_epoch); auto sat_dopp_hz_aux = std::vector(5 * num_epoch); + auto sat_CN0_dBhz_aux = std::vector(5 * num_epoch); uint32_t k = 0U; for (int64_t j = 0; j < num_epoch; j++) @@ -360,6 +363,7 @@ bool Rtklib_Solver::save_matfile() const sat_velZ_aux[k] = sat_velZ[i][j]; sat_prg_m_aux[k] = sat_prg_m[i][j]; sat_dopp_hz_aux[k] = sat_dopp_hz[i][j]; + sat_CN0_dBhz_aux[k] = sat_CN0_dBhz[i][j]; k++; } } @@ -471,6 +475,9 @@ bool Rtklib_Solver::save_matfile() const matvar = Mat_VarCreate("sat_dopp_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_dopp_hz_aux.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("sat_CN0_dBhz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_CN0_dBhz_aux.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); @@ -1428,7 +1435,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = vtl_data.pr_m(n); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = vtl_data.doppler_hz(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = vtl_data.sat_CN0_dB_hz(n); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } // GEO user position Latitude [deg] From 9b2a282b5ad5bb477d52cd48d373c417f0047ad4 Mon Sep 17 00:00:00 2001 From: miguel Date: Thu, 5 Jan 2023 12:43:06 +0100 Subject: [PATCH 114/165] REM: CN0 for vtl dump csv file --- src/algorithms/PVT/libs/vtl_engine.cc | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 22a8e1d57..cdedd5127 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -46,7 +46,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_F(1, 4) = kf_dt; kf_F(2, 5) = kf_dt; kf_F(6, 7) = kf_dt; - + kf_H = arma::zeros(2 * new_data.sat_number, 9); kf_y = arma::zeros(2 * new_data.sat_number, 1); kf_yerr = arma::zeros(2 * new_data.sat_number, 1); @@ -292,8 +292,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) << "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << endl; dump_vtl_file << "filt_dopp_sat" << "," << doppler_hz_filt(0) << "," << doppler_hz_filt(1) << "," << doppler_hz_filt(2) << "," << doppler_hz_filt(3) << "," << doppler_hz_filt(4) << endl; - dump_vtl_file << "CN0_sat" - << "," << new_data.sat_CN0_dB_hz(0) << "," << new_data.sat_CN0_dB_hz(1)<< "," << new_data.sat_CN0_dB_hz(2)<< "," << new_data.sat_CN0_dB_hz(3)<< "," << new_data.sat_CN0_dB_hz(4) << endl; dump_vtl_file.close(); } From 69b15cadc4b5283259ff9f844ee098e646001014 Mon Sep 17 00:00:00 2001 From: miguel Date: Thu, 5 Jan 2023 14:06:29 +0100 Subject: [PATCH 115/165] ADD: increase sat_n for PVT.mat dump --- src/algorithms/PVT/libs/rtklib_solver.cc | 46 ++++++++++++------------ 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index a551687bb..1694a7da2 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -210,7 +210,7 @@ bool Rtklib_Solver::save_matfile() const const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + - sizeof(double) * number_of_double_vars_sat*5+ + sizeof(double) * number_of_double_vars_sat*6+ sizeof(uint32_t) * number_of_uint32_vars + sizeof(uint8_t) * number_of_uint8_vars + sizeof(float) * number_of_float_vars; @@ -258,15 +258,15 @@ bool Rtklib_Solver::save_matfile() const auto clk_bias = std::vector(num_epoch); auto clk_drift = std::vector(num_epoch); - auto sat_posX_m = std::vector>(5, std::vector(num_epoch)); - auto sat_posY_m = std::vector>(5, std::vector(num_epoch)); - auto sat_posZ_m = std::vector>(5, std::vector(num_epoch)); - auto sat_velX = std::vector>(5, std::vector(num_epoch)); - auto sat_velY = std::vector>(5, std::vector(num_epoch)); - auto sat_velZ = std::vector>(5, std::vector(num_epoch)); - auto sat_prg_m = std::vector>(5, std::vector(num_epoch)); - auto sat_dopp_hz = std::vector>(5, std::vector(num_epoch)); - auto sat_CN0_dBhz = std::vector>(5, std::vector(num_epoch)); + auto sat_posX_m = std::vector>(6, std::vector(num_epoch)); + auto sat_posY_m = std::vector>(6, std::vector(num_epoch)); + auto sat_posZ_m = std::vector>(6, std::vector(num_epoch)); + auto sat_velX = std::vector>(6, std::vector(num_epoch)); + auto sat_velY = std::vector>(6, std::vector(num_epoch)); + auto sat_velZ = std::vector>(6, std::vector(num_epoch)); + auto sat_prg_m = std::vector>(6, std::vector(num_epoch)); + auto sat_dopp_hz = std::vector>(6, std::vector(num_epoch)); + auto sat_CN0_dBhz = std::vector>(6, std::vector(num_epoch)); auto latitude = std::vector(num_epoch); auto longitude = std::vector(num_epoch); @@ -306,7 +306,7 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&clk_bias[i]), sizeof(double)); dump_file.read(reinterpret_cast(&clk_drift[i]), sizeof(double)); - for (uint32_t chan = 0; chan < 5; chan++) + for (uint32_t chan = 0; chan < 6; chan++) { dump_file.read(reinterpret_cast(&sat_posX_m[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast(&sat_posY_m[chan][i]), sizeof(double)); @@ -340,20 +340,20 @@ bool Rtklib_Solver::save_matfile() const return false; } - auto sat_posX_m_aux = std::vector(5 * num_epoch); - auto sat_posY_m_aux = std::vector(5 * num_epoch); - auto sat_posZ_m_aux = std::vector(5 * num_epoch); - auto sat_velX_aux = std::vector(5 * num_epoch); - auto sat_velY_aux = std::vector(5 * num_epoch); - auto sat_velZ_aux = std::vector(5 * num_epoch); - auto sat_prg_m_aux = std::vector(5 * num_epoch); - auto sat_dopp_hz_aux = std::vector(5 * num_epoch); - auto sat_CN0_dBhz_aux = std::vector(5 * num_epoch); + auto sat_posX_m_aux = std::vector(6 * num_epoch); + auto sat_posY_m_aux = std::vector(6 * num_epoch); + auto sat_posZ_m_aux = std::vector(6 * num_epoch); + auto sat_velX_aux = std::vector(6 * num_epoch); + auto sat_velY_aux = std::vector(6 * num_epoch); + auto sat_velZ_aux = std::vector(6 * num_epoch); + auto sat_prg_m_aux = std::vector(6 * num_epoch); + auto sat_dopp_hz_aux = std::vector(6 * num_epoch); + auto sat_CN0_dBhz_aux = std::vector(6 * num_epoch); uint32_t k = 0U; for (int64_t j = 0; j < num_epoch; j++) { - for (uint32_t i = 0; i < 5; i++) + for (uint32_t i = 0; i < 6; i++) { sat_posX_m_aux[k] = sat_posX_m[i][j]; sat_posY_m_aux[k] = sat_posY_m[i][j]; @@ -450,7 +450,7 @@ bool Rtklib_Solver::save_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - std::array dims_sat{static_cast(5), static_cast(num_epoch)}; + std::array dims_sat{static_cast(6), static_cast(num_epoch)}; matvar = Mat_VarCreate("sat_posX_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posX_m_aux.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -1418,7 +1418,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = vtl_data.rx_dts(1); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - for (int n = 0; n<5; n++) + for (int n = 0; n<6; n++) { tmp_double = vtl_data.sat_p(n, 0); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); From e361f0d7e100850c338d5aad2b14ecb6fe647314 Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 5 Jan 2023 23:27:20 +0100 Subject: [PATCH 116/165] FIX: sat_CN0_dB_hz scale factor of 0.25 (steps of 0.25 dB-hz) --- src/algorithms/PVT/libs/rtklib_solver.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 1694a7da2..904e8e0a2 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1216,7 +1216,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; vtl_data.sat_var(n) = var[n]; vtl_data.sat_health_flag(n) = svh.at(n); - vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; + vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]*0.25; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) From bd013f9a8618b5d983fb7780ed18a50bef3cb7f4 Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 5 Jan 2023 23:31:17 +0100 Subject: [PATCH 117/165] FORMAT --- src/algorithms/PVT/libs/rtklib_solver.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 904e8e0a2..2cd0ab619 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1216,7 +1216,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; vtl_data.sat_var(n) = var[n]; vtl_data.sat_health_flag(n) = svh.at(n); - vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]*0.25; + vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0] * 0.25; //(0.25 dBHz) // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) From b314cbf5e78ddd0ee114307c43e9ab65970ce8c9 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 6 Jan 2023 17:46:50 +0000 Subject: [PATCH 118/165] ADD: Covariance update based on CN0 of the channel --- src/algorithms/PVT/libs/vtl_engine.cc | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index cdedd5127..67b6cd39a 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -94,9 +94,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_Q(0, 0) = 100.0; kf_Q(1, 1) = 100.0; kf_Q(2, 2) = 100.0; - kf_Q(3, 3) = 10.0; - kf_Q(4, 4) = 10.0; - kf_Q(5, 5) = 10.0; + kf_Q(3, 3) = 8.0; + kf_Q(4, 4) = 8.0; + kf_Q(5, 5) = 8.0; kf_Q(6, 6) = 100.0; kf_Q(7, 7) = 40.0; kf_Q(8, 8) = 1.0; @@ -105,8 +105,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) for (int32_t i = 0; i < new_data.sat_number; i++) { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 20.0; //TODO: fill with real values. - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 2.0; + kf_R(i, i) = 50.0*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 5.0*50.0/new_data.sat_CN0_dB_hz(i); + + if(50.0*50.0/new_data.sat_CN0_dB_hz(i)>70||5.0*50.0/new_data.sat_CN0_dB_hz(i)>7){ + kf_R(i, i) = 10e4; + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; + } } // Kalman state prediction (time update) From c423ef164ea2e7e03dc94e696a8dbfaaade9f654 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 6 Jan 2023 17:47:47 +0000 Subject: [PATCH 119/165] MOD: variable name in kf_tracking.cc --- src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 4ab28257d..a3ea5324b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -654,7 +654,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) arma::vec tmp_x = F_tmp * x_tmp; double old_doppler = d_x_old_old(2); - double old_doppler_shift = d_x_old_old(3); + double old_doppler_rate = d_x_old_old(3); double old_code_phase_chips = d_x_old_old(0); d_x_old_old(2) = tmp_x(2); //replace the Carrier Frequency state d_x_old_old(0) = tmp_x(0); //replace the Code Phase state @@ -687,7 +687,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) else { dump_tracking_file << "doppler_corr" - << ","<< this->d_channel << "," << x_tmp(2) << "," << old_doppler << "," << old_doppler_shift << "," << old_code_phase_chips << "\n"; + << ","<< this->d_channel << "," << x_tmp(2) << "," << old_doppler << "," << old_doppler_rate << "," << old_code_phase_chips << "\n"; dump_tracking_file.close(); } } From 50c7e151ab8960d57c7ead6864cd74ea1fbfbc14 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Tue, 7 Feb 2023 20:57:59 +0100 Subject: [PATCH 120/165] ADD: user acceleration tu VTL engine --- src/algorithms/PVT/libs/vtl_engine.cc | 102 +++++++++++------- src/algorithms/PVT/libs/vtl_engine.h | 3 + .../tracking/gnuradio_blocks/kf_tracking.cc | 15 ++- 3 files changed, 82 insertions(+), 38 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 67b6cd39a..ed05850c2 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -32,34 +32,40 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; - + + // ################## Kalman filter initialization ###################################### // covariances (static) - kf_P_x = arma::eye(9, 9) * 1.0; //TODO: use a real value. - kf_x = arma::zeros(9, 1); + kf_P_x = arma::eye(12, 12) * 1.0; //TODO: use a real value. + kf_x = arma::zeros(12, 1); kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); - double kf_dt = 0.1; - kf_Q = arma::eye(9, 9); + double kf_dt = 0.05; + kf_Q = arma::eye(12, 12); - kf_F = arma::eye(9, 9); + kf_F = arma::eye(12, 12); kf_F(0, 3) = kf_dt; kf_F(1, 4) = kf_dt; kf_F(2, 5) = kf_dt; - kf_F(6, 7) = kf_dt; + + kf_F(3, 6) = kf_dt; + kf_F(4, 7) = kf_dt; + kf_F(5, 8) = kf_dt; - kf_H = arma::zeros(2 * new_data.sat_number, 9); + kf_F(9, 10) = kf_dt; + kf_F(10, 11) = kf_dt; + + kf_H = arma::zeros(2 * new_data.sat_number, 12); kf_y = arma::zeros(2 * new_data.sat_number, 1); kf_yerr = arma::zeros(2 * new_data.sat_number, 1); - kf_xerr = arma::zeros(9, 1); + kf_xerr = arma::zeros(12, 1); kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix // ################## Kalman Tracking ###################################### static uint32_t counter = 0; //counter counter = counter + 1; //uint64_t - cout << "counter" << counter << endl; //new_data.kf_state.print("new_data kf initial"); uint32_t closure_point=3; - + if (counter < closure_point) { // receiver solution from rtklib_solver @@ -69,9 +75,10 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(3) = new_data.rx_v(0); kf_x(4) = new_data.rx_v(1); kf_x(5) = new_data.rx_v(2); - kf_x(6) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; - kf_x(7) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; - kf_x(8) = 1.0; + + kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; + kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; + kf_x(11) = 1.0; kf_x = kf_F * kf_x; // state prediction } @@ -87,8 +94,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(6) = new_data.kf_state[6]; kf_x(7) = new_data.kf_state[7]; kf_x(8) = new_data.kf_state[8]; + kf_x(9) = new_data.kf_state[9]; + kf_x(10) = new_data.kf_state[10]; + kf_x(11) = new_data.kf_state[11]; kf_P_x = new_data.kf_P; } + // State error Covariance Matrix Q (PVT) //careful, values for V and T could not be adecuate. kf_Q(0, 0) = 100.0; @@ -97,10 +108,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_Q(3, 3) = 8.0; kf_Q(4, 4) = 8.0; kf_Q(5, 5) = 8.0; - kf_Q(6, 6) = 100.0; - kf_Q(7, 7) = 40.0; - kf_Q(8, 8) = 1.0; - + kf_Q(6, 6) = .1; + kf_Q(7, 7) = .1; + kf_Q(8, 8) = .1; + kf_Q(9, 9) = 100.0; + kf_Q(10, 10) = 40.0; + kf_Q(11, 11) = 1.0; + // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) { @@ -127,8 +141,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) xDot_u = kf_x(3); yDot_u = kf_x(4); zDot_u = kf_x(5); - cdeltat_u = kf_x(6); - cdeltatDot_u = kf_x(7); + xDot2_u = kf_x(6); + yDot2_u = kf_x(7); + zDot2_u = kf_x(8); + cdeltat_u = kf_x(9); + cdeltatDot_u = kf_x(10); d = arma::zeros(new_data.sat_number, 1); rho_pri = arma::zeros(new_data.sat_number, 1); @@ -160,21 +177,27 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) new_data.sat_LOS(i, 2) = a_z(i); //compute pseudorange rate estimation rhoDot_pri(i) = (new_data.sat_v(i, 0) - xDot_u) * a_x(i) + (new_data.sat_v(i, 1) - yDot_u) * a_y(i) + (new_data.sat_v(i, 2) - zDot_u) * a_z(i); + rhoDot_pri(i) = rhoDot_pri(i) + a_x(i)*xDot2_u*kf_dt+a_y(i)*yDot2_u*kf_dt+a_z(i)*zDot2_u*kf_dt;; } + // cout<50){ + std::cout << " tracking_cmd TOO FAR: " + << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + << " \n"; + }else{ + std::cout << " tracking_cmd NEAR: " + << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + << " \n"; + //d_x_old_old(2) = tmp_x(2); //replace the Code Phase state + } + + //d_x_old_old(0) = tmp_x(0); //replace the Code Phase state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; From dec07c9c0c0a8a0943f6e0dd85feefafbf522cbb Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 9 Feb 2023 00:25:05 +0100 Subject: [PATCH 121/165] MOD: matlab files update --- src/utils/matlab/vtl/doppler_vtl_plot.m | 35 +++ src/utils/matlab/vtl/kf_prototype_model.m | 280 ++++++++++++++++++++++ src/utils/matlab/vtl/pr_m_vtl_plot.m | 49 ++++ src/utils/matlab/vtl/vtl_general_plot.m | 148 +++--------- src/utils/matlab/vtl/vtl_prototype.m | 99 -------- src/utils/matlab/vtl/vtl_tracking.m | 80 ------- 6 files changed, 401 insertions(+), 290 deletions(-) create mode 100644 src/utils/matlab/vtl/doppler_vtl_plot.m create mode 100644 src/utils/matlab/vtl/kf_prototype_model.m create mode 100644 src/utils/matlab/vtl/pr_m_vtl_plot.m delete mode 100644 src/utils/matlab/vtl/vtl_prototype.m delete mode 100644 src/utils/matlab/vtl/vtl_tracking.m diff --git a/src/utils/matlab/vtl/doppler_vtl_plot.m b/src/utils/matlab/vtl/doppler_vtl_plot.m new file mode 100644 index 000000000..bf175bd6c --- /dev/null +++ b/src/utils/matlab/vtl/doppler_vtl_plot.m @@ -0,0 +1,35 @@ +%% LOAD + + Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),channels); + + for i=1:length(refSatData.GPS.SIM_time) + Carrier_Doppler_hz_sim(i,1)=refSatData.GPS.series(i).doppler_shift(4);%PRN 28 + Carrier_Doppler_hz_sim(i,2)=refSatData.GPS.series(i).doppler_shift(3);%PRN 20 + Carrier_Doppler_hz_sim(i,3)=refSatData.GPS.series(i).doppler_shift(8);%PRN 17 + Carrier_Doppler_hz_sim(i,4)=refSatData.GPS.series(i).doppler_shift(6);%PRN 12 + Carrier_Doppler_hz_sim(i,5)=refSatData.GPS.series(i).doppler_shift(5);%PRN 9 + Carrier_Doppler_hz_sim(i,6)=refSatData.GPS.series(i).doppler_shift(2);%PRN 5 + Carrier_Doppler_hz_sim(i,7)=refSatData.GPS.series(i).doppler_shift(1);%PRN 4 + Carrier_Doppler_hz_sim(i,8)=refSatData.GPS.series(i).doppler_shift(7);%PRN 2 + end + +%% + + figure('Name','RX_Carrier_Doppler_hz'); +for channel_cnt=1:channels + subplot(3,3,channel_cnt) + plot(linspace(0,tFinal,length(Carrier_Doppler_hz(channel_cnt,:)')), Carrier_Doppler_hz(channel_cnt,:)','s') + xlim([0,tFinal]); + ylim([min(Carrier_Doppler_hz_sim(:,channel_cnt+1)),max(Carrier_Doppler_hz_sim(:,channel_cnt+1))]); + xlabel('') + ylabel('Doppler (Hz)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend(['PRN' num2str(rx_PRN(channel_cnt)) ' GNSS-SDR'],'Location','eastoutside')% bench + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,channel_cnt+1)','.','DisplayName','SPIRENT reference') + plot(linspace(TTFF_sec+23,tFinal,length(sat_dopp_hz_filt(channel_cnt,:))),sat_dopp_hz_filt(channel_cnt,:),'o','DisplayName','filtered VTL') + hold off;grid minor +end + + diff --git a/src/utils/matlab/vtl/kf_prototype_model.m b/src/utils/matlab/vtl/kf_prototype_model.m new file mode 100644 index 000000000..602665755 --- /dev/null +++ b/src/utils/matlab/vtl/kf_prototype_model.m @@ -0,0 +1,280 @@ +%% vtl KF +load rocket_model_dynamics.mat +%% +sat_number=6; % limit by csv? +%% ################## Kalman filter initialization ###################################### +state_number=9; +% covariances (static) +kf_P_x = eye(state_number); %TODO: use a real value. +kf_x = zeros(state_number, 1); +kf_R = zeros(2*sat_number, 2*sat_number); +kf_dt=0.05; +% kf_F = eye(state_number, state_number); +% kf_F(1, 4) = kf_dt; +% kf_F(2, 5) = kf_dt; +% kf_F(3, 6) = kf_dt; +% kf_F(7, 8) = kf_dt; + +kf_F=[ 1 0 0 kf_dt 0 0 0 0 0 + 0 1 0 0 kf_dt 0 0 0 0 + 0 0 1 0 0 kf_dt 0 0 0 + 0 0 0 1 0 0 0 0 0 + 0 0 0 0 1 0 0 0 0 + 0 0 0 0 0 1 0 0 0 + 0 0 0 0 0 0 1 kf_dt kf_dt^2/2 + 0 0 0 0 0 0 0 1 kf_dt + 0 0 0 0 0 0 0 0 1]; + +% kf_F=[ 1 0 0 kf_dt 0 0 0 0 +% 0 1 0 0 kf_dt 0 0 0 +% 0 0 1 0 0 kf_dt 0 0 +% 0 0 0 1 0 0 0 0 +% 0 0 0 0 1 0 0 0 +% 0 0 0 0 0 1 0 0 +% 0 0 0 0 0 0 1 kf_dt +% 0 0 0 0 0 0 0 1]; +% +kf_H = zeros(2*sat_number,state_number); +kf_y = zeros(2*sat_number, 1); +kf_yerr = zeros(2*sat_number, 1); +% kf_xerr = zeros(8, 1); +kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix + +%% pre-allocate for speed +d = zeros(sat_number, 1); +rho_pri = zeros(sat_number, 1); +rhoDot_pri = zeros(sat_number, 1); +a_x = zeros(sat_number, 1); +a_y = zeros(sat_number, 1); +a_z = zeros(sat_number, 1); +c_pr_m=zeros(sat_number,length(navSolution.RX_time)); + +pr_m_filt=zeros(sat_number,length(navSolution.RX_time)); +rhoDot_pri_filt=zeros(sat_number,length(navSolution.RX_time)); +sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time)); + +%% ################## Kalman Tracking ###################################### +% receiver solution from rtklib_solver +index=0; +modelflag=0; +t_from_lunch=0; +for t=2:length(navSolution.RX_time) + + index=index+1; + %% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS) + if (index < point_of_closure) + %% State error Covariance Matrix Q (PVT) + kf_Q = blkdiag(posx_var,posy_var,posz_var,velx_var,vely_var,velz_var,clk_bias_var,clk_drift_var,clk_d_drift_var); + %% + + % Measurement error Covariance Matrix R assembling + kf_R = blkdiag(eye(sat_number)*pr_var,eye(sat_number)*pr_dot_var); + + else + kf_Q = blkdiag(posx_var,posy_var,posz_var,velx_var,vely_var,velz_var,clk_bias_var,clk_drift_var,clk_d_drift_var); + for chan=1:sat_number + kf_R(chan,chan) = pr_var*sat_CN0_dBhz(chan,t)/200; + kf_R(chan+sat_number,chan+sat_number)= pr_dot_var*sat_CN0_dBhz(chan,t)/200; + end + end + + if(~modelflag) + clear x_u y_u z_u xDot_u yDot_u zDot_u cdeltatDot_u cdeltatDot_u_g cdeltat_u_g + if (index < point_of_closure) + disp('one step') + kf_x(1,t) = navSolution.X(t); + kf_x(2,t) = navSolution.Y(t); + kf_x(3,t) = navSolution.Z(t); + kf_x(4,t) = navSolution.vX(t); + kf_x(5,t) = navSolution.vY(t); + kf_x(6,t) = navSolution.vZ(t); + kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S; + kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1); + kf_x(9,t) = 1.0; + + x_u=kf_x(1,t); + y_u=kf_x(2,t); + z_u=kf_x(3,t); + xDot_u=kf_x(4,t); + yDot_u=kf_x(5,t); + zDot_u=kf_x(6,t); + cdeltat_u(t)=kf_x(7,t); + cdeltatDot_u=kf_x(8,t); + + % Kalman state prediction (time update) + kf_P_x = eye(state_number)*100; %TODO: use a real value. + kf_xpri(:,t) = kf_F * (kf_x(:,t)-kf_x(:,t-1));% state prediction + kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction + else + disp('feedback') + x_u=corr_kf_state(1,t-1); + y_u=corr_kf_state(2,t-1); + z_u=corr_kf_state(3,t-1); + xDot_u=corr_kf_state(4,t-1); + yDot_u=corr_kf_state(5,t-1); + zDot_u=corr_kf_state(6,t-1); + cdeltat_u(t)=corr_kf_state(7,t-1); + cdeltatDot_u=corr_kf_state(8,t-1); + + % Kalman state prediction (time update) + kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction + end + else + disp('MODEL') + t_from_lunch=t_from_lunch+3; + if(t_from_lunch>length(rocket_model_dynamics.vX)) + t_from_lunch=length(rocket_model_dynamics.vX); + end + kf_x(1,t) = rocket_model_dynamics.X(t_from_lunch); + kf_x(2,t) = rocket_model_dynamics.Y(t_from_lunch); + kf_x(3,t) = rocket_model_dynamics.Z(t_from_lunch); + kf_x(4,t) = rocket_model_dynamics.vX(t_from_lunch); + kf_x(5,t) = rocket_model_dynamics.vY(t_from_lunch); + kf_x(6,t) = rocket_model_dynamics.vZ(t_from_lunch); + kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S; + kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1); + kf_x(9,t) = 1.0; + + x_u=kf_x(1,t); + y_u=kf_x(2,t); + z_u=kf_x(3,t); + xDot_u=kf_x(4,t); + yDot_u=kf_x(5,t); + zDot_u=kf_x(6,t); + cdeltat_u(t)=kf_x(7,t); + cdeltatDot_u=kf_x(8,t); + + % Kalman state prediction (time update) + kf_P_x = eye(state_number)*1; %TODO: use a real value. + kf_xpri(:,t) = kf_F * (kf_x(:,t)-kf_x(:,t-1));% state prediction + kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction + end + + for chan=1:sat_number %neccesary quantities + d(chan)=(sat_posX_m(chan,t)-x_u)^2; + d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2; + d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2; + d(chan)=sqrt(d(chan)); + + c_pr_m(chan,t)=d(chan)+cdeltat_u(t); + + a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan); + a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan); + a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan); + + rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... + +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... + +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); + end + + + for chan=1:sat_number % Measurement matrix H assembling + % It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) + kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0; + kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0; + end + + % unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H)); + % !!!! Limitaciones + % obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad + % no se recomienda para las pruebas de observabilidad. Ob será numéricamente singular para la mayoría de los + % sistemas con más de unos cuantos estados. Este hecho está bien documentado en la sección III de [1]. + + % Kalman stimation (measurement update) + for chan=1:sat_number % Measurement matrix H assembling + kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t); + if (t100)>1) + modelflag=1; +end + %% ################## Geometric Transformation ###################################### + + + x_u=corr_kf_state(1,t); + y_u=corr_kf_state(2,t); + z_u=corr_kf_state(3,t); + xDot_u=corr_kf_state(4,t); + yDot_u=corr_kf_state(5,t); + zDot_u=corr_kf_state(6,t); + cdeltat_u_g=corr_kf_state(7,t); + cdeltatDot_u_g=corr_kf_state(8,t); + + for chan=1:sat_number %neccesary quantities + d(chan)=(sat_posX_m(chan,t)-x_u)^2; + d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2; + d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2; + d(chan)=sqrt(d(chan)); + + c_pr_m(chan,t)=d(chan)+cdeltat_u_g; + + a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan); + a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan); + a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan); + + rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)... + +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)... + +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); + end + + kf_H = zeros(2*sat_number,state_number); + + for chan=1:sat_number % Measurement matrix H assembling + % It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) + kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0; + kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0; + end + + % Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x + kf_yerr_g=kf_H*kf_xerr; + disp("kf_yerr_g:") + disp(kf_yerr_g(:,t)) + disp('zDot_u:') + disp(zDot_u) + % Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): + for chan=1:sat_number % Measurement vector + pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered + rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t); + %convert rhoDot_pri to doppler shift! + % d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1)); + % d_dt_clk_drift=0; + sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t))/Lambda_GPS_L1; + err_carrier_phase_rads_filt(chan,t) = trapz(kf_yerr_g(chan+sat_number,1:t)/Lambda_GPS_L1)*2*kf_dt; + carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t); + % carrier_freq_rate_hz_s = 0; + err_code_phase_chips(chan,t) = (kf_yerr_g(chan,t))/SPEED_OF_LIGHT_M_S*1023e3; + end + +end \ No newline at end of file diff --git a/src/utils/matlab/vtl/pr_m_vtl_plot.m b/src/utils/matlab/vtl/pr_m_vtl_plot.m new file mode 100644 index 000000000..da96bb1a6 --- /dev/null +++ b/src/utils/matlab/vtl/pr_m_vtl_plot.m @@ -0,0 +1,49 @@ +%% LOAD + + Pseudorange_m_sim=zeros(length(refSatData.GPS.SIM_time),channels); + + for i=1:length(refSatData.GPS.SIM_time) + Pseudorange_m_sim(i,1)=refSatData.GPS.series(i).pr_m(4);%PRN 28 + Pseudorange_m_sim(i,2)=refSatData.GPS.series(i).pr_m(3);%PRN 20 + Pseudorange_m_sim(i,3)=refSatData.GPS.series(i).pr_m(8);%PRN 17 + Pseudorange_m_sim(i,4)=refSatData.GPS.series(i).pr_m(6);%PRN 12 + Pseudorange_m_sim(i,5)=refSatData.GPS.series(i).pr_m(5);%PRN 9 + Pseudorange_m_sim(i,6)=refSatData.GPS.series(i).pr_m(2);%PRN 5 + Pseudorange_m_sim(i,7)=refSatData.GPS.series(i).pr_m(1);%PRN 4 + Pseudorange_m_sim(i,8)=refSatData.GPS.series(i).pr_m(7);%PRN 2 + end +%% + + figure('Name','RX_PseudoRange_m'); +for channel_cnt=1:channels + subplot(3,3,channel_cnt) + plot(linspace(0,tFinal,length(Pseudorange_m(channel_cnt,:)')), Pseudorange_m(channel_cnt,:)','s') + xlim([0,tFinal]); +% ylim([min(Pseudorange_m_sim(:,channel_cnt)),max(Pseudorange_m_sim(:,channel_cnt))]); + xlabel('') + ylabel('PR [m] (m)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend(['PRN' num2str(rx_PRN(channel_cnt)) ' GNSS-SDR'],'Location','eastoutside')% bench +% plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,channel_cnt)','.','DisplayName','SPIRENT reference') +% plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL') + hold off;grid minor +end +%% + figure('Name','Rsim_PseudoRange_m'); +for channel_cnt=1:channels + subplot(3,3,channel_cnt) + plot(linspace(0,tFinal,length(Pseudorange_m(channel_cnt,:)')), Pseudorange_m(channel_cnt,:)','s') + xlim([0,tFinal]); + ylim([min(Pseudorange_m_sim(:,channel_cnt)),max(Pseudorange_m_sim(:,channel_cnt))]); + xlabel('') + ylabel('PR [m] (m)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend(['PRN' num2str(rx_PRN(channel_cnt)) ' GNSS-SDR'],'Location','eastoutside')% bench + plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,channel_cnt)','.','DisplayName','SPIRENT reference') +% plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL') + hold off;grid minor +end \ No newline at end of file diff --git a/src/utils/matlab/vtl/vtl_general_plot.m b/src/utils/matlab/vtl/vtl_general_plot.m index b6f964ae1..9004ecb75 100644 --- a/src/utils/matlab/vtl/vtl_general_plot.m +++ b/src/utils/matlab/vtl/vtl_general_plot.m @@ -1,28 +1,17 @@ %% % vtl_general_plot.m %% -time_reference_spirent_obs=129780;%s - -if(load_observables) - - Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),6); - - for i=1:length(refSatData.GPS.SIM_time) - Carrier_Doppler_hz_sim(i,1)=refSatData.GPS.series(i).doppler_shift(4);%PRN 28 - Carrier_Doppler_hz_sim(i,2)=refSatData.GPS.series(i).doppler_shift(1);%PRN 4 - Carrier_Doppler_hz_sim(i,3)=refSatData.GPS.series(i).doppler_shift(6);%PRN 17 - Carrier_Doppler_hz_sim(i,4)=refSatData.GPS.series(i).doppler_shift(7);%PRN 15 - Carrier_Doppler_hz_sim(i,5)=refSatData.GPS.series(i).doppler_shift(8);%PRN 27 - Carrier_Doppler_hz_sim(i,6)=refSatData.GPS.series(i).doppler_shift(9);%PRN 9 - - Pseudorange_m_sim(i,1)=refSatData.GPS.series(i).pr_m(4);%PRN 28 - Pseudorange_m_sim(i,2)=refSatData.GPS.series(i).pr_m(1);%PRN 4 - Pseudorange_m_sim(i,3)=refSatData.GPS.series(i).pr_m(6);%PRN 17 - Pseudorange_m_sim(i,4)=refSatData.GPS.series(i).pr_m(7);%PRN 15 - Pseudorange_m_sim(i,5)=refSatData.GPS.series(i).pr_m(8);%PRN 27 - Pseudorange_m_sim(i,6)=refSatData.GPS.series(i).pr_m(9);%PRN 9 - end -end +% +% if(load_observables) +% for i=1:length(refSatData.GPS.SIM_time) +% Pseudorange_m_sim(i,1)=refSatData.GPS.series(i).pr_m(9);%PRN 30 +% Pseudorange_m_sim(i,2)=refSatData.GPS.series(i).pr_m(3);%PRN 29 +% Pseudorange_m_sim(i,3)=refSatData.GPS.series(i).pr_m(5);%PRN 24 +% Pseudorange_m_sim(i,4)=refSatData.GPS.series(i).pr_m(2);%PRN 12 +% Pseudorange_m_sim(i,5)=refSatData.GPS.series(i).pr_m(8);%PRN 10 +% Pseudorange_m_sim(i,6)=refSatData.GPS.series(i).pr_m(6);%PRN 5 +% end +% end % ------------------------------------- % if(load_observables) % Rx_pseudo_all=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m','s') @@ -51,7 +40,7 @@ end % hold off % grid on % end -%---VTL VELOCITY: GNSS SDR plot -------------------------------------- +%% ---VTL VELOCITY: GNSS SDR plot -------------------------------------- VTL_VEL=figure('Name','velocities'); subplot(2,2,1); plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vX,'.'); @@ -62,7 +51,7 @@ plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vX... % plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(4,:),'.'); ylabel('vX (m/s)') xlabel('time U.A.') -ylim([-5 5]) +% ylim([-5 5]) title('Subplot 1: vX ') legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside') @@ -75,7 +64,7 @@ plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vY... ,'.','DisplayName','SPIRENT reference') ylabel('vY (m/s)') xlabel('time U.A.') -ylim([-5 5]) +% ylim([-5 5]) title('Subplot 1: vY ') legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside') @@ -88,7 +77,7 @@ plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vZ... ,'.','DisplayName','SPIRENT reference') ylabel('vZ (m/s)') xlabel('time U.A.') -ylim([-5 5]) +% ylim([-5 5]) title('Subplot 1: vZ ') legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside') @@ -96,7 +85,7 @@ sgtitle('velocities') %% --- VTL UTM centered POSITION: GNSS SDR plot -------------------------------------- -VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION'); +VTL_POS=figure('Name','VTL UTM COORD '); subplot(2,2,1); plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X,'.'); hold on;grid on @@ -131,84 +120,10 @@ title('Subplot 1: Z ') legend ('raw navSolution','raw kf state','SPIRENT ref','Location','eastoutside') sgtitle('VTL UTM COORD') -%% -if(load_observables) - % Rx_Dopp_all=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz','s') - % xlim([0,140]); - % xlabel('') - % ylabel('Doppler (Hz)') - % xlabel('time from simulation init (seconds)') - % grid on - % hold on - % plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim','.') - % plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt,'o') - % legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside') - % hold off - % grid on - Rx_Dopp_28=figure('Name','RX_Carrier_Doppler_hz'); - subplot(2,2,1) - plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','s') - xlim([0,140]); - ylim([-2340,-2220]); - xlabel('') - ylabel('Doppler (Hz)') - xlabel('time from simulation init (seconds)') - grid on - hold on - legend('PRN 28 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','SPIRENT reference') - plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL') - hold off;grid minor - - % Rx_Dopp_4=figure('Name','RX_Carrier_Doppler_hz'); - subplot(2,2,2) - plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(2,:)','s') - xlim([0,140]); - ylim([2540,2640]); - xlabel('') - ylabel('Doppler (Hz)') - xlabel('time from simulation init (seconds)') - grid on - hold on - legend('PRN 4 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,2)','.','DisplayName','SPIRENT reference') - plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(2,:),'o','DisplayName','filtered VTL') - hold off;grid minor - - % Rx_Dopp_17=figure('Name','RX_Carrier_Doppler_hz'); - subplot(2,2,3) - plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(3,:)','s') - xlim([0,140]); - ylim([-1800,-1730]); - xlabel('') - ylabel('Doppler (Hz)') - xlabel('time from simulation init (seconds)') - grid on - hold on - legend('PRN 17 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,3)','.','DisplayName','SPIRENT reference') - plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(3,:),'o','DisplayName','filtered VTL') - hold off;grid minor - - % Rx_Dopp_15=figure('Name','RX_Carrier_Doppler_hz'); - subplot(2,2,4) - plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(4,:)','s') - xlim([0,140]); - ylim([-2680,-2620]); - xlabel('') - ylabel('Doppler (Hz)') - xlabel('time from simulation init (seconds)') - grid on - hold on - legend('PRN 15 GNSS-SDR','Location','eastoutside') - plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,4)','.','DisplayName','SPIRENT reference') - plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(4,:),'o','DisplayName','filtered VTL') - hold off;grid minor -end %% STATE PLOT VTL_STATE=figure('Name','VTL STATE'); -subplot(2,2,1); +subplot(2,3,1); plot(navSolution.RX_time-navSolution.RX_time(1),[navSolution.X-navSolution.X(1);... navSolution.Y-navSolution.Y(1) ;navSolution.Z-navSolution.Z(1)],... 'b.','DisplayName','RTKLIB solution'); @@ -219,13 +134,13 @@ plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.X-refSolution.X(spirent_ind refSolution.Y-refSolution.Y(spirent_index_TTFF) refSolution.Z-refSolution.Z(spirent_index_TTFF)],... 'r.','DisplayName','SPIRENT reference'); legend('Location','eastoutside'); -ylim([-200,200]) -xlim([0,120]) +% ylim([-200,200]) +xlim([0,tFinal]) ylabel('X Y Z (m)') xlabel('time [s]') title('Subplot 1: POSITION [m]') -subplot(2,2,2); +subplot(2,3,2); plot(navSolution.RX_time-navSolution.RX_time(1),[navSolution.vX;... navSolution.vY; navSolution.vZ],... 'b.','DisplayName','RTKLIB solution'); @@ -236,30 +151,41 @@ plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.vX... refSolution.vY refSolution.vZ],... 'r.','DisplayName','SPIRENT reference'); -xlim([0,120]) +xlim([0,tFinal]) ylabel('vX vY vZ (m/s)') xlabel('time [s]') title('Subplot 1: VELOCITIES [m/s]') -subplot(2,2,3); +subplot(2,3,3); plot(navSolution.RX_time-navSolution.RX_time(1),clk_bias_s*SPEED_OF_LIGHT_M_S,... 'b.','DisplayName','RTKLIB solution'); hold on;grid on plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(7,:),... 'k.','DisplayName','filt VTL'); -ylim([3019190, 3019700]) -xlim([0,120]) +% ylim([3633390, 3634580]) +xlim([0,tFinal]) ylabel('clk bias (m)') xlabel('time [s]') title('Subplot 1: clk bias [m]') -subplot(2,2,4); +subplot(2,3,4); plot(navSolution.RX_time-navSolution.RX_time(1),clk_drift*SPEED_OF_LIGHT_M_S,... 'b.','DisplayName','RTKLIB solution'); hold on;grid on plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(8,:),... 'k.','DisplayName','filt VTL'); -xlim([0,120]) +xlim([0,tFinal]) +ylabel('clk drift (m/s)') +xlabel('time [s]') +title('Subplot 1: clk drift [m/s]') + +subplot(2,3,5); +plot(navSolution.RX_time(1:end-1)-navSolution.RX_time(1),diff(clk_drift)/kf_dt*SPEED_OF_LIGHT_M_S,... + 'b.','DisplayName','RTKLIB solution'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(9,:),... + 'k.','DisplayName','filt VTL'); +xlim([0,tFinal]) ylabel('clk drift (m/s)') xlabel('time [s]') title('Subplot 1: clk drift [m/s]') diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m deleted file mode 100644 index c3abe3392..000000000 --- a/src/utils/matlab/vtl/vtl_prototype.m +++ /dev/null @@ -1,99 +0,0 @@ -% VTL prototype -% ------------------------------------------------------------------------- -% -% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. -% This file is part of GNSS-SDR. -% -% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) -% SPDX-License-Identifier: GPL-3.0-or-later -% -% ------------------------------------------------------------------------- -% -%% -clc -close all -clearvars - -% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file') -% addpath('./libs') -% end -% -% if ~exist('cat2geo.m', 'file') -% addpath('./libs/geoFunctions') -% end -SPEED_OF_LIGHT_M_S=299792458.0; -Lambda_GPS_L1=0.1902937; -GPS_L1_freq_hz=1575.42e6; -point_of_closure=4060; - -%% ==================== VARIANCES ============================= -pos_var=100;%m^2 -vel_var=10;%m^2/s^2 -clk_bias_var=40;%m^2 -clk_drift_var=1500;%m^2/s^2 -pr_var=20;%m^2 -pr_dot_var=2;%m^2/s^2 - -% CARLES PAPER LTE GNSS VTL -% pos_var=2;%m^2 -% vel_var=0.2;%m^2/s^2 -% clk_bias_var=1e-7;%m^2 -% clk_drift_var=1e-4;%m^2/s^2 -% pr_var=20;%m^2 -% pr_dot_var=3;%m^2/s^2 -%% ============================================================ -samplingFreq=5000000; -channels=6; -TTFF_sec=41.48; -spirent_index_TTFF=416; - -plot_skyplot=0; -plot_reference=1; -load_observables=1; - -%% ============================ PARSER TO STRUCT ============================ - -navSolution = GnssSDR2struct('PVT_raw.mat'); -refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv'); -% -load observables\observables_raw.mat -% refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); -rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05. -load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'... - ,'sat_velZ','sat_prg_m','clk_bias_s','clk_drift','sat_dopp_hz','user_clk_offset') - -if(load_observables) - load observables\observables_raw.mat - refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); -end -%% -% vtlSolution = Vtl2struct('dump_vtl_file.csv'); -%% - -kf_prototype - -%% ====== FILTERING ======================================================= -% moving_avg_factor= 500; -% LAT_FILT = movmean(navSolution.latitude,moving_avg_factor); -% LON_FILT = movmean(navSolution.longitude,moving_avg_factor); -% HEIGH_FILT = movmean(navSolution.height,moving_avg_factor); -% -% X_FILT = movmean(navSolution.X,moving_avg_factor); -% Y_FILT = movmean(navSolution.Y,moving_avg_factor); -% Z_FILT = movmean(navSolution.Z,moving_avg_factor); -% -% vX_FILT = movmean(navSolution.vX,moving_avg_factor); -% vY_FILT = movmean(navSolution.vY,moving_avg_factor); -% vZ_FILT = movmean(navSolution.vZ,moving_avg_factor); -% -%% -%general_raw_plot -vtl_general_plot - -%% ============================================== ============================================== -%% -figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_yerr(1:5,:)');title('c pr m-error');xlabel('t U.A');ylabel('pr m [m]');grid minor -legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside') -figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_yerr(6:10,:)');title('c pr m DOT-error');xlabel('t U.A');ylabel('pr m dot [m/s]');grid minor -legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside') - diff --git a/src/utils/matlab/vtl/vtl_tracking.m b/src/utils/matlab/vtl/vtl_tracking.m deleted file mode 100644 index c78a70db6..000000000 --- a/src/utils/matlab/vtl/vtl_tracking.m +++ /dev/null @@ -1,80 +0,0 @@ -% ------------------------------------------------------------------------- -% -% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. -% This file is part of GNSS-SDR. -% -% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) -% SPDX-License-Identifier: GPL-3.0-or-later -% -% ------------------------------------------------------------------------- -% -%% -clc -close all -clearvars - -% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file') -% addpath('./libs') -% end -% -% if ~exist('cat2geo.m', 'file') -% addpath('./libs/geoFunctions') -% end -SPEED_OF_LIGHT_M_S=299792458.0; -Lambda_GPS_L1=0.1902937; -%% -trkSolution=trk2struct('dump_trk_file.csv'); - -%% split by solution type -figure;sgtitle('real doppler') -for chan=0:4 -eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.real==chan);']) -eval(['Dopp_real_CH' num2str(chan) '=trkSolution.dopp.real(indCH' num2str(chan) ',2);']) -eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_real_CH' num2str(chan) ')']) -end -figure;sgtitle('cmd doppler') -for chan=0:4 -eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.cmd==chan);']) -eval(['Dopp_cmd_CH' num2str(chan) '=trkSolution.dopp.cmd(indCH' num2str(chan) ',2);']) -eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_cmd_CH' num2str(chan) ')']) -end - -%% -% for chan=0:4 -% load(['tracking\tracking_raw' num2str(chan) '.mat']); -% tracking_channel(chan+1).PRN=PRN; -% tracking_channel(chan+1).CN0_SNV_dB_Hz=CN0_SNV_dB_Hz; -% tracking_channel(chan+1).carrier_doppler_hz=carrier_doppler_hz; -% tracking_channel(chan+1).carrier_doppler_rate_hz=carrier_doppler_rate_hz; -% tracking_channel(chan+1).code_error_chips=code_error_chips; -% tracking_channel(chan+1).code_freq_chips=code_freq_chips; -% tracking_channel(chan+1).code_freq_rate_chips=code_freq_rate_chips; -% tracking_channel(chan+1).acc_carrier_phase_rad=acc_carrier_phase_rad; -% -% clearvars -except tracking_channel chan -% end -% %% -% figure -% for chan=1:5 -% subplot(2,3,chan); -% plot(tracking_channel(chan).carrier_doppler_hz); -% grid minor -% end -% sgtitle('carrier doppler hz channel') -% %% -% figure -% for chan=1:5 -% subplot(2,3,chan); -% plot(tracking_channel(chan).CN0_SNV_dB_Hz); -% grid minor -% end -% sgtitle('CN0 SNV dB Hz channel') -% -% %% -% figure -% for chan=1:5 -% subplot(2,3,chan); -% plot(tracking_channel(chan).acc_carrier_phase_rad); -% grid minor -% end -% sgtitle('acc carrier phase rad') \ No newline at end of file From 8056b54045dceb949246e87e95d33b50a6ae8ee7 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Sun, 12 Feb 2023 12:58:55 +0100 Subject: [PATCH 122/165] REFACTOR: vtl_engine --- src/algorithms/PVT/libs/vtl_engine.cc | 331 +++++++++++++------------- src/algorithms/PVT/libs/vtl_engine.h | 21 +- 2 files changed, 166 insertions(+), 186 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index ed05850c2..6875765e4 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -33,31 +33,28 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) //TODO: Implement main VTL loop here using arma::as_scalar; - + static uint64_t refSampleCounter = new_data.sample_counter; + double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; + refSampleCounter = new_data.sample_counter; // ################## Kalman filter initialization ###################################### + //State variables + int n_of_states=11; + static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; + static arma::mat kf_x = arma::zeros(n_of_states, 1); + arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) - kf_P_x = arma::eye(12, 12) * 1.0; //TODO: use a real value. - kf_x = arma::zeros(12, 1); + kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); - double kf_dt = 0.05; - kf_Q = arma::eye(12, 12); + double kf_dt = delta_t_vtl; //0.05; + kf_Q = arma::eye(n_of_states, n_of_states); - kf_F = arma::eye(12, 12); - kf_F(0, 3) = kf_dt; - kf_F(1, 4) = kf_dt; - kf_F(2, 5) = kf_dt; + kf_F = arma::eye(n_of_states, n_of_states); + bool test = kf_F_fill(kf_F,kf_dt); - kf_F(3, 6) = kf_dt; - kf_F(4, 7) = kf_dt; - kf_F(5, 8) = kf_dt; - - kf_F(9, 10) = kf_dt; - kf_F(10, 11) = kf_dt; - - kf_H = arma::zeros(2 * new_data.sat_number, 12); + //kf_H = arma::zeros(2 * new_data.sat_number, n_of_states); kf_y = arma::zeros(2 * new_data.sat_number, 1); kf_yerr = arma::zeros(2 * new_data.sat_number, 1); - kf_xerr = arma::zeros(12, 1); + kf_xerr = arma::zeros(n_of_states, 1); kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix // ################## Kalman Tracking ###################################### @@ -68,36 +65,39 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) if (counter < closure_point) { - // receiver solution from rtklib_solver + // // receiver solution from rtklib_solver + kf_dx=kf_x; kf_x(0) = new_data.rx_p(0); kf_x(1) = new_data.rx_p(1); kf_x(2) = new_data.rx_p(2); kf_x(3) = new_data.rx_v(0); kf_x(4) = new_data.rx_v(1); kf_x(5) = new_data.rx_v(2); - + kf_x(6) = 0; + kf_x(7) = 0; + kf_x(8) = 0; kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; - kf_x(11) = 1.0; - kf_x = kf_F * kf_x; // state prediction + kf_dx=kf_x-kf_dx; + kf_dx = kf_F * kf_dx; // state prediction } else { // receiver solution from previous KF step - kf_x(0) = new_data.kf_state[0]; - kf_x(1) = new_data.kf_state[1]; - kf_x(2) = new_data.kf_state[2]; - kf_x(3) = new_data.kf_state[3]; - kf_x(4) = new_data.kf_state[4]; - kf_x(5) = new_data.kf_state[5]; - kf_x(6) = new_data.kf_state[6]; - kf_x(7) = new_data.kf_state[7]; - kf_x(8) = new_data.kf_state[8]; - kf_x(9) = new_data.kf_state[9]; - kf_x(10) = new_data.kf_state[10]; - kf_x(11) = new_data.kf_state[11]; - kf_P_x = new_data.kf_P; + // kf_x(0) = x_u; + // kf_x(1) = y_u; + // kf_x(2) = z_u; + // kf_x(3) = xDot_u; + // kf_x(4) = yDot_u; + // kf_x(5) = zDot_u; + // kf_x(6) = xDot2_u; + // kf_x(7) = yDot2_u; + // kf_x(8) = zDot2_u; + // kf_x(9) = cdeltat_u; + // kf_x(10) = cdeltatDot_u; + + //kf_P_x = new_data.kf_P; } // State error Covariance Matrix Q (PVT) @@ -111,41 +111,31 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_Q(6, 6) = .1; kf_Q(7, 7) = .1; kf_Q(8, 8) = .1; - kf_Q(9, 9) = 100.0; - kf_Q(10, 10) = 40.0; - kf_Q(11, 11) = 1.0; + kf_Q(9, 9) = 4.0; + kf_Q(10, 10) = 100.0; // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 50.0*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 5.0*50.0/new_data.sat_CN0_dB_hz(i); + kf_R(i, i) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 1.0;//*50.0/new_data.sat_CN0_dB_hz(i); - if(50.0*50.0/new_data.sat_CN0_dB_hz(i)>70||5.0*50.0/new_data.sat_CN0_dB_hz(i)>7){ - kf_R(i, i) = 10e4; - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; - } + // if(50.0*50.0/new_data.sat_CN0_dB_hz(i)>70||5.0*50.0/new_data.sat_CN0_dB_hz(i)>7){ + // kf_R(i, i) = 10e4; + // kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; + // } } - + // arma::mat test = kf_P_x.diag(); + // test.print("P diagonal"); + // test = kf_Q.diag(); + // test.print("Q diagonal"); // Kalman state prediction (time update) //new_data.kf_state=kf_x; //kf_x = kf_F * kf_x; // state prediction - kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction //from error state variables to variables // From state variables definition // TODO: cast to type properly - x_u = kf_x(0); - y_u = kf_x(1); - z_u = kf_x(2); - xDot_u = kf_x(3); - yDot_u = kf_x(4); - zDot_u = kf_x(5); - xDot2_u = kf_x(6); - yDot2_u = kf_x(7); - zDot2_u = kf_x(8); - cdeltat_u = kf_x(9); - cdeltatDot_u = kf_x(10); d = arma::zeros(new_data.sat_number, 1); rho_pri = arma::zeros(new_data.sat_number, 1); @@ -158,119 +148,41 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) a_x = arma::zeros(new_data.sat_number, 1); a_y = arma::zeros(new_data.sat_number, 1); a_z = arma::zeros(new_data.sat_number, 1); + + test = obsv_calc(rho_pri,rhoDot_pri,a_x, a_y, a_z,new_data.sat_number,new_data.sat_p,new_data.sat_v,kf_x); for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities { - //d(i) is the distance sat(i) to receiver - d(i) = (new_data.sat_p(i, 0) - x_u) * (new_data.sat_p(i, 0) - x_u); - d(i) = d(i) + (new_data.sat_p(i, 1) - y_u) * (new_data.sat_p(i, 1) - y_u); - d(i) = d(i) + (new_data.sat_p(i, 2) - z_u) * (new_data.sat_p(i, 2) - z_u); - d(i) = sqrt(d(i)); - - //compute pseudorange estimation - rho_pri(i) = d(i) + cdeltat_u; - //compute LOS sat-receiver vector components - a_x(i) = -(new_data.sat_p(i, 0) - x_u) / d(i); - a_y(i) = -(new_data.sat_p(i, 1) - y_u) / d(i); - a_z(i) = -(new_data.sat_p(i, 2) - z_u) / d(i); new_data.sat_LOS(i, 0) = a_x(i); new_data.sat_LOS(i, 1) = a_y(i); new_data.sat_LOS(i, 2) = a_z(i); - //compute pseudorange rate estimation - rhoDot_pri(i) = (new_data.sat_v(i, 0) - xDot_u) * a_x(i) + (new_data.sat_v(i, 1) - yDot_u) * a_y(i) + (new_data.sat_v(i, 2) - zDot_u) * a_z(i); - rhoDot_pri(i) = rhoDot_pri(i) + a_x(i)*xDot2_u*kf_dt+a_y(i)*yDot2_u*kf_dt+a_z(i)*zDot2_u*kf_dt;; } // cout<.5){ + kf_xerr.print("kf_xERR: "); + cout<<"kf_dt: "<5){ + // dump_vtl_file << "pr_m" + // << "," << new_data.pr_m(0) << "," << new_data.pr_m(1) << "," << new_data.pr_m(2) + // << "," << new_data.pr_m(3) << "," << new_data.pr_m(4)<<"," << new_data.pr_m(5) << endl; + // dump_vtl_file << "prDot_m" + // << "," << new_data.doppler_hz(0)<< "," << new_data.doppler_hz(1)<< "," << new_data.doppler_hz(2) + // << "," << new_data.doppler_hz(3) << "," << new_data.doppler_hz(4)<< "," << new_data.doppler_hz(5)<< endl; + // dump_vtl_file << "sat_position" + // << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << "," << kf_xerr(5) + // << "," << kf_xerr(6) << "," << kf_xerr(7) << endl; + // dump_vtl_file << "sat_velocity" + // << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) + // << "," << kf_xerr(6) << "," << kf_xerr(7) << endl; + } dump_vtl_file << "kf_state" - << "," << new_data.kf_state(0) << "," << new_data.kf_state(1) << "," << new_data.kf_state(2) << "," << new_data.kf_state(3) << "," << new_data.kf_state(4) << "," << new_data.kf_state(5) << "," << new_data.kf_state(6) << "," << new_data.kf_state(7)<< new_data.kf_state(8) << endl; + << "," << kf_x(0) << "," << kf_x(1) << "," << kf_x(2) << "," << kf_x(3) << "," << kf_x(4) << "," << kf_x(5) << "," << kf_x(6) << "," << kf_x(7)<< "," << kf_x(8) <<"," << kf_x(9) <<"," << kf_x(10)<< endl; + dump_vtl_file << "kf_xerr" + << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) << "," << kf_xerr(6) << "," << kf_xerr(7)<< "," << kf_xerr(8) <<"," << kf_xerr(9) <<"," << kf_xerr(10)<< endl; dump_vtl_file << "rtklib_state" - << "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << endl; - dump_vtl_file << "filt_dopp_sat" - << "," << doppler_hz_filt(0) << "," << doppler_hz_filt(1) << "," << doppler_hz_filt(2) << "," << doppler_hz_filt(3) << "," << doppler_hz_filt(4) << endl; + << "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << "," << delta_t_vtl << endl; + // dump_vtl_file << "filt_dopp_sat" + // << "," << doppler_hz_filt(0) << "," << doppler_hz_filt(1) << "," << doppler_hz_filt(2) << "," << doppler_hz_filt(3) << "," << doppler_hz_filt(4) < Date: Sun, 12 Feb 2023 13:13:29 +0100 Subject: [PATCH 123/165] Refactor: function names --- src/algorithms/PVT/libs/vtl_engine.cc | 4 ++-- src/algorithms/PVT/libs/vtl_engine.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 6875765e4..a26e1511e 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -162,7 +162,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt); // kf_H.print("kf_H diag:"); // Kalman estimation (measurement update) - test = kf_NLmeasurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x); + test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x); kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction // kf_P_x.print("a priori P: "); @@ -328,7 +328,7 @@ bool Vtl_Engine::obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec return -1; } -bool Vtl_Engine::kf_NLmeasurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x) +bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x) { for (int32_t i = 0; i < sat_number; i++) // Measurement vector { diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 127144a68..4cce7371b 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -87,7 +87,7 @@ private: bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation - bool kf_NLmeasurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); + bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); }; /** \} */ From 435fb44949cc669d6e2ff756e3136a94f0add3e5 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Sun, 12 Feb 2023 13:25:38 +0100 Subject: [PATCH 124/165] FIX: kf_K declaration --- src/algorithms/PVT/libs/vtl_engine.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index a26e1511e..56fd0e876 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -56,7 +56,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_yerr = arma::zeros(2 * new_data.sat_number, 1); kf_xerr = arma::zeros(n_of_states, 1); kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix - + kf_K = arma::zeros(n_of_states, 2 * new_data.sat_number); ; // ################## Kalman Tracking ###################################### static uint32_t counter = 0; //counter counter = counter + 1; //uint64_t @@ -171,12 +171,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain kf_xerr = kf_K * (kf_yerr); // Error state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix - kf_x = kf_x;// - kf_xerr; // updated state estimation (a priori + error) + kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) if(abs(delta_t_vtl)>.5){ kf_xerr.print("kf_xERR: "); cout<<"kf_dt: "< Date: Wed, 15 Feb 2023 16:09:48 +0100 Subject: [PATCH 125/165] MOD: vtl tracking --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 2 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 63ae58f71..c087eb6dc 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2324,7 +2324,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { // VTP To.Do: Check why get_PVT is triggered twice. Leave only one get_PVT. - flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); + //flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); } if (flag_pvt_valid == true) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 78b98c2a9..498813360 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -658,17 +658,19 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) double old_code_phase_chips = d_x_old_old(0); if(abs(d_x_old_old(2) - tmp_x(2))>50){ - std::cout << " tracking_cmd TOO FAR: " + std::cout <<"channel: "<< this->d_channel + << " tracking_cmd TOO FAR: " << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" << " \n"; }else{ - std::cout << " tracking_cmd NEAR: " + std::cout <<"channel: "<< this->d_channel + << " tracking_cmd NEAR: " << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" << " \n"; - //d_x_old_old(2) = tmp_x(2); //replace the Code Phase state + //d_x_old_old(2) = tmp_x(2); //replace DOPPLER + //d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE } - //d_x_old_old(0) = tmp_x(0); //replace the Code Phase state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; @@ -698,7 +700,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) else { dump_tracking_file << "doppler_corr" - << ","<< this->d_channel << "," << x_tmp(2) << "," << old_doppler << "," << old_doppler_rate << "," << old_code_phase_chips << "\n"; + << ","<< this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3)<< "," << old_doppler_rate << "\n"; dump_tracking_file.close(); } } From 1991ab4aa6423fb49046c64920d3137b2e271b33 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 16:10:23 +0100 Subject: [PATCH 126/165] MOD: vtl_engine --- src/algorithms/PVT/libs/vtl_engine.cc | 101 +++++++++++--------------- 1 file changed, 42 insertions(+), 59 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 56fd0e876..dc582295c 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -41,7 +41,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) int n_of_states=11; static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; static arma::mat kf_x = arma::zeros(n_of_states, 1); - arma::mat kf_dx = arma::zeros(n_of_states, 1); + static arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); @@ -85,19 +85,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) else { // receiver solution from previous KF step - // kf_x(0) = x_u; - // kf_x(1) = y_u; - // kf_x(2) = z_u; - // kf_x(3) = xDot_u; - // kf_x(4) = yDot_u; - // kf_x(5) = zDot_u; - // kf_x(6) = xDot2_u; - // kf_x(7) = yDot2_u; - // kf_x(8) = zDot2_u; - // kf_x(9) = cdeltat_u; - // kf_x(10) = cdeltatDot_u; - - //kf_P_x = new_data.kf_P; + kf_x(6) = (kf_x(6)-kf_dx(6))/kf_dt; + kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; + kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; } // State error Covariance Matrix Q (PVT) @@ -108,9 +98,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_Q(3, 3) = 8.0; kf_Q(4, 4) = 8.0; kf_Q(5, 5) = 8.0; - kf_Q(6, 6) = .1; - kf_Q(7, 7) = .1; - kf_Q(8, 8) = .1; + kf_Q(6, 6) = 10; + kf_Q(7, 7) = 10; + kf_Q(8, 8) = 10; kf_Q(9, 9) = 4.0; kf_Q(10, 10) = 100.0; @@ -118,18 +108,16 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) for (int32_t i = 0; i < new_data.sat_number; i++) { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 1.0;//*50.0/new_data.sat_CN0_dB_hz(i); + kf_R(i, i) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); - // if(50.0*50.0/new_data.sat_CN0_dB_hz(i)>70||5.0*50.0/new_data.sat_CN0_dB_hz(i)>7){ - // kf_R(i, i) = 10e4; - // kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; - // } + if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ + kf_R(i, i) = 10e4; + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; + cout<<"channel: "<.5){ - kf_xerr.print("kf_xERR: "); - cout<<"kf_dt: "<5){ - // dump_vtl_file << "pr_m" - // << "," << new_data.pr_m(0) << "," << new_data.pr_m(1) << "," << new_data.pr_m(2) - // << "," << new_data.pr_m(3) << "," << new_data.pr_m(4)<<"," << new_data.pr_m(5) << endl; - // dump_vtl_file << "prDot_m" - // << "," << new_data.doppler_hz(0)<< "," << new_data.doppler_hz(1)<< "," << new_data.doppler_hz(2) - // << "," << new_data.doppler_hz(3) << "," << new_data.doppler_hz(4)<< "," << new_data.doppler_hz(5)<< endl; - // dump_vtl_file << "sat_position" - // << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << "," << kf_xerr(5) - // << "," << kf_xerr(6) << "," << kf_xerr(7) << endl; - // dump_vtl_file << "sat_velocity" - // << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) - // << "," << kf_xerr(6) << "," << kf_xerr(7) << endl; + dump_vtl_file << "pr_m" + << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) + << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)< Date: Wed, 15 Feb 2023 16:33:02 +0100 Subject: [PATCH 127/165] ADD: model3DoF prototype --- src/algorithms/PVT/libs/vtl_engine.cc | 42 ++++++++++++++++++--------- src/algorithms/PVT/libs/vtl_engine.h | 1 + 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index dc582295c..dd005866c 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -44,19 +44,19 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) static arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) - kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); + kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); double kf_dt = delta_t_vtl; //0.05; kf_Q = arma::eye(n_of_states, n_of_states); kf_F = arma::eye(n_of_states, n_of_states); bool test = kf_F_fill(kf_F,kf_dt); - //kf_H = arma::zeros(2 * new_data.sat_number, n_of_states); - kf_y = arma::zeros(2 * new_data.sat_number, 1); - kf_yerr = arma::zeros(2 * new_data.sat_number, 1); + //kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); + kf_y = arma::zeros(3 * new_data.sat_number, 1); + kf_yerr = arma::zeros(3 * new_data.sat_number, 1); kf_xerr = arma::zeros(n_of_states, 1); - kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix - kf_K = arma::zeros(n_of_states, 2 * new_data.sat_number); ; + kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix + kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); ; // ################## Kalman Tracking ###################################### static uint32_t counter = 0; //counter counter = counter + 1; //uint64_t @@ -79,15 +79,22 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; - kf_dx=kf_x-kf_dx; + kf_dx = kf_x-kf_dx; kf_dx = kf_F * kf_dx; // state prediction } else { // receiver solution from previous KF step - kf_x(6) = (kf_x(6)-kf_dx(6))/kf_dt; - kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; - kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; + double acc_x = 0; + double acc_y = 0; + double acc_z = 0; + test = model3DoF(acc_x,acc_x,acc_x,kf_x,kf_dt); + kf_x(6) = acc_x; + kf_x(7) = acc_y; + kf_x(8) = acc_z; + // kf_x(6) = (kf_x(6)-kf_dx(6))/kf_dt; + // kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; + // kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; } // State error Covariance Matrix Q (PVT) @@ -98,11 +105,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_Q(3, 3) = 8.0; kf_Q(4, 4) = 8.0; kf_Q(5, 5) = 8.0; - kf_Q(6, 6) = 10; - kf_Q(7, 7) = 10; - kf_Q(8, 8) = 10; + kf_Q(6, 6) = .10; + kf_Q(7, 7) = .10; + kf_Q(8, 8) = .10; kf_Q(9, 9) = 4.0; - kf_Q(10, 10) = 100.0; + kf_Q(10, 10) = 10.0; // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) @@ -110,10 +117,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) kf_R(i, i) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); + kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 100.0;//*50.0/new_data.sat_CN0_dB_hz(i); if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ kf_R(i, i) = 10e4; kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; + kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 10e4; cout<<"channel: "< Date: Wed, 15 Feb 2023 17:05:49 +0100 Subject: [PATCH 128/165] ADD: EmpujeLkTable prototype --- src/algorithms/PVT/libs/vtl_engine.cc | 58 ++++++++++++++++++++++++++- src/algorithms/PVT/libs/vtl_engine.h | 3 +- 2 files changed, 58 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index dd005866c..05f3aa510 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -331,7 +331,61 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r return -1; } -bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double kf_dt) +bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double dt) { - return -1; + arma::colvec u_vec; + arma::colvec acc_vec; + arma::colvec u_dir; + arma::colvec gravity_ECEF = {-7.826024, 0.8616969, -5.833042}; //lat=36.533333 lon=-6.283333 + static double t_disparo; + double Empuje; + double densidad=1.0; + double ballistic_coef = 0.007; + double PI = acos(-1.0); + //vector velocidad + u_vec = kf_x.rows(4, 6); + // double u=sqrt(pow(kf_x(4),2)+pow(kf_x(5),2)+pow(kf_x(6),2)); + + //modulo de la velocidad + double u = norm(u_vec, 2); + + if(u>4){ + t_disparo=t_disparo+dt; + double diam_cohete=120.0e-3;// 120 mm + double mass_rocket=50.0; //50Kg + + if(t_disparo<.2){ + u_dir={.90828, -.13984, .388756}; + }else{ + u_dir = u_vec / u; + } + // lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]); + // [T, sound_v, P, densidad] = atmosisa(lla(3)); + // sound_v=320;% @ 5km and -17.5C + // Mach=u/sound_v; + // CD0 = Cd0_M_LookTable(Mach); + // % ballistic_coef is Cd0/mass_rocket; + // ballistic_coef=CD0/mass_rocket; + // Empuje = EmpujeLookTable(t_disparo); + + acc_vec = -(PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir + +gravity_ECEF+Empuje*u_dir; + + // % return + acc_x = acc_vec(0); + acc_y = acc_vec(1); + acc_z = acc_vec(2); + }else{ + // % return + acc_x = 0; + acc_y = 0; + acc_z = 0; + } + + return -1; +} + +double Vtl_Engine::EmpujeLkTable(double t_disparo) +{ + return -1.0; } \ No newline at end of file diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 6c869c96e..9f04e1939 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -88,7 +88,8 @@ private: bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); - bool model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double kf_dt); + bool model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double dt); + double EmpujeLkTable(double t_disparo); }; /** \} */ From 3cb61f6f2db2e0fb82f3a09466c2ce79b6f24a3d Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 19:09:37 +0100 Subject: [PATCH 129/165] ADD: empuje lkTable --- src/algorithms/PVT/libs/vtl_engine.cc | 197 +++++++++++++++++++++++++- 1 file changed, 193 insertions(+), 4 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 05f3aa510..605f9b61e 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -341,7 +341,6 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x double Empuje; double densidad=1.0; double ballistic_coef = 0.007; - double PI = acos(-1.0); //vector velocidad u_vec = kf_x.rows(4, 6); // double u=sqrt(pow(kf_x(4),2)+pow(kf_x(5),2)+pow(kf_x(6),2)); @@ -368,7 +367,7 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x // ballistic_coef=CD0/mass_rocket; // Empuje = EmpujeLookTable(t_disparo); - acc_vec = -(PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir + acc_vec = -(GNSS_PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir +gravity_ECEF+Empuje*u_dir; // % return @@ -386,6 +385,196 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x } double Vtl_Engine::EmpujeLkTable(double t_disparo) -{ - return -1.0; +{ + double E; + arma::mat LkTable={ + {0.0, 391.083112445424}, + {0.0100578034682081, 385.626317230813}, + {0.0201156069364162, 379.253652903964}, + {0.0301734104046243, 372.850418310078}, + {0.0402312138728324, 366.435105395212}, + {0.0502890173410405, 359.948724887310}, + {0.0603468208092486, 353.452370826679}, + {0.0704046242774566, 346.915160536406}, + {0.0804624277456647, 340.353374212744}, + {0.0905202312138728, 339.982366920698}, + {0.100578034682081, 339.649644036322}, + {0.110635838150289, 339.313119301332}, + {0.120693641618497, 338.971249841340}, + {0.130751445086705, 338.626092336370}, + {0.140809248554913, 338.277926096280}, + {0.150867052023121, 337.923860794114}, + {0.160924855491329, 337.564686821652}, + {0.170982658959538, 337.204565166310}, + {0.181040462427746, 336.836327593982}, + {0.191098265895954, 337.612574596978}, + {0.201156069364162, 338.389812277202}, + {0.211213872832370, 339.172197383571}, + {0.221271676300578, 339.950864133333}, + {0.231329479768786, 340.734286946588}, + {0.241387283236994, 341.513599696996}, + {0.251445086705202, 342.296487929689}, + {0.261502890173410, 343.077730186615}, + {0.271560693641619, 343.862401553296}, + {0.281618497109827, 344.645045182721}, + {0.291676300578035, 345.430222609429}, + {0.301734104046243, 346.216363751051}, + {0.311791907514451, 347.003086022300}, + {0.321849710982659, 347.790403464500}, + {0.331907514450867, 348.576632890101}, + {0.341965317919075, 349.365041767204}, + {0.352023121387283, 350.154345504950}, + {0.362080924855491, 350.943780688588}, + {0.372138728323699, 351.735714016931}, + {0.382196531791908, 352.523676077225}, + {0.392254335260116, 353.317052045327}, + {0.402312138728324, 354.110411623268}, + {0.412369942196532, 354.903279067692}, + {0.422427745664740, 355.692559158889}, + {0.432485549132948, 356.490274978154}, + {0.442543352601156, 357.283451900588}, + {0.452601156069364, 358.078507291348}, + {0.462658959537572, 358.871323078342}, + {0.472716763005780, 359.668187019647}, + {0.482774566473988, 360.465893772213}, + {0.492832369942197, 361.264486481857}, + {0.502890173410405, 362.057713788838}, + {0.512947976878613, 362.855398652135}, + {0.523005780346821, 363.651958513907}, + {0.533063583815029, 364.453139119421}, + {0.543121387283237, 365.248494477390}, + {0.553179190751445, 366.047440571574}, + {0.563236994219653, 366.844541693072}, + {0.573294797687861, 367.645722991578}, + {0.583352601156069, 368.431281642859}, + {0.593410404624277, 369.218032671753}, + {0.603468208092486, 370.005763838889}, + {0.613526011560694, 370.793449087224}, + {0.623583815028902, 371.577331994297}, + {0.633641618497110, 372.361872668242}, + {0.643699421965318, 373.139234321453}, + {0.653757225433526, 373.922395308171}, + {0.663815028901734, 374.698638233448}, + {0.673872832369942, 375.477674739791}, + {0.683930635838150, 376.256375688174}, + {0.693988439306358, 377.561519397833}, + {0.704046242774567, 378.859304679191}, + {0.714104046242775, 380.167718611141}, + {0.724161849710983, 381.477582211590}, + {0.734219653179191, 382.785006750526}, + {0.744277456647399, 384.092555921096}, + {0.754335260115607, 385.403347233778}, + {0.764393063583815, 386.680222025901}, + {0.774450867052023, 387.960348600215}, + {0.784508670520231, 389.241465657245}, + {0.794566473988439, 390.517609695133}, + {0.804624277456647, 391.778341124162}, + {0.814682080924856, 393.038366474572}, + {0.824739884393064, 394.291210028831}, + {0.834797687861272, 395.545183108074}, + {0.844855491329480, 396.779476009029}, + {0.854913294797688, 398.005131564908}, + {0.864971098265896, 399.217607990930}, + {0.875028901734104, 400.433585037519}, + {0.885086705202312, 401.639603766860}, + {0.895144508670520, 402.824960888722}, + {0.905202312138728, 403.999818917039}, + {0.915260115606936, 405.164413592803}, + {0.925317919075145, 406.332094769783}, + {0.935375722543353, 407.489826389568}, + {0.945433526011561, 408.638695158694}, + {0.955491329479769, 409.788208430892}, + {0.965549132947977, 410.920696471959}, + {0.975606936416185, 412.071117188526}, + {0.985664739884393, 413.227362102269}, + {0.995722543352601, 414.376295532213}, + {1.00578034682081, 415.517208260982}, + {1.01583815028902, 416.685111645473}, + {1.02589595375723, 417.874034388355}, + {1.03595375722543, 419.050152356493}, + {1.04601156069364, 420.226540416000}, + {1.05606936416185, 421.431674949807}, + {1.06612716763006, 422.651832857732}, + {1.07618497109827, 423.879432015756}, + {1.08624277456647, 425.096859052146}, + {1.09630057803468, 426.370514159926}, + {1.10635838150289, 427.681420607676}, + {1.11641618497110, 428.990653815442}, + {1.12647398843931, 430.308007852515}, + {1.13653179190751, 431.623136377095}, + {1.14658959537572, 432.942354714805}, + {1.15664739884393, 434.260396644686}, + {1.16670520231214, 435.580636665589}, + {1.17676300578035, 436.903819649131}, + {1.18682080924855, 438.235977095674}, + {1.19687861271676, 439.760282970165}, + {1.20693641618497, 441.285566201751}, + {1.21699421965318, 442.823791210454}, + {1.22705202312139, 444.354330708051}, + {1.23710982658960, 445.893589623130}, + {1.24716763005780, 447.437414139676}, + {1.25722543352601, 449.016306037988}, + {1.26728323699422, 450.605733417807}, + {1.27734104046243, 452.197570123945}, + {1.28739884393064, 453.799041921642}, + {1.29745664739884, 455.395152193126}, + {1.30751445086705, 457.004209080179}, + {1.31757225433526, 458.623015526914}, + {1.32763005780347, 460.238728741943}, + {1.33768786127168, 461.860881948233}, + {1.34774566473988, 463.495649237401}, + {1.35780346820809, 465.129826626932}, + {1.36786127167630, 466.767033528504}, + {1.37791907514451, 468.408766204263}, + {1.38797687861272, 470.059173838784}, + {1.39803468208092, 471.721126584341}, + {1.40809248554913, 473.414912587257}, + {1.41815028901734, 475.097553575053}, + {1.42820809248555, 476.805256760032}, + {1.43826589595376, 478.512506689219}, + {1.44832369942197, 480.230235676597}, + {1.45838150289017, 481.950516788809}, + {1.46843930635838, 483.675596525192}, + {1.47849710982659, 485.408520318711}, + {1.48855491329480, 487.160694244723}, + {1.49861271676301, 488.912737035966}, + {1.50867052023121, 490.676335211920}, + {1.51872832369942, 492.446193358219}, + {1.52878612716763, 494.230761014528}, + {1.53884393063584, 496.014213821540}, + {1.54890173410405, 497.800012215749}, + {1.55895953757225, 499.586844428474}, + {1.56901734104046, 501.389791490613}, + {1.57907514450867, 503.197873321221}, + {1.58913294797688, 505.017976823598}, + {1.59919075144509, 506.842077493572}, + {1.60924855491329, 508.681008225194}, + {1.61930635838150, 510.531745060971}, + {1.62936416184971, 512.383464849326}, + {1.63942196531792, 514.249250804510}, + {1.64947976878613, 516.119098877381}, + {1.65953757225434, 518.005308788433}, + {1.66959537572254, 519.910761252352}, + {1.67965317919075, 521.812275649970}, + {1.68971098265896, 523.727747256148}, + {1.69976878612717, 526.524959987653}, + {1.70982658959538, 529.326074922632}, + {1.71988439306358, 532.152158438731}, + {1.72994219653179, 534.995939192065}, + {1.74000000000000, 537.866310625605},}; + + //encuentra el mas cercano justo anterior. + int index_E = LkTable.elem(find(LkTable<=t_disparo)).max(); + if (index_E<(LkTable.n_rows-1)){ + double tdisparo1=LkTable(index_E,0); + double tdisparo2=LkTable(index_E+1,0); + double E1=LkTable(index_E,1); + double E2=LkTable(index_E+1,1); + + E=(t_disparo-tdisparo1)*(E2-E1)/(tdisparo2-tdisparo1)+E1; + }else{ + E=0; + } + + return E; } \ No newline at end of file From 2115bc0aab63bafe45d97e7174fb7a488128a7bd Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 19:18:49 +0100 Subject: [PATCH 130/165] MOD: model3DoF --- src/algorithms/PVT/libs/vtl_engine.cc | 11 ++++++++--- src/algorithms/PVT/libs/vtl_engine.h | 2 +- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 605f9b61e..56b4073ae 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -331,13 +331,13 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r return -1; } -bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double dt) +bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt) { arma::colvec u_vec; arma::colvec acc_vec; arma::colvec u_dir; arma::colvec gravity_ECEF = {-7.826024, 0.8616969, -5.833042}; //lat=36.533333 lon=-6.283333 - static double t_disparo; + static double t_disparo=0; double Empuje; double densidad=1.0; double ballistic_coef = 0.007; @@ -347,6 +347,10 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x //modulo de la velocidad double u = norm(u_vec, 2); + + // if(t<1000){ + // t_disparo=0; + // } if(u>4){ t_disparo=t_disparo+dt; @@ -365,7 +369,7 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x // CD0 = Cd0_M_LookTable(Mach); // % ballistic_coef is Cd0/mass_rocket; // ballistic_coef=CD0/mass_rocket; - // Empuje = EmpujeLookTable(t_disparo); + Empuje = EmpujeLkTable(t_disparo); acc_vec = -(GNSS_PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir +gravity_ECEF+Empuje*u_dir; @@ -375,6 +379,7 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x acc_y = acc_vec(1); acc_z = acc_vec(2); }else{ + t_disparo=0; // % return acc_x = 0; acc_y = 0; diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 9f04e1939..3611373fe 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -88,7 +88,7 @@ private: bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); - bool model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double dt); + bool model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt); double EmpujeLkTable(double t_disparo); }; From 08c68d4af902bd9f88a3028064aa18d837d13b7c Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 19:44:21 +0100 Subject: [PATCH 131/165] MOD: filtro aceleracion vtl --- src/algorithms/PVT/libs/vtl_engine.cc | 47 +++++++++++++++++++-------- src/algorithms/PVT/libs/vtl_engine.h | 3 +- 2 files changed, 36 insertions(+), 14 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 56b4073ae..27d014b86 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -42,6 +42,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; static arma::mat kf_x = arma::zeros(n_of_states, 1); static arma::mat kf_dx = arma::zeros(n_of_states, 1); + + //TODO: resolver el problema cuando cambie el numero de sat!! + // static arma::colvec rhoDot_pri_old = arma::zeros(new_data.sat_number, 1); // covariances (static) kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); @@ -117,7 +120,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) kf_R(i, i) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); - kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 100.0;//*50.0/new_data.sat_CN0_dB_hz(i); + kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 400.0;//*50.0/new_data.sat_CN0_dB_hz(i); if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ kf_R(i, i) = 10e4; @@ -137,6 +140,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) d = arma::zeros(new_data.sat_number, 1); rho_pri = arma::zeros(new_data.sat_number, 1); rhoDot_pri = arma::zeros(new_data.sat_number, 1); + rhoDot2_pri = arma::zeros(new_data.sat_number, 1); rho_pri_filt = arma::zeros(new_data.sat_number, 1); rhoDot_pri_filt = arma::zeros(new_data.sat_number, 1); @@ -154,26 +158,39 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) new_data.sat_LOS(i, 2) = a_z(i); } - kf_H = arma::zeros(2 * new_data.sat_number, n_of_states); + kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt); + + for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities + { + // rhoDot2_pri(i)=(rhoDot_pri(i)-rhoDot_pri_old(i))/kf_dt; + } // Kalman estimation (measurement update) - test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x); + test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x); kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction // Kalman filter update step - kf_S = kf_H * kf_P_x * kf_H.t() + kf_R; // innovation covariance matrix (S) + kf_S = kf_H * kf_P_x * kf_H.t() + kf_R; // innovation covariance matrix (S) arma::mat B= (kf_P_x * kf_H.t()) ; kf_K = B * arma::inv(kf_S); // Kalman gain - kf_xerr = kf_K * (kf_yerr); // Error state estimation + kf_xerr = kf_K * (kf_yerr); // Error state estimation //kf_xerr.row(5)=kf_K.row(5)*kf_yerr; arma::mat A = (arma::eye(size(kf_P_x)) - kf_K * kf_H); kf_P_x = A * kf_P_x * A.t() + kf_K * kf_R * kf_K.t() ; // update state estimation error covariance matrix kf_dx=kf_x; - kf_x = kf_x-kf_xerr; // updated state estimation (a priori + error) + kf_x = kf_x-kf_xerr; // updated state estimation (a priori + error) // // ################## Geometric Transformation ###################################### test = obsv_calc(rho_pri,rhoDot_pri,a_x, a_y, a_z,new_data.sat_number,new_data.sat_p,new_data.sat_v,kf_x); + + for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities + { + //acc_effect(i)=(a_x(i)*kf_state(7,t)+a_y(chan,t)*kf_state(8,t)+a_z(chan,t)*kf_state(9,t)); + //rhoDot2_pri(chan,t)=(rhoDot_pri(chan,t)-rhoDot_pri(chan,t-1))/kf_dt; + //rhoDot2_pri(chan,t)=-acc_effect(chan,t); + } + test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt); // Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x kf_yerr = kf_H * kf_xerr; @@ -191,7 +208,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // sample code trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction - trk_cmd.carrier_freq_rate_hz_s =-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; + trk_cmd.carrier_freq_rate_hz_s = 0;//-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; trk_cmd.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; trk_cmd.enable_carrier_nco_cmd = true; trk_cmd.enable_code_nco_cmd = true; @@ -278,6 +295,13 @@ bool Vtl_Engine::kf_H_fill(arma::mat &kf_H,int sat_number, arma::colvec ax, arma kf_H(i + sat_number, 8) = az(i)*kf_dt; kf_H(i + sat_number, 10) = 1.0; + kf_H(i + 2*sat_number, 3) = 0;//ax(i); + kf_H(i + 2*sat_number, 4) = 0;//ay(i); + kf_H(i + 2*sat_number, 5) = 0;//az(i); + kf_H(i + 2*sat_number, 6) = ax(i); + kf_H(i + 2*sat_number, 7) = ay(i); + kf_H(i + 2*sat_number, 8) = az(i); + kf_H(i + 2*sat_number, 10) = kf_dt; } return -1; @@ -321,12 +345,13 @@ bool Vtl_Engine::obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec return -1; } -bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x) +bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x) { for (int32_t i = 0; i < sat_number; i++) // Measurement vector { kf_yerr(i) = rho_pri(i) - pr_m(i); kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1+kf_x(10)) - rhoDot_pri(i); + kf_yerr(i + 2*sat_number) = -rhoDot2_pri(i); } return -1; } @@ -347,11 +372,7 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k //modulo de la velocidad double u = norm(u_vec, 2); - - // if(t<1000){ - // t_disparo=0; - // } - + if(u>4){ t_disparo=t_disparo+dt; double diam_cohete=120.0e-3;// 120 mm diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 3611373fe..d41bb6b60 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -56,6 +56,7 @@ private: arma::colvec d; arma::colvec rho_pri; arma::colvec rhoDot_pri; + arma::colvec rhoDot2_pri; arma::colvec rho_pri_filt; arma::colvec rhoDot_pri_filt; arma::colvec doppler_hz_filt; @@ -87,7 +88,7 @@ private: bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation - bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); + bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); bool model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt); double EmpujeLkTable(double t_disparo); }; From 3f348aabd92ca67b954ea8b8ea1c7d54ce42c134 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 20:13:54 +0100 Subject: [PATCH 132/165] FIX: parser velocity error --- src/algorithms/PVT/libs/vtl_engine.cc | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 27d014b86..e5ecfc5d1 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -166,7 +166,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // rhoDot2_pri(i)=(rhoDot_pri(i)-rhoDot_pri_old(i))/kf_dt; } // Kalman estimation (measurement update) - test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x); + test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri*0, new_data.pr_m, new_data.doppler_hz, kf_x); kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction // Kalman filter update step @@ -208,7 +208,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // sample code trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction - trk_cmd.carrier_freq_rate_hz_s = 0;//-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; + trk_cmd.carrier_freq_rate_hz_s =-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; trk_cmd.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; trk_cmd.enable_carrier_nco_cmd = true; trk_cmd.enable_code_nco_cmd = true; @@ -367,7 +367,9 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k double densidad=1.0; double ballistic_coef = 0.007; //vector velocidad - u_vec = kf_x.rows(4, 6); + kf_x.print("kf_x: "); + u_vec = kf_x.rows(3, 5); + // u_vec.print("u_vec"); // double u=sqrt(pow(kf_x(4),2)+pow(kf_x(5),2)+pow(kf_x(6),2)); //modulo de la velocidad @@ -391,10 +393,12 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k // % ballistic_coef is Cd0/mass_rocket; // ballistic_coef=CD0/mass_rocket; Empuje = EmpujeLkTable(t_disparo); - + cout<<"Empuje: "< Date: Wed, 15 Feb 2023 20:34:21 +0100 Subject: [PATCH 133/165] FIX: lkTable bug --- src/algorithms/PVT/libs/vtl_engine.cc | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index e5ecfc5d1..1359531e6 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -367,9 +367,9 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k double densidad=1.0; double ballistic_coef = 0.007; //vector velocidad - kf_x.print("kf_x: "); + u_vec = kf_x.rows(3, 5); - // u_vec.print("u_vec"); + u_vec.print("u_vec"); // double u=sqrt(pow(kf_x(4),2)+pow(kf_x(5),2)+pow(kf_x(6),2)); //modulo de la velocidad @@ -377,6 +377,7 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k if(u>4){ t_disparo=t_disparo+dt; + cout<<"t_disparo: "< Date: Thu, 16 Feb 2023 00:26:14 +0100 Subject: [PATCH 134/165] FIX: bug in vtlEngine --- src/algorithms/PVT/libs/vtl_engine.cc | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 1359531e6..51995847a 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -91,7 +91,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) double acc_x = 0; double acc_y = 0; double acc_z = 0; - test = model3DoF(acc_x,acc_x,acc_x,kf_x,kf_dt); + test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt); kf_x(6) = acc_x; kf_x(7) = acc_y; kf_x(8) = acc_z; @@ -208,7 +208,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // sample code trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction - trk_cmd.carrier_freq_rate_hz_s =-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; + trk_cmd.carrier_freq_rate_hz_s =-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8)) / Lambda_GPS_L1; trk_cmd.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; trk_cmd.enable_carrier_nco_cmd = true; trk_cmd.enable_code_nco_cmd = true; @@ -369,8 +369,6 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k //vector velocidad u_vec = kf_x.rows(3, 5); - u_vec.print("u_vec"); - // double u=sqrt(pow(kf_x(4),2)+pow(kf_x(5),2)+pow(kf_x(6),2)); //modulo de la velocidad double u = norm(u_vec, 2); @@ -382,10 +380,11 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k double mass_rocket=50.0; //50Kg if(t_disparo<.2){ - u_dir={.90828, -.13984, .388756}; + u_dir={.90828, -.13984, -.388756}; }else{ u_dir = u_vec / u; } + // u_dir.print("u_dir"); // lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]); // [T, sound_v, P, densidad] = atmosisa(lla(3)); // sound_v=320;% @ 5km and -17.5C @@ -399,7 +398,7 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k acc_vec = -(GNSS_PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir +gravity_ECEF+Empuje*u_dir; - //acc_vec.print("acc_vec"); + // acc_vec.print("acc_vec"); // % return acc_x = acc_vec(0); acc_y = acc_vec(1); From 1a50d75ee04470e6460df55079a9e2ce1b78b302 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Fri, 17 Feb 2023 17:48:35 +0100 Subject: [PATCH 135/165] MOD: async into rocket model --- src/algorithms/PVT/libs/rtklib_solver.cc | 45 +++-- src/algorithms/PVT/libs/vtl_engine.cc | 187 ++++++++++-------- src/algorithms/PVT/libs/vtl_engine.h | 2 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 34 ++-- 4 files changed, 154 insertions(+), 114 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 2cd0ab619..75a95731f 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1248,9 +1248,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.rx_dts(0) = rx_position_and_time[3]; vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] - //new_vtl_data.debug_print(); - //vtl_data.kf_state.print("kf_state_input"); vtl_engine.vtl_loop(vtl_data); + // cout<<"llegado aqui fuera"< &gnss_observables_ d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = vtl_data.rx_dts(1); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - for (int n = 0; n<6; n++) - { - tmp_double = vtl_data.sat_p(n, 0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_p(n, 1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_p(n, 2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.pr_m(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.doppler_hz(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_CN0_dB_hz(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } + // for (int n = 0; n<6; n++) + // { + // tmp_double = vtl_data.sat_p(n, 0); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_p(n, 1); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_p(n, 2); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_v(n, 0); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_v(n, 1); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_v(n, 2); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.pr_m(n); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.doppler_hz(n); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_CN0_dB_hz(n); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // } // GEO user position Latitude [deg] tmp_double = this->get_latitude(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 51995847a..fe0e8b612 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -36,6 +36,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) static uint64_t refSampleCounter = new_data.sample_counter; double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; refSampleCounter = new_data.sample_counter; + static double delta_t_cmd = 0; + bool flag_cmd = false; + delta_t_cmd = delta_t_cmd+delta_t_vtl; // update timer for vtl trk command + if(delta_t_cmd>=0.3){ + flag_cmd = true; + delta_t_cmd = 0; // reset timer for vtl trk command + } // ################## Kalman filter initialization ###################################### //State variables int n_of_states=11; @@ -91,7 +98,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) double acc_x = 0; double acc_y = 0; double acc_z = 0; - test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt); + test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt,counter); kf_x(6) = acc_x; kf_x(7) = acc_y; kf_x(8) = acc_z; @@ -122,12 +129,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 400.0;//*50.0/new_data.sat_CN0_dB_hz(i); - if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ - kf_R(i, i) = 10e4; - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; - kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 10e4; - cout<<"channel: "<90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ + // kf_R(i, i) = 10e4; + // kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; + // kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 10e4; + // cout<<"channel: "<5){ - dump_vtl_file << "pr_m" - << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) - << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)<5){ + // // dump_vtl_file << "pr_m" + // // << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) + // // << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)<4){ - t_disparo=t_disparo+dt; - cout<<"t_disparo: "<1500){ + if(u>6){ + t_disparo=t_disparo+dt; + std::cout<<"u : "<enable_carrier_nco_cmd){ + if(cmd->enable_code_nco_cmd){ + if(abs(d_x_old_old(2) - tmp_x(2))>50){ + std::cout <<"channel: "<< this->d_channel + << " tracking_cmd TOO FAR: " + << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + << " \n"; + }else{ + std::cout <<"channel: "<< this->d_channel + << " tracking_cmd NEAR: " + << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + << " \n"; + } + d_x_old_old(2) = tmp_x(2); //replace DOPPLER + // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE + + }else{ + // std::cout<<"yet to soon"<50){ - std::cout <<"channel: "<< this->d_channel - << " tracking_cmd TOO FAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; }else{ - std::cout <<"channel: "<< this->d_channel - << " tracking_cmd NEAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; - //d_x_old_old(2) = tmp_x(2); //replace DOPPLER - //d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE + // not 0.5 seg yet } From 1a3df78ba80125df91c018958941ec7602d36644 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 17 Feb 2023 21:58:00 +0000 Subject: [PATCH 136/165] MOD: remove sat variables from pvt.dat file --- src/algorithms/PVT/libs/rtklib_solver.cc | 104 +---------------------- 1 file changed, 3 insertions(+), 101 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 75a95731f..9cbecbced 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -205,7 +205,7 @@ bool Rtklib_Solver::save_matfile() const // READ DUMP FILE const std::string dump_filename = d_dump_filename; const int32_t number_of_double_vars = 23; //+2 MAGL - const int32_t number_of_double_vars_sat = 9; //+ pos(3)/vel(3)/psrange/doppler/CN0 for sat + const int32_t number_of_double_vars_sat = 0; //+ pos(3)/vel(3)/psrange/doppler/CN0 for sat const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; @@ -258,15 +258,6 @@ bool Rtklib_Solver::save_matfile() const auto clk_bias = std::vector(num_epoch); auto clk_drift = std::vector(num_epoch); - auto sat_posX_m = std::vector>(6, std::vector(num_epoch)); - auto sat_posY_m = std::vector>(6, std::vector(num_epoch)); - auto sat_posZ_m = std::vector>(6, std::vector(num_epoch)); - auto sat_velX = std::vector>(6, std::vector(num_epoch)); - auto sat_velY = std::vector>(6, std::vector(num_epoch)); - auto sat_velZ = std::vector>(6, std::vector(num_epoch)); - auto sat_prg_m = std::vector>(6, std::vector(num_epoch)); - auto sat_dopp_hz = std::vector>(6, std::vector(num_epoch)); - auto sat_CN0_dBhz = std::vector>(6, std::vector(num_epoch)); auto latitude = std::vector(num_epoch); auto longitude = std::vector(num_epoch); @@ -306,18 +297,7 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&clk_bias[i]), sizeof(double)); dump_file.read(reinterpret_cast(&clk_drift[i]), sizeof(double)); - for (uint32_t chan = 0; chan < 6; chan++) - { - dump_file.read(reinterpret_cast(&sat_posX_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_posY_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_posZ_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velX[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velY[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velZ[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_prg_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_dopp_hz[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_CN0_dBhz[chan][i]), sizeof(double)); - } + dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); @@ -340,33 +320,6 @@ bool Rtklib_Solver::save_matfile() const return false; } - auto sat_posX_m_aux = std::vector(6 * num_epoch); - auto sat_posY_m_aux = std::vector(6 * num_epoch); - auto sat_posZ_m_aux = std::vector(6 * num_epoch); - auto sat_velX_aux = std::vector(6 * num_epoch); - auto sat_velY_aux = std::vector(6 * num_epoch); - auto sat_velZ_aux = std::vector(6 * num_epoch); - auto sat_prg_m_aux = std::vector(6 * num_epoch); - auto sat_dopp_hz_aux = std::vector(6 * num_epoch); - auto sat_CN0_dBhz_aux = std::vector(6 * num_epoch); - - uint32_t k = 0U; - for (int64_t j = 0; j < num_epoch; j++) - { - for (uint32_t i = 0; i < 6; i++) - { - sat_posX_m_aux[k] = sat_posX_m[i][j]; - sat_posY_m_aux[k] = sat_posY_m[i][j]; - sat_posZ_m_aux[k] = sat_posZ_m[i][j]; - sat_velX_aux[k] = sat_velX[i][j]; - sat_velY_aux[k] = sat_velY[i][j]; - sat_velZ_aux[k] = sat_velZ[i][j]; - sat_prg_m_aux[k] = sat_prg_m[i][j]; - sat_dopp_hz_aux[k] = sat_dopp_hz[i][j]; - sat_CN0_dBhz_aux[k] = sat_CN0_dBhz[i][j]; - k++; - } - } // WRITE MAT FILE mat_t *matfp; @@ -450,36 +403,6 @@ bool Rtklib_Solver::save_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - std::array dims_sat{static_cast(6), static_cast(num_epoch)}; - matvar = Mat_VarCreate("sat_posX_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posX_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_posY_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posY_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_posZ_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posZ_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velX", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velX_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velY", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velY_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velZ", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velZ_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_prg_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_prg_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_dopp_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_dopp_hz_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_CN0_dBhz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_CN0_dBhz_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -1417,28 +1340,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = vtl_data.rx_dts(1); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // for (int n = 0; n<6; n++) - // { - // tmp_double = vtl_data.sat_p(n, 0); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_p(n, 1); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_p(n, 2); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_v(n, 0); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_v(n, 1); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_v(n, 2); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.pr_m(n); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.doppler_hz(n); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_CN0_dB_hz(n); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // } - + // GEO user position Latitude [deg] tmp_double = this->get_latitude(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); From 40d3e9690263190841831e30d136762d81b1d7a7 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Thu, 9 Mar 2023 21:30:15 +0100 Subject: [PATCH 137/165] ADD: vtl raw and mat files --- src/algorithms/PVT/libs/rtklib_solver.cc | 495 +++++++++++++----- src/algorithms/PVT/libs/rtklib_solver.h | 2 + src/algorithms/PVT/libs/vtl_data.cc | 92 +++- src/algorithms/PVT/libs/vtl_data.h | 11 + src/algorithms/PVT/libs/vtl_engine.cc | 3 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 12 +- 6 files changed, 476 insertions(+), 139 deletions(-) mode change 100644 => 100755 src/algorithms/PVT/libs/rtklib_solver.h diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 53a261eae..27a723118 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -162,6 +162,19 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what(); } } + if (d_vtl_dump_file.is_open() == false) + { + try + { + d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); + d_vtl_dump_file.open(d_dump_filename + "_vtl", std::ios::out | std::ios::binary); + LOG(INFO) << "PVT VTL dump enabled Log file: " << d_dump_filename + "_vtl"; + } + catch (const std::ofstream::failure &e) + { + LOG(WARNING) << "Exception opening VTL dump file " << e.what(); + } + } } } @@ -195,6 +208,7 @@ Rtklib_Solver::~Rtklib_Solver() try { save_matfile(); + save_vtl_matfile(); } catch (const std::exception &ex) { @@ -204,17 +218,266 @@ Rtklib_Solver::~Rtklib_Solver() } +bool Rtklib_Solver::save_vtl_matfile() const +{ + // READ DUMP FILE + const std::string dump_filename = d_dump_filename+ "_vtl"; + const int32_t number_of_double_vars = 27; + const int32_t number_of_uint32_vars = 2; + const int32_t number_of_uint8_vars = 1; + const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + + sizeof(uint32_t) * number_of_uint32_vars + + sizeof(uint8_t) * number_of_uint8_vars; + std::ifstream dump_file; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening VTL dump file:" << e.what() << '\n'; + return false; + } + // count number of epochs and rewind + int64_t num_epoch = 0LL; + if (dump_file.is_open()) + { + std::cout << "Generating .mat file for VTL " << dump_filename << '\n'; + const std::ifstream::pos_type size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } + else + { + return false; + } + + //todo: cambiarlo + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto week = std::vector(num_epoch); + auto RX_time = std::vector(num_epoch); + auto user_clk_offset = std::vector(num_epoch); + auto pos_x = std::vector(num_epoch); + auto pos_y = std::vector(num_epoch); + auto pos_z = std::vector(num_epoch); + auto vel_x = std::vector(num_epoch); + auto vel_y = std::vector(num_epoch); + auto vel_z = std::vector(num_epoch); + auto acc_x = std::vector(num_epoch); + auto acc_y = std::vector(num_epoch); + auto acc_z = std::vector(num_epoch); + auto cov_xx = std::vector(num_epoch); + auto cov_yy = std::vector(num_epoch); + auto cov_zz = std::vector(num_epoch); + auto cov_vx = std::vector(num_epoch); + auto cov_vy = std::vector(num_epoch); + auto cov_vz = std::vector(num_epoch); + auto cov_ax = std::vector(num_epoch); + auto cov_ay = std::vector(num_epoch); + auto cov_az = std::vector(num_epoch); + auto latitude = std::vector(num_epoch); + auto longitude = std::vector(num_epoch); + auto height = std::vector(num_epoch); + auto valid_sats = std::vector(num_epoch); + auto gdop = std::vector(num_epoch); + auto pdop = std::vector(num_epoch); + auto hdop = std::vector(num_epoch); + auto vdop = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(uint32_t)); + dump_file.read(reinterpret_cast(&week[i]), sizeof(uint32_t)); + dump_file.read(reinterpret_cast(&RX_time[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&user_clk_offset[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&acc_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&acc_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&acc_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_xx[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_yy[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_zz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_vx[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_vy[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_vz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_ax[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_ay[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_az[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&valid_sats[i]), sizeof(uint8_t)); + dump_file.read(reinterpret_cast(&gdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&hdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vdop[i]), sizeof(double)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return false; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("week", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), week.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), RX_time.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("user_clk_offset", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), user_clk_offset.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_x.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pos_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_y.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pos_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_z.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vel_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_x.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vel_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_y.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vel_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("acc_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_x.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("acc_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_y.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("acc_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_z.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_xx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_xx.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_yy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_yy.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_zz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_zz.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_vx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vx.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_vy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vy.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_vz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vz.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_ax", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_az.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_ay", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_ay.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_az", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_az.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("longitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), longitude.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("height", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), height.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("valid_sats", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), valid_sats.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("gdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), gdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("hdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), hdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + } + + Mat_Close(matfp); + return true; +} + + bool Rtklib_Solver::save_matfile() const { // READ DUMP FILE const std::string dump_filename = d_dump_filename; const int32_t number_of_double_vars = 23; //+2 MAGL - const int32_t number_of_double_vars_sat = 9; //+ pos(3)/vel(3)/psrange/doppler/CN0 for sat const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + - sizeof(double) * number_of_double_vars_sat * 6 + sizeof(uint32_t) * number_of_uint32_vars + sizeof(uint8_t) * number_of_uint8_vars + sizeof(float) * number_of_float_vars; @@ -259,19 +522,6 @@ bool Rtklib_Solver::save_matfile() const auto cov_xy = std::vector(num_epoch); auto cov_yz = std::vector(num_epoch); auto cov_zx = std::vector(num_epoch); - - auto clk_bias = std::vector(num_epoch); - auto clk_drift = std::vector(num_epoch); - auto sat_posX_m = std::vector>(6, std::vector(num_epoch)); - auto sat_posY_m = std::vector>(6, std::vector(num_epoch)); - auto sat_posZ_m = std::vector>(6, std::vector(num_epoch)); - auto sat_velX = std::vector>(6, std::vector(num_epoch)); - auto sat_velY = std::vector>(6, std::vector(num_epoch)); - auto sat_velZ = std::vector>(6, std::vector(num_epoch)); - auto sat_prg_m = std::vector>(6, std::vector(num_epoch)); - auto sat_dopp_hz = std::vector>(6, std::vector(num_epoch)); - auto sat_CN0_dBhz = std::vector>(6, std::vector(num_epoch)); - auto latitude = std::vector(num_epoch); auto longitude = std::vector(num_epoch); auto height = std::vector(num_epoch); @@ -307,21 +557,6 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&cov_xy[i]), sizeof(double)); dump_file.read(reinterpret_cast(&cov_yz[i]), sizeof(double)); dump_file.read(reinterpret_cast(&cov_zx[i]), sizeof(double)); - - dump_file.read(reinterpret_cast(&clk_bias[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&clk_drift[i]), sizeof(double)); - for (uint32_t chan = 0; chan < 6; chan++) - { - dump_file.read(reinterpret_cast(&sat_posX_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_posY_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_posZ_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velX[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velY[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velZ[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_prg_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_dopp_hz[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_CN0_dBhz[chan][i]), sizeof(double)); - } dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); @@ -344,34 +579,6 @@ bool Rtklib_Solver::save_matfile() const return false; } - auto sat_posX_m_aux = std::vector(6 * num_epoch); - auto sat_posY_m_aux = std::vector(6 * num_epoch); - auto sat_posZ_m_aux = std::vector(6 * num_epoch); - auto sat_velX_aux = std::vector(6 * num_epoch); - auto sat_velY_aux = std::vector(6 * num_epoch); - auto sat_velZ_aux = std::vector(6 * num_epoch); - auto sat_prg_m_aux = std::vector(6 * num_epoch); - auto sat_dopp_hz_aux = std::vector(6 * num_epoch); - auto sat_CN0_dBhz_aux = std::vector(6 * num_epoch); - - uint32_t k = 0U; - for (int64_t j = 0; j < num_epoch; j++) - { - for (uint32_t i = 0; i < 6; i++) - { - sat_posX_m_aux[k] = sat_posX_m[i][j]; - sat_posY_m_aux[k] = sat_posY_m[i][j]; - sat_posZ_m_aux[k] = sat_posZ_m[i][j]; - sat_velX_aux[k] = sat_velX[i][j]; - sat_velY_aux[k] = sat_velY[i][j]; - sat_velZ_aux[k] = sat_velZ[i][j]; - sat_prg_m_aux[k] = sat_prg_m[i][j]; - sat_dopp_hz_aux[k] = sat_dopp_hz[i][j]; - sat_CN0_dBhz_aux[k] = sat_CN0_dBhz[i][j]; - k++; - } - } - // WRITE MAT FILE mat_t *matfp; matvar_t *matvar; @@ -446,44 +653,6 @@ bool Rtklib_Solver::save_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("clk_bias_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_bias.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("clk_drift", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_drift.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - std::array dims_sat{static_cast(6), static_cast(num_epoch)}; - matvar = Mat_VarCreate("sat_posX_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posX_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_posY_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posY_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_posZ_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posZ_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velX", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velX_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velY", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velY_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velZ", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velZ_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_prg_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_prg_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_dopp_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_dopp_hz_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_CN0_dBhz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_CN0_dBhz_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -1713,13 +1882,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ //new_vtl_data.debug_print(); //vtl_data.kf_state.print("kf_state_input"); vtl_engine.vtl_loop(vtl_data); - //vtl_data.kf_state.print("kf_state_output"); - // pvt_sol.rr[0] = vtl_data.kf_state[0]; - // pvt_sol.rr[1] = vtl_data.kf_state[1]; - // pvt_sol.rr[2] = vtl_data.kf_state[2]; - // pvt_sol.rr[3] = vtl_data.kf_state[3]; - // pvt_sol.rr[4] = vtl_data.kf_state[4]; - // pvt_sol.rr[5] = vtl_data.kf_state[5]; + } else { @@ -1827,7 +1990,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ this->set_clock_drift_ppm(clock_drift_ppm); // User clock drift [ppm] d_monitor_pvt.user_clk_drift_ppm = clock_drift_ppm; - + // ######## LOG FILE ######### if (d_flag_dump_enabled == true) { @@ -1877,32 +2040,6 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = pvt_sol.qr[5]; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.rx_dts(0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.rx_dts(1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - for (int n = 0; n < 6; n++) - { - tmp_double = vtl_data.sat_p(n, 0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_p(n, 1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_p(n, 2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.pr_m(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.doppler_hz(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_CN0_dB_hz(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - // GEO user position Latitude [deg] tmp_double = this->get_latitude(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); @@ -1934,6 +2071,104 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what(); } + + // VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file + if (enable_vtl == true) + + try + { + + double tmp_double; + uint32_t tmp_uint32; + // TOW + tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; + d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // WEEK + tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); + d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // PVT GPS time + tmp_double = gnss_observables_map.cbegin()->second.RX_time; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // User clock offset [s] + //tmp_double = rx_position_and_time[3]; + tmp_double = vtl_data.get_user_clock_offset_s(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double) + std::vector p_vec_m = vtl_data.get_position_ecef_m(); + tmp_double = p_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + std::vector v_vec_m = vtl_data.get_velocity_ecef_m_s(); + tmp_double = v_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + std::vector a_vec_m = vtl_data.get_accel_ecef_m_s2(); + tmp_double = a_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // position/velocity/acceleration variance/ (units^2) (9 x double) + + std::vector p_var_vec_m = vtl_data.get_position_var_ecef_m(); + tmp_double = p_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + + std::vector v_var_vec_m = vtl_data.get_velocity_var_ecef_m_s(); + tmp_double = v_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + vector a_var_vec_m = vtl_data.get_accel_var_ecef_m_s2(); + tmp_double = a_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // GEO user position Latitude [deg] + tmp_double = this->get_latitude(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Longitude [deg] + tmp_double = this->get_longitude(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Height [m] + tmp_double = this->get_height(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // NUMBER OF VALID SATS + d_vtl_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); + + // GDOP / PDOP / HDOP / VDOP + d_vtl_dump_file.write(reinterpret_cast(&d_dop[0]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[1]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[2]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[3]), sizeof(double)); + } + catch (const std::ofstream::failure &e) + { + LOG(WARNING) << "Exception writing VTL dump file " << e.what(); + } } } } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h old mode 100644 new mode 100755 index 61ce86660..d6781beb8 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -132,6 +132,7 @@ public: private: bool save_matfile() const; + bool save_vtl_matfile() const; void check_has_orbit_clock_validity(const std::map& obs_map); void get_has_biases(const std::map& obs_map); @@ -152,6 +153,7 @@ private: std::string d_dump_filename; std::ofstream d_dump_file; + std::ofstream d_vtl_dump_file; rtk_t d_rtk{}; nav_t d_nav_data{}; Monitor_Pvt d_monitor_pvt{}; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index f1c5e48b8..ddfb2bc78 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -16,13 +16,15 @@ #include "vtl_data.h" +#include "vector" +#include "armadillo" Vtl_Data::Vtl_Data() { epoch_tow_s = 0; sample_counter = 0; - kf_state = arma::mat(8,1); - kf_P = arma::mat(8,8); + kf_state = arma::mat(11,1); + kf_P = arma::mat(11,11); } void Vtl_Data::init_storage(int n_sats) @@ -66,3 +68,89 @@ void Vtl_Data::debug_print() //doppler_hz.print("satellite Carrier Dopplers [Hz]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } + +std::vector Vtl_Data::get_position_ecef_m() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_state[0]; + temp[1] = kf_state[1]; + temp[2] = kf_state[2]; + + return temp; +} + +std::vector Vtl_Data::get_velocity_ecef_m_s() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_state[3]; + temp[1] = kf_state[4]; + temp[2] = kf_state[5]; + + return temp; +} + +std::vector Vtl_Data::get_accel_ecef_m_s2() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_state[6]; + temp[1] = kf_state[7]; + temp[2] = kf_state[8]; + + return temp; +} +std::vector Vtl_Data::get_position_var_ecef_m() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_P(0,0); + temp[1] = kf_P(1,1); + temp[2] = kf_P(2,2); + + return temp; +} + +std::vector Vtl_Data::get_velocity_var_ecef_m_s() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_P(3,3); + temp[1] = kf_P(4,4); + temp[2] = kf_P(5,5); + + return temp; +} + +std::vector Vtl_Data::get_accel_var_ecef_m_s2() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_P(6,6); + temp[1] = kf_P(7,7); + temp[2] = kf_P(8,8); + + return temp; +} + +double Vtl_Data::get_latitude() +{ + + return -1.0; +} + +double Vtl_Data::get_longitude() +{ + + + return -1.0; +} + +double Vtl_Data::get_height() +{ + return -1.0; +} + +double Vtl_Data::get_user_clock_offset_s() +{ + double temp=0; + temp=kf_state[9]; + + return temp; +} + diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 787c69f13..182098ba2 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -22,6 +22,7 @@ #include #include #include +#include /** \addtogroup PVT * \{ */ @@ -61,6 +62,16 @@ public: double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] void debug_print(); + std::vector get_position_ecef_m(); // get_position_ecef_m + std::vector get_velocity_ecef_m_s(); // get_velocity_ecef_m_s + std::vector get_accel_ecef_m_s2(); // get_accel_ecef_m_s2 + std::vector get_position_var_ecef_m(); // get_position_var_ecef_m + std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s + std::vector get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 + double get_latitude(); // get_latitude + double get_longitude(); // get_longitude + double get_height(); // get_height + double get_user_clock_offset_s(); // get_user_clock_offset_s; }; diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 56fd0e876..08a212083 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -172,7 +172,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_xerr = kf_K * (kf_yerr); // Error state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) - + new_data.kf_state = kf_x; + new_data.kf_P = kf_P_x; if(abs(delta_t_vtl)>.5){ kf_xerr.print("kf_xERR: "); cout<<"kf_dt: "<50){ - std::cout << " tracking_cmd TOO FAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; + // std::cout << " tracking_cmd TOO FAR: " + // << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + // << " \n"; }else{ - std::cout << " tracking_cmd NEAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; + // std::cout << " tracking_cmd NEAR: " + // << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + // << " \n"; //d_x_old_old(2) = tmp_x(2); //replace the Code Phase state } From 5b44ec996535585c5625b91f36e8d29b0fd21416 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Thu, 9 Mar 2023 23:52:05 +0100 Subject: [PATCH 138/165] MOD: remove static variables --- src/algorithms/PVT/libs/rtklib_solver.cc | 14 +-- src/algorithms/PVT/libs/vtl_data.cc | 86 ------------------- src/algorithms/PVT/libs/vtl_data.h | 10 --- src/algorithms/PVT/libs/vtl_engine.cc | 104 +++++++++++++++++++++-- src/algorithms/PVT/libs/vtl_engine.h | 18 +++- 5 files changed, 121 insertions(+), 111 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 27a723118..8f7c827c8 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -2091,11 +2091,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // User clock offset [s] //tmp_double = rx_position_and_time[3]; - tmp_double = vtl_data.get_user_clock_offset_s(); + tmp_double = vtl_engine.get_user_clock_offset_s(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double) - std::vector p_vec_m = vtl_data.get_position_ecef_m(); + std::vector p_vec_m = vtl_engine.get_position_ecef_m(); tmp_double = p_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = p_vec_m[1]; @@ -2103,7 +2103,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = p_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector v_vec_m = vtl_data.get_velocity_ecef_m_s(); + std::vector v_vec_m = vtl_engine.get_velocity_ecef_m_s(); tmp_double = v_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = v_vec_m[1]; @@ -2111,7 +2111,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = v_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector a_vec_m = vtl_data.get_accel_ecef_m_s2(); + std::vector a_vec_m = vtl_engine.get_accel_ecef_m_s2(); tmp_double = a_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = a_vec_m[1]; @@ -2121,7 +2121,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // position/velocity/acceleration variance/ (units^2) (9 x double) - std::vector p_var_vec_m = vtl_data.get_position_var_ecef_m(); + std::vector p_var_vec_m = vtl_engine.get_position_var_ecef_m(); tmp_double = p_var_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = p_var_vec_m[1]; @@ -2130,7 +2130,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector v_var_vec_m = vtl_data.get_velocity_var_ecef_m_s(); + std::vector v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); tmp_double = v_var_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = v_var_vec_m[1]; @@ -2138,7 +2138,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = v_var_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - vector a_var_vec_m = vtl_data.get_accel_var_ecef_m_s2(); + vector a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); tmp_double = a_var_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = a_var_vec_m[1]; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index ddfb2bc78..74057a159 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -23,8 +23,6 @@ Vtl_Data::Vtl_Data() { epoch_tow_s = 0; sample_counter = 0; - kf_state = arma::mat(11,1); - kf_P = arma::mat(11,11); } void Vtl_Data::init_storage(int n_sats) @@ -69,88 +67,4 @@ void Vtl_Data::debug_print() // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } -std::vector Vtl_Data::get_position_ecef_m() -{ - std::vector temp = {42,42,42}; - temp[0] = kf_state[0]; - temp[1] = kf_state[1]; - temp[2] = kf_state[2]; - - return temp; -} - -std::vector Vtl_Data::get_velocity_ecef_m_s() -{ - std::vector temp = {42,42,42}; - temp[0] = kf_state[3]; - temp[1] = kf_state[4]; - temp[2] = kf_state[5]; - - return temp; -} - -std::vector Vtl_Data::get_accel_ecef_m_s2() -{ - std::vector temp = {42,42,42}; - temp[0] = kf_state[6]; - temp[1] = kf_state[7]; - temp[2] = kf_state[8]; - - return temp; -} -std::vector Vtl_Data::get_position_var_ecef_m() -{ - std::vector temp = {42,42,42}; - temp[0] = kf_P(0,0); - temp[1] = kf_P(1,1); - temp[2] = kf_P(2,2); - - return temp; -} - -std::vector Vtl_Data::get_velocity_var_ecef_m_s() -{ - std::vector temp = {42,42,42}; - temp[0] = kf_P(3,3); - temp[1] = kf_P(4,4); - temp[2] = kf_P(5,5); - - return temp; -} - -std::vector Vtl_Data::get_accel_var_ecef_m_s2() -{ - std::vector temp = {42,42,42}; - temp[0] = kf_P(6,6); - temp[1] = kf_P(7,7); - temp[2] = kf_P(8,8); - - return temp; -} - -double Vtl_Data::get_latitude() -{ - - return -1.0; -} - -double Vtl_Data::get_longitude() -{ - - - return -1.0; -} - -double Vtl_Data::get_height() -{ - return -1.0; -} - -double Vtl_Data::get_user_clock_offset_s() -{ - double temp=0; - temp=kf_state[9]; - - return temp; -} diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 182098ba2..b01e49ffe 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -62,16 +62,6 @@ public: double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] void debug_print(); - std::vector get_position_ecef_m(); // get_position_ecef_m - std::vector get_velocity_ecef_m_s(); // get_velocity_ecef_m_s - std::vector get_accel_ecef_m_s2(); // get_accel_ecef_m_s2 - std::vector get_position_var_ecef_m(); // get_position_var_ecef_m - std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s - std::vector get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 - double get_latitude(); // get_latitude - double get_longitude(); // get_longitude - double get_height(); // get_height - double get_user_clock_offset_s(); // get_user_clock_offset_s; }; diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 08a212083..6cae756df 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -22,25 +22,31 @@ using namespace std; Vtl_Engine::Vtl_Engine() { + counter=0; + refSampleCounter=0; + n_of_states=11; + kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; + kf_x = arma::zeros(n_of_states, 1); } Vtl_Engine::~Vtl_Engine() { } -bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) +bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; - static uint64_t refSampleCounter = new_data.sample_counter; + if (refSampleCounter=0) + { + refSampleCounter=new_data.sample_counter; + } double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; refSampleCounter = new_data.sample_counter; // ################## Kalman filter initialization ###################################### //State variables - int n_of_states=11; - static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; - static arma::mat kf_x = arma::zeros(n_of_states, 1); + arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) @@ -58,8 +64,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix kf_K = arma::zeros(n_of_states, 2 * new_data.sat_number); ; // ################## Kalman Tracking ###################################### - static uint32_t counter = 0; //counter - counter = counter + 1; //uint64_t + counter++; //uint64_t //new_data.kf_state.print("new_data kf initial"); uint32_t closure_point=3; @@ -338,4 +343,89 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1+kf_x(10)) - rhoDot_pri(i); } return -1; +} + +std::vector Vtl_Engine::get_position_ecef_m() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_x[0]; + temp[1] = kf_x[1]; + temp[2] = kf_x[2]; + + return temp; +} + +std::vector Vtl_Engine::get_velocity_ecef_m_s() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_x[3]; + temp[1] = kf_x[4]; + temp[2] = kf_x[5]; + + return temp; +} + +std::vector Vtl_Engine::get_accel_ecef_m_s2() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_x[6]; + temp[1] = kf_x[7]; + temp[2] = kf_x[8]; + + return temp; +} +std::vector Vtl_Engine::get_position_var_ecef_m() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_P_x(0,0); + temp[1] = kf_P_x(1,1); + temp[2] = kf_P_x(2,2); + + return temp; +} + +std::vector Vtl_Engine::get_velocity_var_ecef_m_s() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_P_x(3,3); + temp[1] = kf_P_x(4,4); + temp[2] = kf_P_x(5,5); + + return temp; +} + +std::vector Vtl_Engine::get_accel_var_ecef_m_s2() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_P_x(6,6); + temp[1] = kf_P_x(7,7); + temp[2] = kf_P_x(8,8); + + return temp; +} + +double Vtl_Engine::get_latitude() +{ + + return -1.0; +} + +double Vtl_Engine::get_longitude() +{ + + + return -1.0; +} + +double Vtl_Engine::get_height() +{ + return -1.0; +} + +double Vtl_Engine::get_user_clock_offset_s() +{ + double temp=0; + temp = kf_x[9]; + + return temp; } \ No newline at end of file diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 4cce7371b..dd13d6f4d 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -42,11 +42,21 @@ public: void configure(Vtl_Conf config_); //set config parameters //TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...) - bool vtl_loop(Vtl_Data& new_data); + bool vtl_loop(Vtl_Data new_data); void reset(); // reset all internal states void debug_print(); // print debug information std::vector trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs + std::vector get_position_ecef_m(); // get_position_ecef_m + std::vector get_velocity_ecef_m_s(); // get_velocity_ecef_m_s + std::vector get_accel_ecef_m_s2(); // get_accel_ecef_m_s2 + std::vector get_position_var_ecef_m(); // get_position_var_ecef_m + std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s + std::vector get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 + double get_latitude(); // get_latitude + double get_longitude(); // get_longitude + double get_height(); // get_height + double get_user_clock_offset_s(); // get_user_clock_offset_s; private: Vtl_Conf config; @@ -67,6 +77,7 @@ private: arma::mat kf_P_x_ini; // initial state error covariance matrix // arma::mat kf_P_x; // state error covariance matrix arma::mat kf_P_x_pre; // Predicted state error covariance matrix + arma::mat kf_P_x; arma::mat kf_S; // innovation covariance matrix arma::mat kf_F; // state transition matrix @@ -84,6 +95,11 @@ private: // Gaussian estimator arma::mat kf_R_est; // measurement error covariance + + uint32_t counter; + int n_of_states; + uint64_t refSampleCounter; + bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation From 0dab0311277f3c9ab26b698424d041e432004f49 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Sat, 11 Mar 2023 18:47:11 +0100 Subject: [PATCH 139/165] fix: name of vtl raw dump file --- src/algorithms/PVT/libs/rtklib_solver.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 589e6ae08..2b474868d 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -167,7 +167,8 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, try { d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); - d_vtl_dump_file.open(d_dump_filename + "_vtl", std::ios::out | std::ios::binary); + int end_filename = d_dump_filename.length()-4; + d_vtl_dump_file.open(d_dump_filename.insert(end_filename, "_vtl"), std::ios::out | std::ios::binary); LOG(INFO) << "PVT VTL dump enabled Log file: " << d_dump_filename + "_vtl"; } catch (const std::ofstream::failure &e) From fd69416f4e602d0c1a28e566ba2110fbccc227b6 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 11 Mar 2023 18:56:08 +0000 Subject: [PATCH 140/165] fix: bug in mat vtl and pvt files --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 3 ++ src/algorithms/PVT/libs/rtklib_solver.cc | 28 +++++++++++++------ src/algorithms/PVT/libs/rtklib_solver.h | 1 + 3 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index e43cda93f..236cd78d6 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -251,6 +251,9 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, std::cerr << "GNSS-SDR cannot create dump file for the PVT block. Wrong permissions?\n"; d_dump = false; } + // TODO: if(vtl_enable) then + // uint end_filename = d_dump_filename.length()-4; + // d_vtl_dump_filename = d_dump_filename.insert(end_filename, "_vtl"); } // initialize kml_printer diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 2b474868d..f5696a96b 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -162,14 +162,17 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what(); } } + // TODO: if(vtl_enable) then if (d_vtl_dump_file.is_open() == false) { try { d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); - int end_filename = d_dump_filename.length()-4; - d_vtl_dump_file.open(d_dump_filename.insert(end_filename, "_vtl"), std::ios::out | std::ios::binary); - LOG(INFO) << "PVT VTL dump enabled Log file: " << d_dump_filename + "_vtl"; + uint end_filename = d_dump_filename.length()-4; + d_vtl_dump_filename = d_dump_filename; + d_vtl_dump_filename = d_vtl_dump_filename.insert(end_filename, "_vtl"); + d_vtl_dump_file.open(d_vtl_dump_filename, std::ios::out | std::ios::binary); + LOG(INFO) << "PVT VTL dump enabled Log file: " << d_vtl_dump_filename; } catch (const std::ofstream::failure &e) { @@ -209,20 +212,30 @@ Rtklib_Solver::~Rtklib_Solver() try { save_matfile(); - save_vtl_matfile(); } catch (const std::exception &ex) { LOG(WARNING) << "Exception in destructor saving the PVT .mat dump file " << ex.what(); } } + if (d_flag_dump_mat_enabled) + { + try + { + save_vtl_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor saving the PVT VTL .mat dump file " << ex.what(); + } + } } bool Rtklib_Solver::save_vtl_matfile() const { // READ DUMP FILE - const std::string dump_filename = d_dump_filename+ "_vtl"; + const std::string dump_filename = d_vtl_dump_filename; const int32_t number_of_double_vars = 27; const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 1; @@ -233,7 +246,7 @@ bool Rtklib_Solver::save_vtl_matfile() const dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); try { - dump_file.open(dump_filename.c_str(), std::ios::binary | std::ios::ate); + dump_file.open(dump_filename, std::ios::binary | std::ios::ate); } catch (const std::ifstream::failure &e) { @@ -254,7 +267,6 @@ bool Rtklib_Solver::save_vtl_matfile() const return false; } - //todo: cambiarlo auto TOW_at_current_symbol_ms = std::vector(num_epoch); auto week = std::vector(num_epoch); auto RX_time = std::vector(num_epoch); @@ -328,7 +340,7 @@ bool Rtklib_Solver::save_vtl_matfile() const } catch (const std::ifstream::failure &e) { - std::cerr << "Problem reading dump file:" << e.what() << '\n'; + std::cerr << "Problem reading VTL dump file:" << e.what() << '\n'; return false; } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index d6781beb8..3f3b05ad3 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -152,6 +152,7 @@ private: std::map> d_has_obs_corr_map; // first key is signal, second key is PRN std::string d_dump_filename; + std::string d_vtl_dump_filename; std::ofstream d_dump_file; std::ofstream d_vtl_dump_file; rtk_t d_rtk{}; From b815ee4d9dd45bdd6c1efb7d2216926aa61b4547 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 11 Mar 2023 19:32:16 +0000 Subject: [PATCH 141/165] FORMAT: clang-format applied --- src/algorithms/PVT/libs/rtklib_solver.cc | 34 +- src/algorithms/PVT/libs/vtl_data.cc | 8 +- src/algorithms/PVT/libs/vtl_data.h | 32 +- src/algorithms/PVT/libs/vtl_engine.cc | 767 +++++++++--------- src/algorithms/PVT/libs/vtl_engine.h | 42 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 77 +- 6 files changed, 489 insertions(+), 471 deletions(-) mode change 100755 => 100644 src/algorithms/PVT/libs/rtklib_solver.cc mode change 100755 => 100644 src/algorithms/PVT/libs/vtl_data.cc mode change 100755 => 100644 src/algorithms/PVT/libs/vtl_data.h mode change 100755 => 100644 src/algorithms/PVT/libs/vtl_engine.cc mode change 100755 => 100644 src/algorithms/PVT/libs/vtl_engine.h diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc old mode 100755 new mode 100644 index f5696a96b..7dbc2f711 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -168,7 +168,7 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, try { d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); - uint end_filename = d_dump_filename.length()-4; + uint end_filename = d_dump_filename.length() - 4; d_vtl_dump_filename = d_dump_filename; d_vtl_dump_filename = d_vtl_dump_filename.insert(end_filename, "_vtl"); d_vtl_dump_file.open(d_vtl_dump_filename, std::ios::out | std::ios::binary); @@ -218,7 +218,7 @@ Rtklib_Solver::~Rtklib_Solver() LOG(WARNING) << "Exception in destructor saving the PVT .mat dump file " << ex.what(); } } - if (d_flag_dump_mat_enabled) + if (d_flag_dump_mat_enabled) { try { @@ -236,7 +236,7 @@ bool Rtklib_Solver::save_vtl_matfile() const { // READ DUMP FILE const std::string dump_filename = d_vtl_dump_filename; - const int32_t number_of_double_vars = 27; + const int32_t number_of_double_vars = 27; const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 1; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + @@ -353,7 +353,6 @@ bool Rtklib_Solver::save_vtl_matfile() const matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); if (reinterpret_cast(matfp) != nullptr) { - std::array dims{1, static_cast(num_epoch)}; matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE @@ -474,7 +473,6 @@ bool Rtklib_Solver::save_vtl_matfile() const matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - } Mat_Close(matfp); @@ -486,7 +484,7 @@ bool Rtklib_Solver::save_matfile() const { // READ DUMP FILE const std::string dump_filename = d_dump_filename; - const int32_t number_of_double_vars = 21; + const int32_t number_of_double_vars = 21; const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; @@ -1893,7 +1891,6 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] vtl_engine.vtl_loop(vtl_data); - } else { @@ -2001,7 +1998,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ this->set_clock_drift_ppm(clock_drift_ppm); // User clock drift [ppm] d_monitor_pvt.user_clk_drift_ppm = clock_drift_ppm; - + // ######## LOG FILE ######### if (d_flag_dump_enabled == true) { @@ -2085,10 +2082,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file if (enable_vtl == true) - + try { - double tmp_double; uint32_t tmp_uint32; // TOW @@ -2104,7 +2100,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ //tmp_double = rx_position_and_time[3]; tmp_double = vtl_engine.get_user_clock_offset_s(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double) std::vector p_vec_m = vtl_engine.get_position_ecef_m(); tmp_double = p_vec_m[0]; @@ -2113,8 +2109,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = p_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - - std::vector v_vec_m = vtl_engine.get_velocity_ecef_m_s(); + + std::vector v_vec_m = vtl_engine.get_velocity_ecef_m_s(); tmp_double = v_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = v_vec_m[1]; @@ -2122,7 +2118,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = v_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector a_vec_m = vtl_engine.get_accel_ecef_m_s2(); + std::vector a_vec_m = vtl_engine.get_accel_ecef_m_s2(); tmp_double = a_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = a_vec_m[1]; @@ -2132,7 +2128,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // position/velocity/acceleration variance/ (units^2) (9 x double) - std::vector p_var_vec_m = vtl_engine.get_position_var_ecef_m(); + std::vector p_var_vec_m = vtl_engine.get_position_var_ecef_m(); tmp_double = p_var_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = p_var_vec_m[1]; @@ -2141,7 +2137,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); + std::vector v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); tmp_double = v_var_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = v_var_vec_m[1]; @@ -2149,7 +2145,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = v_var_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - vector a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); + vector a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); tmp_double = a_var_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = a_var_vec_m[1]; @@ -2158,13 +2154,13 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Latitude [deg] - tmp_double = this->get_latitude(); + tmp_double = this->get_latitude(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Longitude [deg] tmp_double = this->get_longitude(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Height [m] - tmp_double = this->get_height(); + tmp_double = this->get_height(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // NUMBER OF VALID SATS diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc old mode 100755 new mode 100644 index 74057a159..b7d5d7fd7 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -16,8 +16,8 @@ #include "vtl_data.h" -#include "vector" #include "armadillo" +#include "vector" Vtl_Data::Vtl_Data() { @@ -46,7 +46,7 @@ void Vtl_Data::init_storage(int n_sats) rx_dts = arma::mat(1, 2); rx_var = arma::vec(1); rx_pvt_var = arma::vec(8); - + epoch_tow_s = 0; sample_counter = 0; } @@ -61,10 +61,8 @@ void Vtl_Data::debug_print() // sat_health_flag.print("VTL Sat health"); //sat_LOS.print("VTL SAT LOS"); kf_state.print("EKF STATE"); - + //pr_m.print("Satellite Code pseudoranges [m]"); //doppler_hz.print("satellite Carrier Dopplers [Hz]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } - - diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h old mode 100755 new mode 100644 index b01e49ffe..502b99351 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -36,29 +36,29 @@ public: Vtl_Data(); void init_storage(int n_sats); - arma::mat sat_p; // Satellite ECEF Position [m] - arma::mat sat_v; // Satellite Velocity [m/s] - arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] + arma::mat sat_p; // Satellite ECEF Position [m] + arma::mat sat_v; // Satellite Velocity [m/s] + arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] arma::colvec sat_var; // sat position and clock error variance [m^2] arma::colvec sat_health_flag; // sat health flag (0 is ok) - arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz - arma::mat sat_LOS; // sat LOS - int sat_number; // on-view sat number - + arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz + arma::mat sat_LOS; // sat LOS + int sat_number; // on-view sat number + arma::colvec pr_m; // Satellite Code pseudoranges [m] arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz] arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads] arma::colvec pr_res; // pseudorange residual - - arma::mat rx_p; // Receiver ENU Position [m] - arma::mat rx_v; // Receiver Velocity [m/s] - arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] - arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] - arma::colvec rx_var; // Receiver position and clock error variance [m^2] - arma::mat kf_state; // KF STATE - arma::mat kf_P; // KF STATE covariance + + arma::mat rx_p; // Receiver ENU Position [m] + arma::mat rx_v; // Receiver Velocity [m/s] + arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] + arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] + arma::colvec rx_var; // Receiver position and clock error variance [m^2] + arma::mat kf_state; // KF STATE + arma::mat kf_P; // KF STATE covariance // time handling - double PV[6]; // position and Velocity + double PV[6]; // position and Velocity double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] void debug_print(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc old mode 100755 new mode 100644 index 7384e2659..46aeb28b2 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -22,10 +22,10 @@ using namespace std; Vtl_Engine::Vtl_Engine() { - counter=0; - refSampleCounter=0; - n_of_states=11; - kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; + counter = 0; + refSampleCounter = 0; + n_of_states = 11; + kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; kf_x = arma::zeros(n_of_states, 1); } @@ -37,48 +37,50 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; - - if (refSampleCounter=0) - { - refSampleCounter=new_data.sample_counter; - } - double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; + + if (refSampleCounter = 0) + { + refSampleCounter = new_data.sample_counter; + } + double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; refSampleCounter = new_data.sample_counter; static double delta_t_cmd = 0; bool flag_cmd = false; - delta_t_cmd = delta_t_cmd+delta_t_vtl; // update timer for vtl trk command - if(delta_t_cmd>=0.3){ - flag_cmd = true; - delta_t_cmd = 0; // reset timer for vtl trk command - } + delta_t_cmd = delta_t_cmd + delta_t_vtl; // update timer for vtl trk command + if (delta_t_cmd >= 0.3) + { + flag_cmd = true; + delta_t_cmd = 0; // reset timer for vtl trk command + } // ################## Kalman filter initialization ###################################### //State variables arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) - + kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); - double kf_dt = delta_t_vtl; //0.05; + double kf_dt = delta_t_vtl; //0.05; kf_Q = arma::eye(n_of_states, n_of_states); kf_F = arma::eye(n_of_states, n_of_states); - bool test = kf_F_fill(kf_F,kf_dt); + bool test = kf_F_fill(kf_F, kf_dt); //kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); kf_y = arma::zeros(3 * new_data.sat_number, 1); kf_yerr = arma::zeros(3 * new_data.sat_number, 1); kf_xerr = arma::zeros(n_of_states, 1); kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix - kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); ; + kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); + ; // ################## Kalman Tracking ###################################### - counter++; //uint64_t + counter++; //uint64_t //new_data.kf_state.print("new_data kf initial"); - uint32_t closure_point=3; - + uint32_t closure_point = 3; + if (counter < closure_point) - { + { // // receiver solution from rtklib_solver - kf_dx=kf_x; + kf_dx = kf_x; kf_x(0) = new_data.rx_p(0); kf_x(1) = new_data.rx_p(1); kf_x(2) = new_data.rx_p(2); @@ -91,7 +93,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; - kf_dx = kf_x-kf_dx; + kf_dx = kf_x - kf_dx; kf_dx = kf_F * kf_dx; // state prediction } else @@ -100,7 +102,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) double acc_x = 0; double acc_y = 0; double acc_z = 0; - test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt,counter); + test = model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter); kf_x(6) = acc_x; kf_x(7) = acc_y; kf_x(8) = acc_z; @@ -108,7 +110,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; // kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; } - + // State error Covariance Matrix Q (PVT) //careful, values for V and T could not be adecuate. kf_Q(0, 0) = 100.0; @@ -122,15 +124,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_Q(8, 8) = .10; kf_Q(9, 9) = 4.0; kf_Q(10, 10) = 10.0; - + // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); - kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 400.0;//*50.0/new_data.sat_CN0_dB_hz(i); - + kf_R(i, i) = 80.0; //*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; //*50.0/new_data.sat_CN0_dB_hz(i); + kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; //*50.0/new_data.sat_CN0_dB_hz(i); + // if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ // kf_R(i, i) = 10e4; // kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; @@ -159,7 +161,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) a_y = arma::zeros(new_data.sat_number, 1); a_z = arma::zeros(new_data.sat_number, 1); // cout<<"llegado aqui tambien"<5){ + // if(new_data.sat_number>5){ // // dump_vtl_file << "pr_m" - // // << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) + // // << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) // // << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)<1500){ - if(u>6){ - t_disparo=t_disparo+dt; - std::cout<<"u : "< 1500) + { + if (u > 6) + { + t_disparo = t_disparo + dt; + std::cout << "u : " << u << endl; + double diam_cohete = 120.0e-3; // 120 mm + double mass_rocket = 50.0; //50Kg - // acc_vec.print("acc_vec"); - // % return - cout<<"modelo 3Dof actuando,t:"< Vtl_Engine::get_position_ecef_m() { - std::vector temp = {42,42,42}; + std::vector temp = {42, 42, 42}; temp[0] = kf_x[0]; temp[1] = kf_x[1]; temp[2] = kf_x[2]; @@ -655,7 +677,7 @@ std::vector Vtl_Engine::get_position_ecef_m() std::vector Vtl_Engine::get_velocity_ecef_m_s() { - std::vector temp = {42,42,42}; + std::vector temp = {42, 42, 42}; temp[0] = kf_x[3]; temp[1] = kf_x[4]; temp[2] = kf_x[5]; @@ -665,7 +687,7 @@ std::vector Vtl_Engine::get_velocity_ecef_m_s() std::vector Vtl_Engine::get_accel_ecef_m_s2() { - std::vector temp = {42,42,42}; + std::vector temp = {42, 42, 42}; temp[0] = kf_x[6]; temp[1] = kf_x[7]; temp[2] = kf_x[8]; @@ -674,44 +696,41 @@ std::vector Vtl_Engine::get_accel_ecef_m_s2() } std::vector Vtl_Engine::get_position_var_ecef_m() { - std::vector temp = {42,42,42}; - temp[0] = kf_P_x(0,0); - temp[1] = kf_P_x(1,1); - temp[2] = kf_P_x(2,2); + std::vector temp = {42, 42, 42}; + temp[0] = kf_P_x(0, 0); + temp[1] = kf_P_x(1, 1); + temp[2] = kf_P_x(2, 2); return temp; } std::vector Vtl_Engine::get_velocity_var_ecef_m_s() { - std::vector temp = {42,42,42}; - temp[0] = kf_P_x(3,3); - temp[1] = kf_P_x(4,4); - temp[2] = kf_P_x(5,5); + std::vector temp = {42, 42, 42}; + temp[0] = kf_P_x(3, 3); + temp[1] = kf_P_x(4, 4); + temp[2] = kf_P_x(5, 5); return temp; } std::vector Vtl_Engine::get_accel_var_ecef_m_s2() { - std::vector temp = {42,42,42}; - temp[0] = kf_P_x(6,6); - temp[1] = kf_P_x(7,7); - temp[2] = kf_P_x(8,8); + std::vector temp = {42, 42, 42}; + temp[0] = kf_P_x(6, 6); + temp[1] = kf_P_x(7, 7); + temp[2] = kf_P_x(8, 8); return temp; } double Vtl_Engine::get_latitude() { - return -1.0; } double Vtl_Engine::get_longitude() { - - return -1.0; } @@ -722,7 +741,7 @@ double Vtl_Engine::get_height() double Vtl_Engine::get_user_clock_offset_s() { - double temp=0; + double temp = 0; temp = kf_x[9]; return temp; diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h old mode 100755 new mode 100644 index 751e30362..5317114a1 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -17,10 +17,10 @@ #ifndef GNSS_SDR_VTL_ENGINE_H #define GNSS_SDR_VTL_ENGINE_H +#include "MATH_CONSTANTS.h" #include "trackingcmd.h" #include "vtl_conf.h" #include "vtl_data.h" -#include "MATH_CONSTANTS.h" #include #include #include @@ -46,22 +46,22 @@ public: void reset(); // reset all internal states void debug_print(); // print debug information - std::vector trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs - std::vector get_position_ecef_m(); // get_position_ecef_m - std::vector get_velocity_ecef_m_s(); // get_velocity_ecef_m_s - std::vector get_accel_ecef_m_s2(); // get_accel_ecef_m_s2 - std::vector get_position_var_ecef_m(); // get_position_var_ecef_m - std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s - std::vector get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 - double get_latitude(); // get_latitude - double get_longitude(); // get_longitude - double get_height(); // get_height - double get_user_clock_offset_s(); // get_user_clock_offset_s; + std::vector trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs + std::vector get_position_ecef_m(); // get_position_ecef_m + std::vector get_velocity_ecef_m_s(); // get_velocity_ecef_m_s + std::vector get_accel_ecef_m_s2(); // get_accel_ecef_m_s2 + std::vector get_position_var_ecef_m(); // get_position_var_ecef_m + std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s + std::vector get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 + double get_latitude(); // get_latitude + double get_longitude(); // get_longitude + double get_height(); // get_height + double get_user_clock_offset_s(); // get_user_clock_offset_s; private: Vtl_Conf config; //TODO: Internal VTL persistent variables here - + // Transformation variables arma::colvec d; arma::colvec rho_pri; @@ -73,13 +73,13 @@ private: arma::colvec a_x; arma::colvec a_y; arma::colvec a_z; - + // Kalman filter matrices arma::mat kf_P_x_ini; // initial state error covariance matrix // arma::mat kf_P_x; // state error covariance matrix arma::mat kf_P_x_pre; // Predicted state error covariance matrix arma::mat kf_P_x; - arma::mat kf_S; // innovation covariance matrix + arma::mat kf_S; // innovation covariance matrix arma::mat kf_F; // state transition matrix arma::mat kf_H; // system matrix @@ -91,7 +91,7 @@ private: arma::mat kf_y; // measurement vector arma::mat kf_yerr; // ERROR measurement vector arma::mat kf_xerr; // ERROR state vector - arma::mat kf_K; // Kalman gain matrix + arma::mat kf_K; // Kalman gain matrix // Gaussian estimator arma::mat kf_R_est; // measurement error covariance @@ -101,12 +101,12 @@ private: int n_of_states; uint64_t refSampleCounter; - bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor - bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor - bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation + bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor + bool kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor + bool obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); - bool model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt, int counter); - double EmpujeLkTable(double t_disparo); + bool model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); + double EmpujeLkTable(double t_disparo); }; /** \} */ diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 0bcec91a6..cd61ed51f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -49,16 +49,16 @@ #include // for Mat_VarCreate #include // for mp #include +#include "iostream" #include // for fill_n #include #include // for fmod, round, floor #include // for exception -#include // for cout, cerr +#include +#include // for cout, cerr #include #include #include -#include "iostream" -#include #if HAS_GENERIC_LAMBDA #else @@ -637,7 +637,6 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) //std::cout<< "test cast CH "<sample_counter <<"\n"; if (cmd->channel_id == this->d_channel) { - arma::vec x_tmp; arma::mat F_tmp; @@ -645,7 +644,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) //To.Do: apply VTL corrections to the KF states double delta_t_s = static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in; // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s - x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; + x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; //ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0}, {0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)}, @@ -656,29 +655,35 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) double old_doppler = d_x_old_old(2); double old_doppler_rate = d_x_old_old(3); double old_code_phase_chips = d_x_old_old(0); - - if(cmd->enable_carrier_nco_cmd){ - if(cmd->enable_code_nco_cmd){ - if(abs(d_x_old_old(2) - tmp_x(2))>50){ - std::cout <<"channel: "<< this->d_channel - << " tracking_cmd TOO FAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; - }else{ - std::cout <<"channel: "<< this->d_channel - << " tracking_cmd NEAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; - } - d_x_old_old(2) = tmp_x(2); //replace DOPPLER - // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE - }else{ - // std::cout<<"yet to soon"<enable_carrier_nco_cmd) + { + if (cmd->enable_code_nco_cmd) + { + if (abs(d_x_old_old(2) - tmp_x(2)) > 50) + { + std::cout << "channel: " << this->d_channel + << " tracking_cmd TOO FAR: " + << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" + << " \n"; + } + else + { + std::cout << "channel: " << this->d_channel + << " tracking_cmd NEAR: " + << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" + << " \n"; + } + d_x_old_old(2) = tmp_x(2); //replace DOPPLER + // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE + } + else + { + // std::cout<<"yet to soon"<sample_counter // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" // << " [s]\n"; - // if(cmd->channel_id ==0) + // if(cmd->channel_id ==0) // { // std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " // << delta_t_s << "[s]" @@ -702,15 +707,15 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) dump_tracking_file.open("dump_trk_file.csv", std::ios::out | std::ios::app); dump_tracking_file.precision(15); if (!dump_tracking_file) - { - std::cout << "dump_tracking_file not created!"; - } + { + std::cout << "dump_tracking_file not created!"; + } else - { - dump_tracking_file << "doppler_corr" - << ","<< this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3)<< "," << old_doppler_rate << "\n"; - dump_tracking_file.close(); - } + { + dump_tracking_file << "doppler_corr" + << "," << this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3) << "," << old_doppler_rate << "\n"; + dump_tracking_file.close(); + } } } else @@ -1252,7 +1257,7 @@ void kf_tracking::run_Kf() // new code phase estimation d_code_error_kf_chips = d_x_new_new(0); - d_x_new_new(0)=0; // reset error estimation because the NCO corrects the code phase + d_x_new_new(0) = 0; // reset error estimation because the NCO corrects the code phase // new carrier phase estimation d_carrier_phase_kf_rad = d_x_new_new(1); From edad601ed3ee4a1ddc20b389357af9f99988a8ab Mon Sep 17 00:00:00 2001 From: miguekf Date: Sat, 11 Mar 2023 21:43:57 +0100 Subject: [PATCH 142/165] FIX: vtl_data debug_print() method --- src/algorithms/PVT/libs/vtl_data.cc | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index b7d5d7fd7..47e3228aa 100644 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -54,15 +54,15 @@ void Vtl_Data::init_storage(int n_sats) void Vtl_Data::debug_print() { std::cout << "vtl_data debug print at RX TOW: " << epoch_tow_s << ", TRK sample counter: " << sample_counter << "\n"; - // sat_p.print("VTL Sat Positions"); - //sat_v.print("VTL Sat Velocities"); - // sat_dts.print("VTL Sat clocks"); - // sat_var.print("VTL Sat clock variances"); - // sat_health_flag.print("VTL Sat health"); - //sat_LOS.print("VTL SAT LOS"); + sat_p.print("VTL Sat Positions"); + sat_v.print("VTL Sat Velocities"); + sat_dts.print("VTL Sat clocks"); + sat_var.print("VTL Sat clock variances"); + sat_health_flag.print("VTL Sat health"); + sat_LOS.print("VTL SAT LOS"); kf_state.print("EKF STATE"); - //pr_m.print("Satellite Code pseudoranges [m]"); - //doppler_hz.print("satellite Carrier Dopplers [Hz]"); - // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); + pr_m.print("Satellite Code pseudoranges [m]"); + doppler_hz.print("satellite Carrier Dopplers [Hz]"); + carrier_phase_rads.print("satellite accumulated carrier phases [rad]"); } From 2d4c5b4d7bc4830fe21f2b73ea76ad925c5321d7 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Tue, 14 Mar 2023 15:29:18 +0100 Subject: [PATCH 143/165] fix: vtl_engine bug --- src/algorithms/PVT/libs/vtl_engine.cc | 131 ++++++++++++-------------- src/algorithms/PVT/libs/vtl_engine.h | 11 ++- 2 files changed, 64 insertions(+), 78 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 46aeb28b2..66d20189d 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -25,8 +25,11 @@ Vtl_Engine::Vtl_Engine() counter = 0; refSampleCounter = 0; n_of_states = 11; + delta_t_cmd = 0; + kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; kf_x = arma::zeros(n_of_states, 1); + } Vtl_Engine::~Vtl_Engine() @@ -38,14 +41,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //TODO: Implement main VTL loop here using arma::as_scalar; - if (refSampleCounter = 0) + if (refSampleCounter == 0) { refSampleCounter = new_data.sample_counter; } double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; refSampleCounter = new_data.sample_counter; - static double delta_t_cmd = 0; + bool flag_cmd = false; + bool flag_time_cmd = false; delta_t_cmd = delta_t_cmd + delta_t_vtl; // update timer for vtl trk command if (delta_t_cmd >= 0.3) { @@ -63,7 +67,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_Q = arma::eye(n_of_states, n_of_states); kf_F = arma::eye(n_of_states, n_of_states); - bool test = kf_F_fill(kf_F, kf_dt); + kf_F_fill(kf_F, kf_dt); //kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); kf_y = arma::zeros(3 * new_data.sat_number, 1); @@ -74,6 +78,10 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) ; // ################## Kalman Tracking ###################################### counter++; //uint64_t + + if (counter>2500){ + flag_time_cmd = true; + } //new_data.kf_state.print("new_data kf initial"); uint32_t closure_point = 3; @@ -102,7 +110,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) double acc_x = 0; double acc_y = 0; double acc_z = 0; - test = model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter); + // model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter); kf_x(6) = acc_x; kf_x(7) = acc_y; kf_x(8) = acc_z; @@ -161,7 +169,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) a_y = arma::zeros(new_data.sat_number, 1); a_z = arma::zeros(new_data.sat_number, 1); // cout<<"llegado aqui tambien"<5){ - // // dump_vtl_file << "pr_m" - // // << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) - // // << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)< 6) { t_disparo = t_disparo + dt; - std::cout << "u : " << u << endl; + // std::cout << "u : " << u << endl; double diam_cohete = 120.0e-3; // 120 mm double mass_rocket = 50.0; //50Kg @@ -438,7 +425,7 @@ bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma // acc_vec.print("acc_vec"); // % return - cout << "modelo 3Dof actuando,t:" << t_disparo << endl; + // cout << "modelo 3Dof actuando,t:" << t_disparo << endl; acc_x = acc_vec(0); acc_y = acc_vec(1); acc_z = acc_vec(2); @@ -459,8 +446,6 @@ bool Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma acc_y = 0; acc_z = 0; } - - return -1; } double Vtl_Engine::EmpujeLkTable(double t_disparo) diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 5317114a1..a7b081f3f 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -100,12 +100,13 @@ private: uint32_t counter; int n_of_states; uint64_t refSampleCounter; + double delta_t_cmd = 0; - bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor - bool kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor - bool obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation - bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); - bool model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); + void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor + void kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor + void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation + void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); + void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); double EmpujeLkTable(double t_disparo); }; From 8445ed3cbb2492ed379ee1e8155144f9f214cab3 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Tue, 14 Mar 2023 15:29:46 +0100 Subject: [PATCH 144/165] fix: kf_tracking bug --- .../tracking/gnuradio_blocks/kf_tracking.cc | 43 ++++++++----------- 1 file changed, 19 insertions(+), 24 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index cd61ed51f..bda054d7a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -634,18 +634,18 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr).hash_code()) { const auto cmd = wht::any_cast>(pmt::any_ref(msg)); - //std::cout<< "test cast CH "<sample_counter <<"\n"; + // std::cout<< "test cast CH "<sample_counter <<"\n"; if (cmd->channel_id == this->d_channel) { arma::vec x_tmp; arma::mat F_tmp; gr::thread::scoped_lock lock(d_setlock); - //To.Do: apply VTL corrections to the KF states + // To.Do: apply VTL corrections to the KF states double delta_t_s = static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in; // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; - //ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces + // ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0}, {0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)}, {0.0, 0.0, 1.0, delta_t_s}, @@ -658,31 +658,26 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) if (cmd->enable_carrier_nco_cmd) { - if (cmd->enable_code_nco_cmd) + if (abs(d_x_old_old(2) - tmp_x(2)) > 50) { - if (abs(d_x_old_old(2) - tmp_x(2)) > 50) - { - std::cout << "channel: " << this->d_channel - << " tracking_cmd TOO FAR: " - << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" - << " \n"; - } - else - { - std::cout << "channel: " << this->d_channel - << " tracking_cmd NEAR: " - << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" - << " \n"; - } - d_x_old_old(2) = tmp_x(2); //replace DOPPLER - // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE + std::cout << "channel: " << this->d_channel + << " tracking_cmd TOO FAR: " + << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" + << " \n"; } else { - // std::cout<<"yet to soon"<d_channel + << " tracking_cmd NEAR: " + << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" + << " \n"; } + // d_x_old_old(2) = tmp_x(2); // replace DOPPLER + // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE + } + else + { + // std::cout << "correction not applied" << std::endl; } // set vtl corrections flag to inform VTL from gnss_synchro object @@ -1242,7 +1237,7 @@ void kf_tracking::run_Kf() // Kalman loop // Prediction - //d_x_old_old(0)=0; // reset error estimation because the NCO corrects the code phase + // d_x_old_old(0)=0; // reset error estimation because the NCO corrects the code phase d_x_new_old = d_F * d_x_old_old; d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q; From 8f4866151ddbfbdf030d6287a14ba0238e2ebbf9 Mon Sep 17 00:00:00 2001 From: miguekf Date: Tue, 14 Mar 2023 16:14:21 +0100 Subject: [PATCH 145/165] MOD: format in vtl_engine --- src/algorithms/PVT/libs/vtl_engine.cc | 104 +++++++++----------------- 1 file changed, 37 insertions(+), 67 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 66d20189d..3e9f2ab88 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -69,20 +69,29 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_F = arma::eye(n_of_states, n_of_states); kf_F_fill(kf_F, kf_dt); - //kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); kf_y = arma::zeros(3 * new_data.sat_number, 1); kf_yerr = arma::zeros(3 * new_data.sat_number, 1); kf_xerr = arma::zeros(n_of_states, 1); kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); - ; + d = arma::zeros(new_data.sat_number, 1); + rho_pri = arma::zeros(new_data.sat_number, 1); + rhoDot_pri = arma::zeros(new_data.sat_number, 1); + rhoDot2_pri = arma::zeros(new_data.sat_number, 1); + + rho_pri_filt = arma::zeros(new_data.sat_number, 1); + rhoDot_pri_filt = arma::zeros(new_data.sat_number, 1); + doppler_hz_filt = arma::zeros(new_data.sat_number, 1); + + a_x = arma::zeros(new_data.sat_number, 1); + a_y = arma::zeros(new_data.sat_number, 1); + a_z = arma::zeros(new_data.sat_number, 1); // ################## Kalman Tracking ###################################### counter++; //uint64_t if (counter>2500){ flag_time_cmd = true; } - //new_data.kf_state.print("new_data kf initial"); uint32_t closure_point = 3; if (counter < closure_point) @@ -120,7 +129,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } // State error Covariance Matrix Q (PVT) - //careful, values for V and T could not be adecuate. kf_Q(0, 0) = 100.0; kf_Q(1, 1) = 100.0; kf_Q(2, 2) = 100.0; @@ -136,39 +144,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) { - // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 80.0; //*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; //*50.0/new_data.sat_CN0_dB_hz(i); - kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; //*50.0/new_data.sat_CN0_dB_hz(i); - - // if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ - // kf_R(i, i) = 10e4; - // kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; - // kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 10e4; - // cout<<"channel: "< Date: Tue, 14 Mar 2023 16:57:56 +0100 Subject: [PATCH 146/165] MOD: vtl_engine format --- src/algorithms/PVT/libs/vtl_engine.cc | 64 ++++++++++++--------------- 1 file changed, 29 insertions(+), 35 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 3e9f2ab88..beb14138d 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -67,8 +67,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_Q = arma::eye(n_of_states, n_of_states); kf_F = arma::eye(n_of_states, n_of_states); - kf_F_fill(kf_F, kf_dt); - + kf_y = arma::zeros(3 * new_data.sat_number, 1); kf_yerr = arma::zeros(3 * new_data.sat_number, 1); kf_xerr = arma::zeros(n_of_states, 1); @@ -94,9 +93,35 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } uint32_t closure_point = 3; + // State error Covariance Matrix Q (PVT) + kf_Q(0, 0) = 100.0; + kf_Q(1, 1) = 100.0; + kf_Q(2, 2) = 100.0; + kf_Q(3, 3) = 8.0; + kf_Q(4, 4) = 8.0; + kf_Q(5, 5) = 8.0; + kf_Q(6, 6) = .10; + kf_Q(7, 7) = .10; + kf_Q(8, 8) = .10; + kf_Q(9, 9) = 4.0; + kf_Q(10, 10) = 10.0; + + // Measurement error Covariance Matrix R assembling + for (int32_t i = 0; i < new_data.sat_number; i++) + { + kf_R(i, i) = 80.0; + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; + kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; + } + + //************************************** + // Kalman state prediction (time update) + //************************************** + kf_F_fill(kf_F, kf_dt); + if (counter < closure_point) { - // // receiver solution from rtklib_solver + // receiver solution from rtklib_solver kf_dx = kf_x; kf_x(0) = new_data.rx_p(0); kf_x(1) = new_data.rx_p(1); @@ -128,46 +153,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; } - // State error Covariance Matrix Q (PVT) - kf_Q(0, 0) = 100.0; - kf_Q(1, 1) = 100.0; - kf_Q(2, 2) = 100.0; - kf_Q(3, 3) = 8.0; - kf_Q(4, 4) = 8.0; - kf_Q(5, 5) = 8.0; - kf_Q(6, 6) = .10; - kf_Q(7, 7) = .10; - kf_Q(8, 8) = .10; - kf_Q(9, 9) = 4.0; - kf_Q(10, 10) = 10.0; - - // Measurement error Covariance Matrix R assembling - for (int32_t i = 0; i < new_data.sat_number; i++) - { - kf_R(i, i) = 80.0; - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; - kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; - } - - //************************************** - // Kalman state prediction (time update) - //************************************** - obsv_calc(rho_pri, rhoDot_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); - for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities - { - new_data.sat_LOS(i, 0) = a_x(i); - new_data.sat_LOS(i, 1) = a_y(i); - new_data.sat_LOS(i, 2) = a_z(i); - } kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction + //************************************** // Kalman estimation (measurement update) //************************************** kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt); - kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri * 0, new_data.pr_m, new_data.doppler_hz, kf_x); //************************************** From 956142cbe2f44fc26679fb624ef5a3cb84387085 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Tue, 14 Mar 2023 17:29:51 +0100 Subject: [PATCH 147/165] fix: vtl_engine bug --- src/algorithms/PVT/libs/vtl_engine.cc | 106 +++++++++++++------------- 1 file changed, 53 insertions(+), 53 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index beb14138d..8d0824d31 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -27,9 +27,8 @@ Vtl_Engine::Vtl_Engine() n_of_states = 11; delta_t_cmd = 0; - kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; + kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; // TODO: use a real value.; kf_x = arma::zeros(n_of_states, 1); - } Vtl_Engine::~Vtl_Engine() @@ -38,7 +37,7 @@ Vtl_Engine::~Vtl_Engine() bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { - //TODO: Implement main VTL loop here + // TODO: Implement main VTL loop here using arma::as_scalar; if (refSampleCounter == 0) @@ -57,17 +56,17 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) delta_t_cmd = 0; // reset timer for vtl trk command } // ################## Kalman filter initialization ###################################### - //State variables + // State variables arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); - double kf_dt = delta_t_vtl; //0.05; + double kf_dt = delta_t_vtl; // 0.05; kf_Q = arma::eye(n_of_states, n_of_states); kf_F = arma::eye(n_of_states, n_of_states); - + kf_y = arma::zeros(3 * new_data.sat_number, 1); kf_yerr = arma::zeros(3 * new_data.sat_number, 1); kf_xerr = arma::zeros(n_of_states, 1); @@ -86,11 +85,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) a_y = arma::zeros(new_data.sat_number, 1); a_z = arma::zeros(new_data.sat_number, 1); // ################## Kalman Tracking ###################################### - counter++; //uint64_t + counter++; // uint64_t - if (counter>2500){ - flag_time_cmd = true; - } + if (counter > 2500) + { + flag_time_cmd = true; + } uint32_t closure_point = 3; // State error Covariance Matrix Q (PVT) @@ -109,9 +109,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) { - kf_R(i, i) = 80.0; - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; - kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; + kf_R(i, i) = 80.0; + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; + kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; } //************************************** @@ -141,6 +141,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) else { // receiver solution from previous KF step + kf_x = kf_x; + // acceleration model double acc_x = 0; double acc_y = 0; double acc_z = 0; @@ -163,7 +165,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt); kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri * 0, new_data.pr_m, new_data.doppler_hz, kf_x); - //************************************** // Kalman filter update step //************************************** @@ -175,13 +176,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_xerr = kf_K * (kf_yerr); // Error state estimation arma::mat A = (arma::eye(size(kf_P_x)) - kf_K * kf_H); kf_P_x = A * kf_P_x * A.t() + kf_K * kf_R * kf_K.t(); // update state estimation error covariance matrix - kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) + kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) kf_dx = kf_x; //************************* // Geometric Transformation //************************* - + obsv_calc(rho_pri, rhoDot_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt); @@ -200,24 +201,24 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_rate_hz_s = -(a_x(channel) * kf_x(6) + a_y(channel) * kf_x(7) + a_z(channel) * kf_x(8)) / Lambda_GPS_L1; - trk_cmd.code_phase_chips = 0; //kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; + trk_cmd.code_phase_chips = 0; // kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; if (flag_time_cmd) - { - if (flag_cmd) - { - trk_cmd.enable_carrier_nco_cmd = true; - } - else - { - trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue - } - } + { + if (flag_cmd) + { + trk_cmd.enable_carrier_nco_cmd = true; + } + else + { + trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue + } + } else - { - trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue - } - + { + trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue + } + trk_cmd.sample_counter = new_data.sample_counter; trk_cmd.channel_id = channel; @@ -233,13 +234,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) } else { - dump_vtl_file << "kf_state" - << "," << kf_x(0) << "," << kf_x(1) << "," << kf_x(2) << "," << kf_x(3) << "," << kf_x(4) << "," << kf_x(5) << "," << kf_x(6) << "," << kf_x(7)<< "," << kf_x(8) <<"," << kf_x(9) <<"," << kf_x(10)<< endl; + << "," << kf_x(0) << "," << kf_x(1) << "," << kf_x(2) << "," << kf_x(3) << "," << kf_x(4) << "," << kf_x(5) << "," << kf_x(6) << "," << kf_x(7) << "," << kf_x(8) << "," << kf_x(9) << "," << kf_x(10) << endl; dump_vtl_file << "kf_xerr" - << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) << "," << kf_xerr(6) << "," << kf_xerr(7)<< "," << kf_xerr(8) <<"," << kf_xerr(9) <<"," << kf_xerr(10)<< endl; + << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) << "," << kf_xerr(6) << "," << kf_xerr(7) << "," << kf_xerr(8) << "," << kf_xerr(9) << "," << kf_xerr(10) << endl; dump_vtl_file << "rtklib_state" - << "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << "," << delta_t_vtl << endl; + << "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << "," << delta_t_vtl << endl; // dump_vtl_file << "filt_dopp_sat" // << "," << doppler_hz_filt(0) << "," << doppler_hz_filt(1) << "," << doppler_hz_filt(2) << "," << doppler_hz_filt(3) << "," << doppler_hz_filt(4) < 1500) @@ -364,7 +364,7 @@ void Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma t_disparo = t_disparo + dt; // std::cout << "u : " << u << endl; double diam_cohete = 120.0e-3; // 120 mm - double mass_rocket = 50.0; //50Kg + double mass_rocket = 50.0; // 50Kg if (t_disparo < .2) { @@ -592,8 +592,8 @@ double Vtl_Engine::EmpujeLkTable(double t_disparo) {1.74000000000000, 537.866310625605}, }; - //encuentra el mas cercano justo anterior. - // int index_E = LkTable.elem(find(LkTable<=t_disparo)).max(); + // encuentra el mas cercano justo anterior. + // int index_E = LkTable.elem(find(LkTable<=t_disparo)).max(); arma::uvec index_E = find(LkTable <= t_disparo, 1, "last"); // index_E.print("indice E: "); // uint kk = index_E(0); From c2f89e97217935846320752492432b0b48f944d1 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Tue, 14 Mar 2023 17:48:33 +0100 Subject: [PATCH 148/165] MOD: vtl_engine rocket model working --- src/algorithms/PVT/libs/vtl_engine.cc | 9 ++++++++- .../tracking/gnuradio_blocks/kf_tracking.cc | 18 +++++++++--------- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 8d0824d31..5ba20aab0 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -112,6 +112,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_R(i, i) = 80.0; kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; + + if (i == 6) + { + kf_R(i, i) = 10e5; + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e5; + kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 10e6; + } } //************************************** @@ -146,7 +153,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) double acc_x = 0; double acc_y = 0; double acc_z = 0; - // model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter); + model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter); kf_x(6) = acc_x; kf_x(7) = acc_y; kf_x(8) = acc_z; diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index bda054d7a..1c67a3f5f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -660,19 +660,19 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) { if (abs(d_x_old_old(2) - tmp_x(2)) > 50) { - std::cout << "channel: " << this->d_channel - << " tracking_cmd TOO FAR: " - << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" - << " \n"; + // std::cout << "channel: " << this->d_channel + // << " tracking_cmd TOO FAR: " + // << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" + // << " \n"; } else { - std::cout << "channel: " << this->d_channel - << " tracking_cmd NEAR: " - << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" - << " \n"; + // std::cout << "channel: " << this->d_channel + // << " tracking_cmd NEAR: " + // << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" + // << " \n"; } - // d_x_old_old(2) = tmp_x(2); // replace DOPPLER + d_x_old_old(2) = tmp_x(2); // replace DOPPLER // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE } else From 7a271afd45479c6888ae7363ecb17633b313fa57 Mon Sep 17 00:00:00 2001 From: miguekf Date: Wed, 15 Mar 2023 00:51:33 +0100 Subject: [PATCH 149/165] ADD: accelerometer to vtl_engine --- src/algorithms/PVT/libs/vtl_engine.cc | 2572 +++++++++++++++++++++++++ src/algorithms/PVT/libs/vtl_engine.h | 2 + 2 files changed, 2574 insertions(+) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 5ba20aab0..149d220d9 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -419,6 +419,2578 @@ void Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma } } +void Vtl_Engine::accelerometer(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter) +{ + arma::colvec u_vec; + arma::colvec acc_vec; + arma::colvec u_dir; + static double t_disparo = 0; + double accelerometer; + + u_vec = kf_x.rows(3, 5); + + double u = norm(u_vec, 2); + + if (counter > 1500) + { + if (u > 6) + { + t_disparo = t_disparo + dt; + + if (t_disparo < .2) + { + u_dir = {.90828, -.13984, -.388756}; + } + else + { + u_dir = u_vec / u; + } + + accelerometer = AccLkTable(t_disparo); + acc_vec = accelerometer * u_dir; + + acc_x = acc_vec(0); + acc_y = acc_vec(1); + acc_z = acc_vec(2); + } + else + { + t_disparo = 0; + // % return + acc_x = 0; + acc_y = 0; + acc_z = 0; + } + } + else + { + // % return + acc_x = 0; + acc_y = 0; + acc_z = 0; + } +} + +double Vtl_Engine::AccLkTable(double t_disparo) +{ + double Acc; + arma::mat LkTable={ +{0,191.3693381}, +{0.01,391.0831124}, +{0.02,385.6263172}, +{0.03,379.2536529}, +{0.04,372.8504183}, +{0.05,366.4351054}, +{0.06,359.9487249}, +{0.07,353.4523708}, +{0.08,346.9151605}, +{0.09,340.3533742}, +{0.1,339.9823669}, +{0.11,339.649644}, +{0.12,339.3131193}, +{0.13,338.9712498}, +{0.14,338.6260923}, +{0.15,338.2779261}, +{0.16,337.9238608}, +{0.17,337.5646868}, +{0.18,337.2045652}, +{0.19,336.8363276}, +{0.2,337.6125746}, +{0.21,338.3898123}, +{0.22,339.1721974}, +{0.23,339.9508641}, +{0.24,340.7342869}, +{0.25,341.5135997}, +{0.26,342.2964879}, +{0.27,343.0777302}, +{0.28,343.8624016}, +{0.29,344.6450452}, +{0.3,345.4302226}, +{0.31,346.2163638}, +{0.32,347.003086}, +{0.33,347.7904035}, +{0.34,348.5766329}, +{0.35,349.3650418}, +{0.36,350.1543455}, +{0.37,350.9437807}, +{0.38,351.735714}, +{0.39,352.5236761}, +{0.4,353.317052}, +{0.41,354.1104116}, +{0.42,354.9032791}, +{0.43,355.6925592}, +{0.44,356.490275}, +{0.45,357.2834519}, +{0.46,358.0785073}, +{0.47,358.8713231}, +{0.48,359.668187}, +{0.49,360.4658938}, +{0.5,361.2644865}, +{0.51,362.0577138}, +{0.52,362.8553987}, +{0.53,363.6519585}, +{0.54,364.4531391}, +{0.55,365.2484945}, +{0.56,366.0474406}, +{0.57,366.8445417}, +{0.58,367.645723}, +{0.59,368.4312816}, +{0.6,369.2180327}, +{0.61,370.0057638}, +{0.62,370.7934491}, +{0.63,371.577332}, +{0.64,372.3618727}, +{0.65,373.1392343}, +{0.66,373.9223953}, +{0.67,374.6986382}, +{0.68,375.4776747}, +{0.69,376.2563757}, +{0.7,377.5615194}, +{0.71,378.8593047}, +{0.72,380.1677186}, +{0.73,381.4775822}, +{0.74,382.7850068}, +{0.75,384.0925559}, +{0.76,385.4033472}, +{0.77,386.680222}, +{0.78,387.9603486}, +{0.79,389.2414657}, +{0.8,390.5176097}, +{0.81,391.7783411}, +{0.82,393.0383665}, +{0.83,394.29121}, +{0.84,395.5451831}, +{0.85,396.779476}, +{0.86,398.0051316}, +{0.87,399.217608}, +{0.88,400.433585}, +{0.89,401.6396038}, +{0.9,402.8249609}, +{0.91,403.9998189}, +{0.92,405.1644136}, +{0.93,406.3320948}, +{0.94,407.4898264}, +{0.95,408.6386952}, +{0.96,409.7882084}, +{0.97,410.9206965}, +{0.98,412.0711172}, +{0.99,413.2273621}, +{1,414.3762955}, +{1.01,415.5172083}, +{1.02,416.6851116}, +{1.03,417.8740344}, +{1.04,419.0501524}, +{1.05,420.2265404}, +{1.06,421.4316749}, +{1.07,422.6518329}, +{1.08,423.879432}, +{1.09,425.0968591}, +{1.1,426.3705142}, +{1.11,427.6814206}, +{1.12,428.9906538}, +{1.13,430.3080079}, +{1.14,431.6231364}, +{1.15,432.9423547}, +{1.16,434.2603966}, +{1.17,435.5806367}, +{1.18,436.9038196}, +{1.19,438.2359771}, +{1.2,439.760283}, +{1.21,441.2855662}, +{1.22,442.8237912}, +{1.23,444.3543307}, +{1.24,445.8935896}, +{1.25,447.4374141}, +{1.26,449.016306}, +{1.27,450.6057334}, +{1.28,452.1975701}, +{1.29,453.7990419}, +{1.3,455.3951522}, +{1.31,457.0042091}, +{1.32,458.6230155}, +{1.33,460.2387287}, +{1.34,461.8608819}, +{1.35,463.4956492}, +{1.36,465.1298266}, +{1.37,466.7670335}, +{1.38,468.4087662}, +{1.39,470.0591738}, +{1.4,471.7211266}, +{1.41,473.4149126}, +{1.42,475.0975536}, +{1.43,476.8052568}, +{1.44,478.5125067}, +{1.45,480.2302357}, +{1.46,481.9505168}, +{1.47,483.6755965}, +{1.48,485.4085203}, +{1.49,487.1606942}, +{1.5,488.912737}, +{1.51,490.6763352}, +{1.52,492.4461934}, +{1.53,494.230761}, +{1.54,496.0142138}, +{1.55,497.8000122}, +{1.56,499.5868444}, +{1.57,501.3897915}, +{1.58,503.1978733}, +{1.59,505.0179768}, +{1.6,506.8420775}, +{1.61,508.6810082}, +{1.62,510.5317451}, +{1.63,512.3834648}, +{1.64,514.2492508}, +{1.65,516.1190989}, +{1.66,518.0053088}, +{1.67,519.9107613}, +{1.68,521.8122756}, +{1.69,523.7277473}, +{1.7,526.52496}, +{1.71,529.326074}, +{1.72,532.1521584}, +{1.73,534.9959392}, +{1.74,537.8663106}, +{1.75,532.5465503}, +{1.76,527.1860241}, +{1.77,521.7618548}, +{1.78,516.3328104}, +{1.79,510.856735}, +{1.8,505.3589191}, +{1.81,499.7424161}, +{1.82,494.1151004}, +{1.83,488.4906155}, +{1.84,482.7875458}, +{1.85,477.0777855}, +{1.86,471.3509003}, +{1.87,465.5580034}, +{1.88,459.7619075}, +{1.89,453.944495}, +{1.9,448.0865852}, +{1.91,442.2123527}, +{1.92,436.3035816}, +{1.93,430.3563548}, +{1.94,424.4002234}, +{1.95,406.4045572}, +{1.96,388.2433759}, +{1.97,370.1079284}, +{1.98,351.8103035}, +{1.99,333.4616772}, +{2,320.4688275}, +{2.01,307.452342}, +{2.02,294.3528443}, +{2.03,281.2674437}, +{2.04,268.111713}, +{2.05,254.9637706}, +{2.06,241.7924305}, +{2.07,228.5260562}, +{2.08,215.276914}, +{2.09,201.9631912}, +{2.1,188.6795429}, +{2.11,175.4304874}, +{2.12,162.00753}, +{2.13,148.7345206}, +{2.14,135.3574292}, +{2.15,126.6856641}, +{2.16,118.020273}, +{2.17,109.3739993}, +{2.18,100.6846937}, +{2.19,92.00283749}, +{2.2,83.35951249}, +{2.21,74.73486872}, +{2.22,66.0716783}, +{2.23,57.44510561}, +{2.24,48.79168522}, +{2.25,40.27062222}, +{2.26,31.7147632}, +{2.27,23.35713095}, +{2.28,15.32234163}, +{2.29,8.698553788}, +{2.3,7.708774611}, +{2.31,7.248557512}, +{2.32,7.391062373}, +{2.33,8.108155956}, +{2.34,9.264564858}, +{2.35,10.7236636}, +{2.36,12.36361185}, +{2.37,14.13412431}, +{2.38,15.9852933}, +{2.39,17.90058977}, +{2.4,105.5371052}, +{2.41,105.310299}, +{2.42,105.0834932}, +{2.43,104.8576945}, +{2.44,104.6317221}, +{2.45,104.4076761}, +{2.46,104.1836307}, +{2.47,103.9608891}, +{2.48,103.7368451}, +{2.49,103.5154945}, +{2.5,103.2936742}, +{2.51,103.0736278}, +{2.52,102.8522788}, +{2.53,102.6331529}, +{2.54,102.413108}, +{2.55,102.1952862}, +{2.56,101.9774649}, +{2.57,101.7597303}, +{2.58,101.5428295}, +{2.59,101.3269349}, +{2.6,101.1113383}, +{2.61,100.8953588}, +{2.62,100.6826079}, +{2.63,100.4688523}, +{2.64,100.2551829}, +{2.65,100.0437366}, +{2.66,99.83090244}, +{2.67,99.61945733}, +{2.68,99.4088471}, +{2.69,99.19870611}, +{2.7,98.98910215}, +{2.71,98.77988202}, +{2.72,98.57288514}, +{2.73,98.36467091}, +{2.74,98.15767517}, +{2.75,97.95168467}, +{2.76,97.74469009}, +{2.77,97.53870065}, +{2.78,97.33484982}, +{2.79,97.13016437}, +{2.8,96.92731915}, +{2.81,96.7234702}, +{2.82,96.52062617}, +{2.83,96.31816569}, +{2.84,96.11754558}, +{2.85,95.91692608}, +{2.86,95.71630718}, +{2.87,95.51699175}, +{2.88,95.31729406}, +{2.89,95.11889985}, +{2.9,94.92059003}, +{2.91,94.72349986}, +{2.92,94.52602756}, +{2.93,94.32994227}, +{2.94,94.13385751}, +{2.95,93.93768991}, +{2.96,93.7438292}, +{2.97,93.54904893}, +{2.98,93.35610946}, +{2.99,93.16225043}, +{3,92.96922916}, +{3.01,92.7776772}, +{3.02,92.58566036}, +{3.03,92.39494689}, +{3.04,92.20423405}, +{3.05,92.0148246}, +{3.06,91.8250333}, +{3.07,91.63662788}, +{3.08,91.44814059}, +{3.09,91.25973628}, +{3.1,91.0721706}, +{3.11,90.8859905}, +{3.12,90.69972888}, +{3.13,90.51354996}, +{3.14,90.32829199}, +{3.15,90.14433732}, +{3.16,89.96000096}, +{3.17,89.77512703}, +{3.18,89.59301505}, +{3.19,89.40998325}, +{3.2,89.22825475}, +{3.21,89.04614472}, +{3.22,88.86533802}, +{3.23,88.68461319}, +{3.24,88.5047283}, +{3.25,88.32530723}, +{3.26,88.14542365}, +{3.27,87.96684341}, +{3.28,87.78834468}, +{3.29,87.61168778}, +{3.3,87.43411084}, +{3.31,87.25775651}, +{3.32,87.08148342}, +{3.33,86.90613162}, +{3.34,86.73078044}, +{3.35,86.55542988}, +{3.36,86.38230327}, +{3.37,86.20695398}, +{3.38,86.03474946}, +{3.39,85.86170475}, +{3.4,85.68988324}, +{3.41,85.51768068}, +{3.42,85.34686104}, +{3.43,85.17596234}, +{3.44,85.00598522}, +{3.45,84.83516737}, +{3.46,84.6661919}, +{3.47,84.4965984}, +{3.48,84.32854516}, +{3.49,84.16087409}, +{3.5,83.99358512}, +{3.51,83.82583632}, +{3.52,83.65908817}, +{3.53,83.49272211}, +{3.54,83.32727771}, +{3.55,83.16183397}, +{3.56,82.99731197}, +{3.57,82.8328691}, +{3.58,82.66872971}, +{3.59,82.50551211}, +{3.6,82.34199214}, +{3.61,82.18007827}, +{3.62,82.01870507}, +{3.63,81.85648938}, +{3.64,81.69549878}, +{3.65,81.53458669}, +{3.66,81.37421531}, +{3.67,81.21452915}, +{3.68,81.05546147}, +{3.69,80.89547312}, +{3.7,80.73770906}, +{3.71,80.5790243}, +{3.72,80.4212615}, +{3.73,80.26442074}, +{3.74,80.10758065}, +{3.75,79.95112215}, +{3.76,79.79558572}, +{3.77,79.63966914}, +{3.78,79.48505557}, +{3.79,79.32959782}, +{3.8,79.17536637}, +{3.81,79.02167638}, +{3.82,78.8684442}, +{3.83,78.71605789}, +{3.84,78.56282692}, +{3.85,78.41143969}, +{3.86,78.25905551}, +{3.87,78.10805022}, +{3.88,77.95666494}, +{3.89,77.80650681}, +{3.9,77.6564251}, +{3.91,77.50726573}, +{3.92,77.35718532}, +{3.93,77.20840774}, +{3.94,77.06017211}, +{3.95,76.9123176}, +{3.96,76.76446376}, +{3.97,76.62527605}, +{3.98,76.48731601}, +{3.99,76.34943145}, +{4,76.21154735}, +{4.01,76.0745109}, +{4.02,75.93754972}, +{4.03,75.80096926}, +{4.04,75.66493099}, +{4.05,75.5283515}, +{4.06,75.39323622}, +{4.07,75.25757969}, +{4.08,75.12284564}, +{4.09,74.98719015}, +{4.1,74.85375922}, +{4.11,74.7199488}, +{4.12,74.58613897}, +{4.13,74.4527097}, +{4.14,74.32020305}, +{4.15,74.18677489}, +{4.16,74.05434288}, +{4.17,73.92275998}, +{4.18,73.79025558}, +{4.19,73.65874716}, +{4.2,73.52754586}, +{4.21,73.39603843}, +{4.22,73.26668262}, +{4.23,73.13647818}, +{4.24,73.00527914}, +{4.25,72.87692017}, +{4.26,72.74664453}, +{4.27,72.61828668}, +{4.28,72.48938671}, +{4.29,72.36095745}, +{4.3,72.23298084}, +{4.31,72.10500473}, +{4.32,71.97664957}, +{4.33,71.85089875}, +{4.34,71.72384656}, +{4.35,71.59672298}, +{4.36,71.47059439}, +{4.37,71.34484579}, +{4.38,71.21909773}, +{4.39,71.0938933}, +{4.4,70.96821788}, +{4.41,70.8433939}, +{4.42,70.71949301}, +{4.43,70.59467021}, +{4.44,70.4711498}, +{4.45,70.34824439}, +{4.46,70.22472508}, +{4.47,70.10174977}, +{4.48,69.97915428}, +{4.49,69.85663016}, +{4.5,69.73403583}, +{4.51,69.61243537}, +{4.52,69.4907649}, +{4.53,69.37046724}, +{4.54,69.248798}, +{4.55,69.12757867}, +{4.56,69.00683342}, +{4.57,68.88746064}, +{4.58,68.7680884}, +{4.59,68.64772398}, +{4.6,68.52927568}, +{4.61,68.40990511}, +{4.62,68.29183674}, +{4.63,68.17246724}, +{4.64,68.05433046}, +{4.65,67.93718666}, +{4.66,67.81966471}, +{4.67,67.70252207}, +{4.68,67.58537999}, +{4.69,67.4682385}, +{4.7,67.35202056}, +{4.71,67.23618183}, +{4.72,67.12003386}, +{4.73,67.00511924}, +{4.74,66.88928217}, +{4.75,66.77399025}, +{4.76,66.65907746}, +{4.77,66.54508839}, +{4.78,66.43024508}, +{4.79,66.31718035}, +{4.8,66.20357152}, +{4.81,66.08965293}, +{4.82,65.97696844}, +{4.83,65.86397411}, +{4.84,65.75129087}, +{4.85,65.63959913}, +{4.86,65.52691713}, +{4.87,65.4152265}, +{4.88,65.30346905}, +{4.89,65.19177955}, +{4.9,65.08101391}, +{4.91,64.97055983}, +{4.92,64.85979538}, +{4.93,64.74940953}, +{4.94,64.63994759}, +{4.95,64.5304195}, +{4.96,64.42058083}, +{4.97,64.31112069}, +{4.98,64.20296247}, +{4.99,64.09350346}, +{5,63.98496852}, +{5.01,63.8777355}, +{5.02,63.76920181}, +{5.03,63.66197001}, +{5.04,63.55473882}, +{5.05,63.44713053}, +{5.06,63.33990058}, +{5.07,63.23359487}, +{5.08,63.1272898}, +{5.09,63.0214284}, +{5.1,62.91512453}, +{5.11,62.809745}, +{5.12,62.70480883}, +{5.13,62.59980807}, +{5.14,62.49480796}, +{5.15,62.39041969}, +{5.16,62.28542084}, +{5.17,62.18134645}, +{5.18,62.07733733}, +{5.19,61.9745654}, +{5.2,61.8705574}, +{5.21,61.76740949}, +{5.22,61.66470377}, +{5.23,61.56162134}, +{5.24,61.45977657}, +{5.25,61.35799646}, +{5.26,61.25615307}, +{5.27,61.15437415}, +{5.28,61.05259579}, +{5.29,60.951742}, +{5.3,60.84990134}, +{5.31,60.74942582}, +{5.32,60.64949795}, +{5.33,60.54902358}, +{5.34,60.44909694}, +{5.35,60.34862373}, +{5.36,60.24999934}, +{5.37,60.15007461}, +{5.38,60.05182832}, +{5.39,59.95320583}, +{5.4,59.85365978}, +{5.41,59.75596279}, +{5.42,59.65734225}, +{5.43,59.55909902}, +{5.44,59.46140404}, +{5.45,59.36408636}, +{5.46,59.26775553}, +{5.47,59.17043906}, +{5.48,59.07404759}, +{5.49,58.97635595}, +{5.5,58.88040385}, +{5.51,58.78493873}, +{5.52,58.68854994}, +{5.53,58.5931475}, +{5.54,58.49806084}, +{5.55,58.40265956}, +{5.56,58.30757421}, +{5.57,58.21347488}, +{5.58,58.1180146}, +{5.59,58.02391646}, +{5.6,57.92975833}, +{5.61,57.83566137}, +{5.62,57.74242924}, +{5.63,57.64925811}, +{5.64,57.55646364}, +{5.65,57.46323365}, +{5.66,57.37006437}, +{5.67,57.27727167}, +{5.68,57.18540423}, +{5.69,57.09310183}, +{5.7,57.00123568}, +{5.71,56.90937012}, +{5.72,56.81842995}, +{5.73,56.7265656}, +{5.74,56.63562668}, +{5.75,56.54468839}, +{5.76,56.45505132}, +{5.77,56.3641143}, +{5.78,56.27447849}, +{5.79,56.18446768}, +{5.8,56.09483315}, +{5.81,56.00482369}, +{5.82,55.91519047}, +{5.83,55.82648287}, +{5.84,55.73685095}, +{5.85,55.6481447}, +{5.86,55.56036419}, +{5.87,55.47171714}, +{5.88,55.38338824}, +{5.89,55.29560974}, +{5.9,55.20783199}, +{5.91,55.12048763}, +{5.92,55.03271123}, +{5.93,54.94623595}, +{5.94,54.85889329}, +{5.95,54.77241935}, +{5.96,54.6856279}, +{5.97,54.59953044}, +{5.98,54.51274022}, +{5.99,54.42756936}, +{6,54.34208069}, +{6.01,54.2565362}, +{6.02,54.17142378}, +{6.03,54.08588077}, +{6.04,54.00169487}, +{6.05,53.91615335}, +{6.06,53.8319687}, +{6.07,53.74778462}, +{6.08,53.66317074}, +{6.09,53.57991343}, +{6.1,53.49573124}, +{6.11,53.41247515}, +{6.12,53.32823895}, +{6.13,53.24628434}, +{6.14,53.16303021}, +{6.15,53.07977671}, +{6.16,52.99782396}, +{6.17,52.91457174}, +{6.18,52.83354588}, +{6.19,52.75252068}, +{6.2,52.67149614}, +{6.21,52.59177232}, +{6.22,52.51172899}, +{6.23,52.43200639}, +{6.24,52.35191018}, +{6.25,52.27311468}, +{6.26,52.19344784}, +{6.27,52.11427943}, +{6.28,52.03461365}, +{6.29,51.95582072}, +{6.3,51.8770285}, +{6.31,51.7992161}, +{6.32,51.72042521}, +{6.33,51.64298797}, +{6.34,51.56512432}, +{6.35,51.4863883}, +{6.36,51.40889994}, +{6.37,51.33109103}, +{6.38,51.25453}, +{6.39,51.1767223}, +{6.4,51.10021497}, +{6.41,51.02365598}, +{6.42,50.94677613}, +{6.43,50.87027056}, +{6.44,50.79371366}, +{6.45,50.71813543}, +{6.46,50.64163163}, +{6.47,50.566003}, +{6.48,50.49042663}, +{6.49,50.41522435}, +{6.5,50.33964914}, +{6.51,50.26407455}, +{6.52,50.18974918}, +{6.53,50.11510217}, +{6.54,50.04082915}, +{6.55,49.96525709}, +{6.56,49.8919116}, +{6.57,49.81764038}, +{6.58,49.74336976}, +{6.59,49.67002613}, +{6.6,49.59575673}, +{6.61,49.52241437}, +{6.62,49.44907265}, +{6.63,49.37665805}, +{6.64,49.30331765}, +{6.65,49.2299779}, +{6.66,49.15793829}, +{6.67,49.08552639}, +{6.68,49.01218863}, +{6.69,48.94015097}, +{6.7,48.86779023}, +{6.71,48.7966804}, +{6.72,48.72464462}, +{6.73,48.65353614}, +{6.74,48.58205566}, +{6.75,48.51007046}, +{6.76,48.43896396}, +{6.77,48.3687849}, +{6.78,48.29767983}, +{6.79,48.22662358}, +{6.8,48.1568191}, +{6.81,48.08669074}, +{6.82,48.01558828}, +{6.83,47.94541339}, +{6.84,47.8756592}, +{6.85,47.80641256}, +{6.86,47.73665949}, +{6.87,47.66778656}, +{6.88,47.59858928}, +{6.89,47.5287908}, +{6.9,47.45992002}, +{6.91,47.39072463}, +{6.92,47.32282897}, +{6.93,47.25396027}, +{6.94,47.18513879}, +{6.95,47.11719857}, +{6.96,47.04967737}, +{6.97,46.98178475}, +{6.98,46.91384668}, +{6.99,46.84595533}, +{7,46.77839063}, +{7.01,46.7114277}, +{7.02,46.64353826}, +{7.03,46.57694835}, +{7.04,46.51031367}, +{7.05,46.44335335}, +{7.06,46.37676531}, +{7.07,46.31017787}, +{7.08,46.24414673}, +{7.09,46.17751571}, +{7.1,46.11185753}, +{7.11,46.04619998}, +{7.12,45.98054307}, +{7.13,45.91395938}, +{7.14,45.84960253}, +{7.15,45.78394749}, +{7.16,45.7182931}, +{7.17,45.65356684}, +{7.18,45.58921251}, +{7.19,45.52356007}, +{7.2,45.45883582}, +{7.21,45.39448341}, +{7.22,45.33013163}, +{7.23,45.26633701}, +{7.24,45.20198655}, +{7.25,45.13856437}, +{7.26,45.07477189}, +{7.27,45.01042343}, +{7.28,44.9470033}, +{7.29,44.88358387}, +{7.3,44.82016513}, +{7.31,44.75771711}, +{7.32,44.69429966}, +{7.33,44.63218143}, +{7.34,44.56969317}, +{7.35,44.50627783}, +{7.36,44.44383274}, +{7.37,44.38171718}, +{7.38,44.31923174}, +{7.39,44.25808684}, +{7.4,44.19560278}, +{7.41,44.13441787}, +{7.42,44.07197633}, +{7.43,44.01079279}, +{7.44,43.94960999}, +{7.45,43.88809827}, +{7.46,43.82691687}, +{7.47,43.76577664}, +{7.48,43.70552469}, +{7.49,43.64434543}, +{7.5,43.58413512}, +{7.51,43.52295728}, +{7.52,43.46274819}, +{7.53,43.40286997}, +{7.54,43.3426621}, +{7.55,43.28241531}, +{7.56,43.22350686}, +{7.57,43.16330081}, +{7.58,43.10398447}, +{7.59,43.04414954}, +{7.6,42.98483474}, +{7.61,42.92592934}, +{7.62,42.86702451}, +{7.63,42.80771199}, +{7.64,42.74880843}, +{7.65,42.68990543}, +{7.66,42.63059523}, +{7.67,42.57169353}, +{7.68,42.51145649}, +{7.69,42.45122031}, +{7.7,42.39102268}, +{7.71,42.332086}, +{7.72,42.27281824}, +{7.73,42.21295458}, +{7.74,42.1536882}, +{7.75,42.09475466}, +{7.76,42.03678752}, +{7.77,41.97748641}, +{7.78,41.91859202}, +{7.79,41.86059036}, +{7.8,41.80169732}, +{7.81,41.74369731}, +{7.82,41.68573437}, +{7.83,41.62684333}, +{7.84,41.56884576}, +{7.85,41.51181369}, +{7.86,41.45381785}, +{7.87,41.39585845}, +{7.88,41.33919733}, +{7.89,41.28216814}, +{7.9,41.2241756}, +{7.91,41.16751658}, +{7.92,41.11141854}, +{7.93,41.05379713}, +{7.94,40.99770067}, +{7.95,40.94104445}, +{7.96,40.88494948}, +{7.97,40.82829464}, +{7.98,40.77256952}, +{7.99,40.7168111}, +{8,40.65979014}, +{8.01,40.6049964}, +{8.02,40.5492742}, +{8.03,40.49355288}, +{8.04,40.4378322}, +{8.05,40.38304144}, +{8.06,40.32769028}, +{8.07,40.27290099}, +{8.08,40.21811247}, +{8.09,40.16332474}, +{8.1,40.10890567}, +{8.11,40.05411945}, +{8.12,39.99933402}, +{8.13,39.94491715}, +{8.14,39.89106263}, +{8.15,39.83664725}, +{8.16,39.78279435}, +{8.17,39.7293099}, +{8.18,39.67582622}, +{8.19,39.62197577}, +{8.2,39.56852494}, +{8.21,39.51504348}, +{8.22,39.46156279}, +{8.23,39.40901245}, +{8.24,39.35553335}, +{8.25,39.30301525}, +{8.26,39.24953762}, +{8.27,39.19699041}, +{8.28,39.14444404}, +{8.29,39.0919286}, +{8.3,39.04068058}, +{8.31,38.98813658}, +{8.32,38.93562314}, +{8.33,38.88344769}, +{8.34,38.83183584}, +{8.35,38.78062111}, +{8.36,38.7290109}, +{8.37,38.67683857}, +{8.38,38.62525902}, +{8.39,38.57494795}, +{8.4,38.5237365}, +{8.41,38.47213045}, +{8.42,38.42089193}, +{8.43,38.37061247}, +{8.44,38.31937553}, +{8.45,38.26909742}, +{8.46,38.21879208}, +{8.47,38.16721889}, +{8.48,38.11821167}, +{8.49,38.06700627}, +{8.5,38.01766165}, +{8.51,37.9673604}, +{8.52,37.91708712}, +{8.53,37.86771773}, +{8.54,37.81744589}, +{8.55,37.76844436}, +{8.56,37.71910415}, +{8.57,37.67013066}, +{8.58,37.61983531}, +{8.59,37.57179349}, +{8.6,37.52245614}, +{8.61,37.47345951}, +{8.62,37.42412368}, +{8.63,37.37608468}, +{8.64,37.32711599}, +{8.65,37.27868733}, +{8.66,37.2297201}, +{8.67,37.18168396}, +{8.68,37.1336485}, +{8.69,37.08558897}, +{8.7,37.03755502}, +{8.71,36.98915633}, +{8.72,36.94241975}, +{8.73,36.89438792}, +{8.74,36.84635679}, +{8.75,36.79923301}, +{8.76,36.75120345}, +{8.77,36.70410523}, +{8.78,36.65700777}, +{8.79,36.61027618}, +{8.8,36.5631802}, +{8.81,36.51608498}, +{8.82,36.46935551}, +{8.83,36.42226178}, +{8.84,36.37646449}, +{8.85,36.32937229}, +{8.86,36.28357649}, +{8.87,36.23648583}, +{8.88,36.19069152}, +{8.89,36.14489797}, +{8.9,36.0987405}, +{8.91,36.05294848}, +{8.92,36.00715721}, +{8.93,35.96136669}, +{8.94,35.91557693}, +{8.95,35.86978793}, +{8.96,35.82493069}, +{8.97,35.77916419}, +{8.98,35.7343084}, +{8.99,35.68852237}, +{9,35.64366819}, +{9.01,35.5988148}, +{9.02,35.55396228}, +{9.03,35.50913068}, +{9.04,35.46427965}, +{9.05,35.41979346}, +{9.06,35.37587524}, +{9.07,35.33102668}, +{9.08,35.28749351}, +{9.09,35.24264645}, +{9.1,35.19909533}, +{9.11,35.15424994}, +{9.12,35.11071932}, +{9.13,35.0668068}, +{9.14,35.02325881}, +{9.15,34.9793665}, +{9.16,34.93582004}, +{9.17,34.8922744}, +{9.18,34.84838415}, +{9.19,34.8057715}, +{9.2,34.76222825}, +{9.21,34.71963493}, +{9.22,34.67609322}, +{9.23,34.63350121}, +{9.24,34.59089259}, +{9.25,34.54830191}, +{9.26,34.50476332}, +{9.27,34.46215719}, +{9.28,34.4205002}, +{9.29,34.37789572}, +{9.3,34.33530849}, +{9.31,34.29270565}, +{9.32,34.25141436}, +{9.33,34.20881316}, +{9.34,34.1671604}, +{9.35,34.12549266}, +{9.36,34.08327228}, +{9.37,34.0416216}, +{9.38,34.00031895}, +{9.39,33.95866972}, +{9.4,33.91736872}, +{9.41,33.87572094}, +{9.42,33.8344216}, +{9.43,33.79277528}, +{9.44,33.75242393}, +{9.45,33.71112702}, +{9.46,33.67041482}, +{9.47,33.62913357}, +{9.48,33.58877113}, +{9.49,33.54749128}, +{9.5,33.506782}, +{9.51,33.4664221}, +{9.52,33.42607642}, +{9.53,33.38349213}, +{9.54,33.34185389}, +{9.55,33.29963327}, +{9.56,33.2579839}, +{9.57,33.21541585}, +{9.58,33.17376836}, +{9.59,33.13248356}, +{9.6,33.09085011}, +{9.61,33.04920553}, +{9.62,33.0079235}, +{9.63,32.96629251}, +{9.64,32.92501229}, +{9.65,32.88337165}, +{9.66,32.84303684}, +{9.67,32.80139814}, +{9.68,32.76106491}, +{9.69,32.71978933}, +{9.7,32.67909651}, +{9.71,32.63875528}, +{9.72,32.59841507}, +{9.73,32.55808586}, +{9.74,32.51738663}, +{9.75,32.47705906}, +{9.76,32.43673222}, +{9.77,32.39639674}, +{9.78,32.35700414}, +{9.79,32.31667062}, +{9.8,32.27634704}, +{9.81,32.23694815}, +{9.82,32.19755893}, +{9.83,32.15722936}, +{9.84,32.11820228}, +{9.85,32.07881552}, +{9.86,32.03942161}, +{9.87,32.00003659}, +{9.88,31.96101262}, +{9.89,31.9216217}, +{9.9,31.88317208}, +{9.91,31.84415051}, +{9.92,31.80476263}, +{9.93,31.76667564}, +{9.94,31.72729651}, +{9.95,31.68920457}, +{9.96,31.65076021}, +{9.97,31.61267652}, +{9.98,31.57459362}, +{9.99,31.53521874}, +{10,31.49713169}, +{10.01,31.4599844}, +{10.02,31.42190484}, +{10.03,31.38346657}, +{10.04,31.34538864}, +{10.05,31.30823975}, +{10.06,31.27016359}, +{10.07,31.23302145}, +{10.08,31.19494693}, +{10.09,31.15780647}, +{10.1,31.12066686}, +{10.11,31.08352811}, +{10.12,31.04639022}, +{10.13,31.00924954}, +{10.14,30.97211354}, +{10.15,30.9353373}, +{10.16,30.89820299}, +{10.17,30.86106955}, +{10.18,30.82487043}, +{10.19,30.78809747}, +{10.2,30.75096665}, +{10.21,30.71477021}, +{10.22,30.67893324}, +{10.23,30.64180509}, +{10.24,30.60596983}, +{10.25,30.569777}, +{10.26,30.53394347}, +{10.27,30.49775249}, +{10.28,30.4619207}, +{10.29,30.42608978}, +{10.3,30.38990248}, +{10.31,30.35407315}, +{10.32,30.31917839}, +{10.33,30.2829928}, +{10.34,30.24809987}, +{10.35,30.2122741}, +{10.36,30.17738297}, +{10.37,30.14155897}, +{10.38,30.10631156}, +{10.39,30.07142306}, +{10.4,30.03653549}, +{10.41,30.00164884}, +{10.42,29.96676312}, +{10.43,29.93187719}, +{10.44,29.89699318}, +{10.45,29.86211011}, +{10.46,29.82758532}, +{10.47,29.79363808}, +{10.48,29.75875585}, +{10.49,29.72387638}, +{10.5,29.68993191}, +{10.51,29.65634308}, +{10.52,29.6214663}, +{10.53,29.5875246}, +{10.54,29.55358391}, +{10.55,29.519998}, +{10.56,29.48512494}, +{10.57,29.45154383}, +{10.58,29.41760332}, +{10.59,29.38402392}, +{10.6,29.35102307}, +{10.61,29.31744152}, +{10.62,29.28350828}, +{10.63,29.25086245}, +{10.64,29.21693108}, +{10.65,29.18335712}, +{10.66,29.15071358}, +{10.67,29.11678509}, +{10.68,29.08414305}, +{10.69,29.05021647}, +{10.7,29.01758148}, +{10.71,28.98400733}, +{10.72,28.95137415}, +{10.73,28.91837999}, +{10.74,28.88481418}, +{10.75,28.85218387}, +{10.76,28.81954804}, +{10.77,28.78691958}, +{10.78,28.75428528}, +{10.79,28.72130305}, +{10.8,28.68960496}, +{10.81,28.65698032}, +{10.82,28.62434918}, +{10.83,28.5926611}, +{10.84,28.56003155}, +{10.85,28.52741071}, +{10.86,28.49607273}, +{10.87,28.46438852}, +{10.88,28.43176212}, +{10.89,28.40007989}, +{10.9,28.36838991}, +{10.91,28.33670971}, +{10.92,28.30537634}, +{10.93,28.27368876}, +{10.94,28.24201145}, +{10.95,28.21068035}, +{10.96,28.17900504}, +{10.97,28.14732081}, +{10.98,28.115992}, +{10.99,28.0852546}, +{11,28.05392737}, +{11.01,28.02224636}, +{11.02,27.99186643}, +{11.03,27.96018712}, +{11.04,27.92979799}, +{11.05,27.89906669}, +{11.06,27.86774413}, +{11.07,27.8373574}, +{11.08,27.80662916}, +{11.09,27.77624411}, +{11.1,27.74585982}, +{11.11,27.71512233}, +{11.12,27.6847521}, +{11.13,27.65437027}, +{11.14,27.62398919}, +{11.15,27.59325515}, +{11.16,27.56382412}, +{11.17,27.53344555}, +{11.18,27.50306776}, +{11.19,27.47362606}, +{11.2,27.44324983}, +{11.21,27.41382376}, +{11.22,27.38438464}, +{11.23,27.35401093}, +{11.24,27.32457343}, +{11.25,27.29513675}, +{11.26,27.26570089}, +{11.27,27.23626586}, +{11.28,27.20719987}, +{11.29,27.17776661}, +{11.3,27.14833418}, +{11.31,27.1189026}, +{11.32,27.08947185}, +{11.33,27.06039469}, +{11.34,27.03096558}, +{11.35,27.00247292}, +{11.36,26.97304553}, +{11.37,26.94397151}, +{11.38,26.91548144}, +{11.39,26.88642605}, +{11.4,26.85793786}, +{11.41,26.82945057}, +{11.42,26.80038071}, +{11.43,26.77189515}, +{11.44,26.74376266}, +{11.45,26.71527887}, +{11.46,26.68714803}, +{11.47,26.65901801}, +{11.48,26.63053685}, +{11.49,26.6024085}, +{11.5,26.57391023}, +{11.51,26.54578339}, +{11.52,26.51765738}, +{11.53,26.4895322}, +{11.54,26.46199216}, +{11.55,26.43386869}, +{11.56,26.40574606}, +{11.57,26.37856025}, +{11.58,26.35008791}, +{11.59,26.32290389}, +{11.6,26.29572074}, +{11.61,26.2676024}, +{11.62,26.24040005}, +{11.63,26.21321936}, +{11.64,26.18510344}, +{11.65,26.1579245}, +{11.66,26.13074645}, +{11.67,26.1035693}, +{11.68,26.07639304}, +{11.69,26.04919555}, +{11.7,26.02202092}, +{11.71,25.99578343}, +{11.72,25.96861063}, +{11.73,25.94143874}, +{11.74,25.91459531}, +{11.75,25.8883613}, +{11.76,25.86119195}, +{11.77,25.83402352}, +{11.78,25.80779235}, +{11.79,25.78095219}, +{11.8,25.75472269}, +{11.81,25.72849417}, +{11.82,25.70168028}, +{11.83,25.67542909}, +{11.84,25.64920322}, +{11.85,25.62332824}, +{11.86,25.59614278}, +{11.87,25.5702694}, +{11.88,25.54404713}, +{11.89,25.51780034}, +{11.9,25.49192951}, +{11.91,25.46570998}, +{11.92,25.4407515}, +{11.93,25.41488322}, +{11.94,25.38866641}, +{11.95,25.36277341}, +{11.96,25.33655837}, +{11.97,25.30721209}, +{11.98,25.27786728}, +{11.99,25.24849653}, +{12,25.21915441}, +{12.01,25.1898137}, +{12.02,25.16047458}, +{12.03,25.13204538}, +{12.04,25.10270896}, +{12.05,25.07337403}, +{12.06,25.04494849}, +{12.07,25.01561634}, +{12.08,24.9862857}, +{12.09,24.95821224}, +{12.1,24.92982124}, +{12.11,24.90049487}, +{12.12,24.87207673}, +{12.13,24.84403826}, +{12.14,24.81562252}, +{12.15,24.78723881}, +{12.16,24.75917343}, +{12.17,24.73079259}, +{12.18,24.70241334}, +{12.19,24.67435154}, +{12.2,24.64597519}, +{12.21,24.61885284}, +{12.22,24.59047942}, +{12.23,24.56335946}, +{12.24,24.53533627}, +{12.25,24.50787145}, +{12.26,24.47981777}, +{12.27,24.4527359}, +{12.28,24.42527482}, +{12.29,24.39725858}, +{12.3,24.37014694}, +{12.31,24.34307082}, +{12.32,24.3156149}, +{12.33,24.28850687}, +{12.34,24.26143502}, +{12.35,24.23432943}, +{12.36,24.20726048}, +{12.37,24.18109478}, +{12.38,24.15399282}, +{12.39,24.12692815}, +{12.4,24.10076615}, +{12.41,24.07366783}, +{12.42,24.04754501}, +{12.43,24.02044923}, +{12.44,23.99429215}, +{12.45,23.96817377}, +{12.46,23.94108172}, +{12.47,23.91492845}, +{12.48,23.88912194}, +{12.49,23.86300939}, +{12.5,23.8368599}, +{12.51,23.81071164}, +{12.52,23.78490986}, +{12.53,23.75876398}, +{12.54,23.73265881}, +{12.55,23.7077984}, +{12.56,23.6816564}, +{12.57,23.65551566}, +{12.58,23.62972097}, +{12.59,23.60452048}, +{12.6,23.57876888}, +{12.61,23.55357111}, +{12.62,23.52871915}, +{12.63,23.50258603}, +{12.64,23.47773645}, +{12.65,23.45288805}, +{12.66,23.42675865}, +{12.67,23.40191266}, +{12.68,23.37706784}, +{12.69,23.3518802}, +{12.7,23.32708081}, +{12.71,23.29967609}, +{12.72,23.27266003}, +{12.73,23.24564574}, +{12.74,23.21863323}, +{12.75,23.19251634}, +{12.76,23.16516372}, +{12.77,23.13909454}, +{12.78,23.112044}, +{12.79,23.08597821}, +{12.8,23.05897606}, +{12.81,23.03286817}, +{12.82,23.00680764}, +{12.83,22.97976445}, +{12.84,22.95370737}, +{12.85,22.92799483}, +{12.86,22.90189438}, +{12.87,22.87584259}, +{12.88,22.84974514}, +{12.89,22.82369687}, +{12.9,22.79854085}, +{12.91,22.77283829}, +{12.92,22.74674687}, +{12.93,22.72164409}, +{12.94,22.69589765}, +{12.95,22.67079842}, +{12.96,22.64471311}, +{12.97,22.6199592}, +{12.98,22.59387694}, +{12.99,22.56907614}, +{13,22.54398583}, +{13.01,22.519188}, +{13.02,22.49410134}, +{13.03,22.46836788}, +{13.04,22.44417194}, +{13.05,22.41943166}, +{13.06,22.39464117}, +{13.07,22.36951124}, +{13.08,22.3447761}, +{13.09,22.31999001}, +{13.1,22.29580348}, +{13.11,22.27107355}, +{13.12,22.24629192}, +{13.13,22.22245045}, +{13.14,22.19772569}, +{13.15,22.1738873}, +{13.16,22.14971031}, +{13.17,22.12493602}, +{13.18,22.1011571}, +{13.19,22.07732477}, +{13.2,22.0534939}, +{13.21,22.02966448}, +{13.22,22.00589262}, +{13.23,21.98206637}, +{13.24,21.9582416}, +{13.25,21.9344183}, +{13.26,21.91093565}, +{13.27,21.88711523}, +{13.28,21.86429292}, +{13.29,21.84047575}, +{13.3,21.81666008}, +{13.31,21.79412373}, +{13.32,21.770311}, +{13.33,21.74743888}, +{13.34,21.72362921}, +{13.35,21.70109859}, +{13.36,21.67823104}, +{13.37,21.6544259}, +{13.38,21.63195974}, +{13.39,21.60909704}, +{13.4,21.58657391}, +{13.41,21.56371429}, +{13.42,21.54119407}, +{13.43,21.51833755}, +{13.44,21.49582027}, +{13.45,21.47296687}, +{13.46,21.45045254}, +{13.47,21.42760227}, +{13.48,21.40509091}, +{13.49,21.38352034}, +{13.5,21.36067478}, +{13.51,21.33810451}, +{13.52,21.31532552}, +{13.53,21.29060603}, +{13.54,21.26689173}, +{13.55,21.24311512}, +{13.56,21.22034434}, +{13.57,21.19657128}, +{13.58,21.17286521}, +{13.59,21.1490957}, +{13.6,21.12539377}, +{13.61,21.10256736}, +{13.62,21.07886961}, +{13.63,21.05510731}, +{13.64,21.03228625}, +{13.65,21.00859467}, +{13.66,20.98611288}, +{13.67,20.96329714}, +{13.68,20.94055131}, +{13.69,20.91679968}, +{13.7,20.89432461}, +{13.71,20.87158501}, +{13.72,20.84877843}, +{13.73,20.82630851}, +{13.74,20.80357522}, +{13.75,20.78110883}, +{13.76,20.7583094}, +{13.77,20.73551176}, +{13.78,20.71312132}, +{13.79,20.69160153}, +{13.8,20.66880938}, +{13.81,20.64635316}, +{13.82,20.62457646}, +{13.83,20.60212387}, +{13.84,20.58027891}, +{13.85,20.5578297}, +{13.86,20.53632199}, +{13.87,20.51455595}, +{13.88,20.49305197}, +{13.89,20.47060979}, +{13.9,20.44910917}, +{13.91,20.42727711}, +{13.92,20.40577999}, +{13.93,20.38428458}, +{13.94,20.36279089}, +{13.95,20.34137471}, +{13.96,20.31988474}, +{13.97,20.29839651}, +{13.98,20.27785001}, +{13.99,20.25636527}, +{14,20.23488228}, +{14.01,20.2134010}, +{14.02,20.19286161}, +{14.03,20.17138391}, +{14.04,20.15084805}, +{14.05,20.12937391}, +{14.06,20.10884164}, +{14.07,20.08831118}, +{14.08,20.06684243}, +{14.09,20.0463156}, +{14.1,20.02612189}, +{14.11,20.00559862}, +{14.12,19.98413703}, +{14.13,19.96361743}, +{14.14,19.94334949}, +{14.15,19.92283321}, +{14.16,19.90231878}, +{14.17,19.88213686}, +{14.18,19.86256627}, +{14.19,19.84205738}, +{14.2,19.82188069}, +{14.21,19.80137546}, +{14.22,19.78205917}, +{14.23,19.76155735}, +{14.24,19.74138739}, +{14.25,19.72182956}, +{14.26,19.70166313}, +{14.27,19.68210906}, +{14.28,19.66280148}, +{14.29,19.64231061}, +{14.3,19.62309135}, +{14.31,19.60387385}, +{14.32,19.5833025}, +{14.33,19.56408835}, +{14.34,19.54487598}, +{14.35,19.52533651}, +{14.36,19.50604058}, +{14.37,19.48683338}, +{14.38,19.4672994}, +{14.39,19.4480959}, +{14.4,19.42880586}, +{14.41,19.40960569}, +{14.42,19.39040734}, +{14.43,19.36994231}, +{14.44,19.35041974}, +{14.45,19.33028662}, +{14.46,19.31076803}, +{14.47,19.29157915}, +{14.48,19.2711241}, +{14.49,19.25184804}, +{14.5,19.23266453}, +{14.51,19.21221515}, +{14.52,19.19303548}, +{14.53,19.1738577}, +{14.54,19.15435486}, +{14.55,19.13518095}, +{14.56,19.11600895}, +{14.57,19.09651222}, +{14.58,19.07725048}, +{14.59,19.05808401}, +{14.6,19.03986}, +{14.61,19.020371}, +{14.62,19.00121059}, +{14.63,18.98195683}, +{14.64,18.9637404}, +{14.65,18.94458535}, +{14.66,18.92543225}, +{14.67,18.90722172}, +{14.68,18.88797596}, +{14.69,18.86976908}, +{14.7,18.85156418}, +{14.71,18.83242066}, +{14.72,18.81412198}, +{14.73,18.79592278}, +{14.74,18.77678495}, +{14.75,18.75849112}, +{14.76,18.74029765}, +{14.77,18.72210621}, +{14.78,18.70391681}, +{14.79,18.68562961}, +{14.8,18.66744399}, +{14.81,18.64958453}, +{14.82,18.63130218}, +{14.83,18.61312231}, +{14.84,18.59484321}, +{14.85,18.57760787}, +{14.86,18.55975749}, +{14.87,18.54148333}, +{14.88,18.52331312}, +{14.89,18.506409}, +{14.9,18.48813982}, +{14.91,18.47091622}, +{14.92,18.45297334}, +{14.93,18.43575355}, +{14.94,18.41749095}, +{14.95,18.40059776}, +{14.96,18.38233843}, +{14.97,18.36544889}, +{14.98,18.34823896}, +{14.99,18.33030698}, +{15,18.31310094}, +{15.01,18.2961128}, +{15.02,18.27891069}, +{15.03,18.26192569}, +{15.04,18.24504922}, +{15.05,18.22774566}, +{15.06,18.21076522}, +{15.07,18.19357277}, +{15.08,18.1765955}, +{15.09,18.15972822}, +{15.1,18.14243297}, +{15.11,18.12556948}, +{15.12,18.10953931}, +{15.13,18.0925698}, +{15.14,18.07539106}, +{15.15,18.05936558}, +{15.16,18.04251136}, +{15.17,18.02554826}, +{15.18,18.0095274}, +{15.19,17.99235852}, +{15.2,17.97634095}, +{15.21,17.95938407}, +{15.22,17.94348177}, +{15.23,17.9274689}, +{15.24,17.91062964}, +{15.25,17.89462}, +{15.26,17.8786118}, +{15.27,17.86166442}, +{15.28,17.84577334}, +{15.29,17.82976996}, +{15.3,17.8137681}, +{15.31,17.79788268}, +{15.32,17.78188412}, +{15.33,17.76588709}, +{15.34,17.75000738}, +{15.35,17.73401367}, +{15.36,17.7180215}, +{15.37,17.70203087}, +{15.38,17.68615874}, +{15.39,17.67048955}, +{15.4,17.65544451}, +{15.41,17.63946025}, +{15.42,17.62347755}, +{15.43,17.6076148}, +{15.44,17.59289385}, +{15.45,17.57691603}, +{15.46,17.56093978}, +{15.47,17.5459059}, +{15.48,17.53036979}, +{15.49,17.51533925}, +{15.5,17.49936953}, +{15.51,17.48465902}, +{15.52,17.46963317}, +{15.53,17.45366815}, +{15.54,17.43896204}, +{15.55,17.42312196}, +{15.56,17.40810274}, +{15.57,17.39340133}, +{15.58,17.37838525}, +{15.59,17.36368679}, +{15.6,17.34773311}, +{15.61,17.33303762}, +{15.62,17.31802786}, +{15.63,17.30333534}, +{15.64,17.28832878}, +{15.65,17.27363925}, +{15.66,17.25876085}, +{15.67,17.24407461}, +{15.68,17.22907476}, +{15.69,17.21533227}, +{15.7,17.20065051}, +{15.71,17.18565547}, +{15.72,17.17097676}, +{15.73,17.15629954}, +{15.74,17.14225006}, +{15.75,17.12757589}, +{15.76,17.11290323}, +{15.77,17.09885862}, +{15.78,17.08418904}, +{15.79,17.06952097}, +{15.8,17.05548126}, +{15.81,17.0408163}, +{15.82,17.02709353}, +{15.83,17.01337227}, +{15.84,16.99858182}, +{15.85,16.98455001}, +{15.86,16.96989246}, +{15.87,16.9561771}, +{15.88,16.94246328}, +{15.89,16.92781034}, +{15.9,16.91409959}, +{15.91,16.90039038}, +{15.92,16.88637013}, +{15.93,16.8726641}, +{15.94,16.85895964}, +{15.95,16.8443161}, +{15.96,16.83048134}, +{15.97,16.81678126}, +{15.98,16.80308275}, +{15.99,16.78938581}, +{16,16.77569045}, +{16.01,16.7619966}, +{16.02,16.74924503}, +{16.03,16.73541903}, +{16.04,16.7217297}, +{16.05,16.70804196}, +{16.06,16.69435581}, +{16.07,16.68098232}, +{16.08,16.66810304}, +{16.09,16.6544213}, +{16.1,16.64074118}, +{16.11,16.62800319}, +{16.12,16.61432629}, +{16.13,16.60051285}, +{16.14,16.58808973}, +{16.15,16.5744173}, +{16.16,16.56168699}, +{16.17,16.54787856}, +{16.18,16.53515122}, +{16.19,16.52179485}, +{16.2,16.50907066}, +{16.21,16.49620775}, +{16.22,16.48285561}, +{16.23,16.47013594}, +{16.24,16.45741793}, +{16.25,16.44361967}, +{16.26,16.43121377}, +{16.27,16.41850033}, +{16.28,16.40564625}, +{16.29,16.39230423}, +{16.3,16.37959539}, +{16.31,16.3671968}, +{16.32,16.35434777}, +{16.33,16.34164355}, +{16.34,16.32924928}, +{16.35,16.31640408}, +{16.36,16.30401257}, +{16.37,16.29131459}, +{16.38,16.2787811}, +{16.39,16.2660861}, +{16.4,16.25355487}, +{16.41,16.24086288}, +{16.42,16.22848006}, +{16.43,16.21658521}, +{16.44,16.20420519}, +{16.45,16.19151954}, +{16.46,16.1789954}, +{16.47,16.16725301}, +{16.48,16.15473116}, +{16.49,16.14235835}, +{16.5,16.12968041}, +{16.51,16.11810231}, +{16.52,16.10573389}, +{16.53,16.09385165}, +{16.54,16.08148609}, +{16.55,16.0696064}, +{16.56,16.05724372}, +{16.57,16.04582273}, +{16.58,16.03331263}, +{16.59,16.02158869}, +{16.6,16.00908095}, +{16.61,15.99766564}, +{16.62,15.98610027}, +{16.63,15.97453592}, +{16.64,15.96391265}, +{16.65,15.95250262}, +{16.66,15.94094157}, +{16.67,15.92938153}, +{16.68,15.91797559}, +{16.69,15.90641786}, +{16.7,15.89580114}, +{16.71,15.8843993}, +{16.72,15.87284489}, +{16.73,15.86129153}, +{16.74,15.85067917}, +{16.75,15.83928272}, +{16.76,15.82803683}, +{16.77,15.8174277}, +{16.78,15.8058797}, +{16.79,15.79542848}, +{16.8,15.78388282}, +{16.81,15.7732781}, +{16.82,15.7620381}, +{16.83,15.75159221}, +{16.84,15.74005092}, +{16.85,15.72945053}, +{16.86,15.71791138}, +{16.87,15.70777403}, +{16.88,15.69623716}, +{16.89,15.68564114}, +{16.9,15.67504619}, +{16.91,15.66381524}, +{16.92,15.65338142}, +{16.93,15.64278981}, +{16.94,15.63219927}, +{16.95,15.62097241}, +{16.96,15.61038392}, +{16.97,15.59995686}, +{16.98,15.5896728}, +{16.99,15.57908768}, +{17,15.567564}, +{17.01,15.55728}, +{17.02,15.5467008}, +{17.03,15.53628186}, +{17.04,15.5260038}, +{17.05,15.51542526}, +{17.06,15.50484782}, +{17.07,15.49457266}, +{17.08,15.48399732}, +{17.09,15.47372408}, +{17.1,15.46331442}, +{17.11,15.45304336}, +{17.12,15.44247247}, +{17.13,15.43220334}, +{17.14,15.42163458}, +{17.15,15.41200639}, +{17.16,15.40174019}, +{17.17,15.39117468}, +{17.18,15.381076}, +{17.19,15.37051287}, +{17.2,15.36119022}, +{17.21,15.35062924}, +{17.22,15.34036916}, +{17.23,15.33011005}, +{17.24,15.32049157}, +{17.25,15.31023442}, +{17.26,15.29967879}, +{17.27,15.29036291}, +{17.28,15.27980946}, +{17.29,15.27049554}, +{17.3,15.26024331}, +{17.31,15.24969308}, +{17.32,15.24055107}, +{17.33,15.23000328}, +{17.34,15.2206945}, +{17.35,15.2104475}, +{17.36,15.20084211}, +{17.37,15.19059712}, +{17.38,15.18129224}, +{17.39,15.17169008}, +{17.4,15.16144808}, +{17.41,15.15214615}, +{17.42,15.14160817}, +{17.43,15.13230825}, +{17.44,15.12300929}, +{17.45,15.11277228}, +{17.46,15.10317768}, +{17.47,15.09388171}, +{17.48,15.0845867}, +{17.49,15.07435372}, +{17.5,15.06476346}, +{17.51,15.05547146}, +{17.52,15.04618045}, +{17.53,15.0368904}, +{17.54,15.02636563}, +{17.55,15.01707765}, +{17.56,15.00779064}, +{17.57,14.99850463}, +{17.58,14.98921959}, +{17.59,14.97976001}, +{17.6,14.97018043}, +{17.61,14.9608982}, +{17.62,14.95161697}, +{17.63,14.94233673}, +{17.64,14.93305749}, +{17.65,14.92377925}, +{17.66,14.914502}, +{17.67,14.90522576}, +{17.68,14.89565497}, +{17.69,14.88638082}, +{17.7,14.87710768}, +{17.71,14.86783555}, +{17.72,14.85950302}, +{17.73,14.85005401}, +{17.74,14.84078465}, +{17.75,14.83151631}, +{17.76,14.82224899}, +{17.77,14.81392119}, +{17.78,14.80465589}, +{17.79,14.79539161}, +{17.8,14.78612836}, +{17.81,14.77780457}, +{17.82,14.76836233}, +{17.83,14.7591019}, +{17.84,14.75078089}, +{17.85,14.74152251}, +{17.86,14.73226517}, +{17.87,14.72394719}, +{17.88,14.71469191}, +{17.89,14.70619329}, +{17.9,14.69693982}, +{17.91,14.68768739}, +{17.92,14.67937427}, +{17.93,14.67041721}, +{17.94,14.66192218}, +{17.95,14.65361177}, +{17.96,14.64559541}, +{17.97,14.63710245}, +{17.98,14.62879476}, +{17.99,14.62078086}, +{18,14.61228996}, +{18.01,14.603985}, +{18.02,14.59578802}, +{18.03,14.58748477}, +{18.04,14.57918256}, +{18.05,14.57098741}, +{18.06,14.56268691}, +{18.07,14.55449291}, +{18.08,14.54619414}, +{18.09,14.53789641}, +{18.1,14.52970426}, +{18.11,14.52234619}, +{18.12,14.51415519}, +{18.13,14.50586092}, +{18.14,14.49785922}, +{18.15,14.48937856}, +{18.16,14.48108708}, +{18.17,14.47383695}, +{18.18,14.46554721}, +{18.19,14.45736041}, +{18.2,14.44907243}, +{18.21,14.44107645}, +{18.22,14.43353834}, +{18.23,14.42554394}, +{18.24,14.41706943}, +{18.25,14.41001433}, +{18.26,14.40154117}, +{18.27,14.39354998}, +{18.28,14.38601581}, +{18.29,14.37802622}, +{18.3,14.36974732}, +{18.31,14.36250539}, +{18.32,14.35451831}, +{18.33,14.34698749}, +{18.34,14.33900202}, +{18.35,14.33053502}, +{18.36,14.32348868}, +{18.37,14.31502305}, +{18.38,14.3079783}, +{18.39,14.29980353}, +{18.4,14.29247089}, +{18.41,14.28429732}, +{18.42,14.27696645}, +{18.43,14.2687941}, +{18.44,14.26175406}, +{18.45,14.25329386}, +{18.46,14.24625544}, +{18.47,14.23902275}, +{18.48,14.23075982}, +{18.49,14.22352832}, +{18.5,14.21555581}, +{18.51,14.20803692}, +{18.52,14.20100324}, +{18.53,14.19283689}, +{18.54,14.18551649}, +{18.55,14.17828851}, +{18.56,14.17032089}, +{18.57,14.16280601}, +{18.58,14.15577712}, +{18.59,14.14855148}, +{18.6,14.14029932}, +{18.61,14.1330749}, +{18.62,14.12585095}, +{18.63,14.11882591}, +{18.64,14.11131564}, +{18.65,14.10335527}, +{18.66,14.09613369}, +{18.67,14.08911186}, +{18.68,14.08160428}, +{18.69,14.07458408}, +{18.7,14.06736486}, +{18.71,14.05940941}, +{18.72,14.05190453}, +{18.73,14.04468704}, +{18.74,14.03767075}, +{18.75,14.03045441}, +{18.76,14.0234397}, +{18.77,14.01593811}, +{18.78,14.00892505}, +{18.79,14.00171111}, +{18.8,13.99449767}, +{18.81,13.98748687}, +{18.82,13.97998859}, +{18.83,13.97297946}, +{18.84,13.96576844}, +{18.85,13.95855791}, +{18.86,13.95155106}, +{18.87,13.94434172}, +{18.88,13.93733647}, +{18.89,13.92984286}, +{18.9,13.92263527}, +{18.91,13.91656883}, +{18.92,13.90936242}, +{18.93,13.90236113}, +{18.94,13.89515592}, +{18.95,13.88795122}, +{18.96,13.88095224}, +{18.97,13.87374874}, +{18.98,13.8671973}, +{18.99,13.86020068}, +{19,13.85299895}, +{19.01,13.8460039}, +{19.02,13.83973967}, +{19.03,13.83253964}, +{19.04,13.82554697}, +{19.05,13.81834816}, +{19.06,13.81208601}, +{19.07,13.80509565}, +{19.08,13.79789857}, +{19.09,13.79163808}, +{19.1,13.78465005}, +{19.11,13.7774547}, +{19.12,13.77140432}, +{19.13,13.76421018}, +{19.14,13.75701657}, +{19.15,13.75096847}, +{19.16,13.74377608}, +{19.17,13.73658422}, +{19.18,13.73053841}, +{19.19,13.72334777}, +{19.2,13.7170935}, +{19.21,13.71011415}, +{19.22,13.70386106}, +{19.23,13.6966727}, +{19.24,13.69042063}, +{19.25,13.68344434}, +{19.26,13.67719346}, +{19.27,13.67000739}, +{19.28,13.66396915}, +{19.29,13.65678432}, +{19.3,13.65053567}, +{19.31,13.64356412}, +{19.32,13.63759873}, +{19.33,13.63041613}, +{19.34,13.62438244}, +{19.35,13.61813662}, +{19.36,13.61095581}, +{19.37,13.60471102}, +{19.38,13.59774489}, +{19.39,13.59150132}, +{19.4,13.58525826}, +{19.41,13.57857588}, +{19.42,13.57233396}, +{19.43,13.56609255}, +{19.44,13.55891633}, +{19.45,13.55289095}, +{19.46,13.54665128}, +{19.47,13.54069304}, +{19.48,13.53445433}, +{19.49,13.52821614}, +{19.5,13.52225917}, +{19.51,13.51623801}, +{19.52,13.51000148}, +{19.53,13.50404595}, +{19.54,13.49781038}, +{19.55,13.49157533}, +{19.56,13.48534082}, +{19.57,13.479387}, +{19.58,13.47315345}, +{19.59,13.46713797}, +{19.6,13.46118561}, +{19.61,13.45495373}, +{19.62,13.44900223}, +{19.63,13.44277133}, +{19.64,13.43654096}, +{19.65,13.43059075}, +{19.66,13.42436136}, +{19.67,13.41906737}, +{19.68,13.41311843}, +{19.69,13.40710987}, +{19.7,13.40088269}, +{19.71,13.39493524}, +{19.72,13.38870905}, +{19.73,13.38369717}, +{19.74,13.37747196}, +{19.75,13.37124729}, +{19.76,13.36530198}, +{19.77,13.3590783}, +{19.78,13.35406848}, +{19.79,13.3480669}, +{19.8,13.34184492}, +{19.81,13.33590196}, +{19.82,13.32968098}, +{19.83,13.32467339}, +{19.84,13.3184534}, +{19.85,13.31251216}, +{19.86,13.30722762}, +{19.87,13.30128723}, +{19.88,13.29506924}, +{19.89,13.28978619}, +{19.9,13.28384711}, +{19.91,13.27763066}, +{19.92,13.27262679}, +{19.93,13.26641135}, +{19.94,13.26069779}, +{19.95,13.2554178}, +{19.96,13.2494815}, +{19.97,13.24326825}, +{19.98,13.23826703}, +{19.99,13.23205479}, +{20,13.2270544}, +{20.01,13.220843}, +{20.02,13.2149095}, +{20.03,13.20963338}, +{20.04,13.20370058}, +{20.05,13.19842544}, +{20.06,13.19249351}, +{20.07,13.18721934}, +{20.08,13.18128829}, +{20.09,13.1760151}, +{20.1,13.17008493}, +{20.11,13.1650891}, +{20.12,13.15888343}, +{20.13,13.15388844}, +{20.14,13.14768379}, +{20.15,13.14268964}, +{20.16,13.13648602}, +{20.17,13.13149272}, +{20.18,13.12529013}, +{20.19,13.12029767}, +{20.2,13.11437189}, +{20.21,13.10910451}, +{20.22,13.10411329}, +{20.23,13.09791323}, +{20.24,13.09292286}, +{20.25,13.08699927}, +{20.26,13.08173433}, +{20.27,13.0767452}, +{20.28,13.07054769}, +{20.29,13.06555942}, +{20.3,13.06057154}, +{20.31,13.05437555}, +{20.32,13.04938853}, +{20.33,13.04412703}, +{20.34,13.03820743}, +{20.35,13.03322167}, +{20.36,13.02796162}, +{20.37,13.02204335}, +{20.38,13.01705885}, +{20.39,13.01180026}, +{20.4,13.00588332}, +{20.41,13.00090008}, +{20.42,12.99564296}, +{20.43,12.99066057}, +{20.44,12.9847454}, +{20.45,12.97948974}, +{20.46,12.97450862}, +{20.47,12.9695279}, +{20.48,12.96427372}, +{20.49,12.95836078}, +{20.5,12.95338133}, +{20.51,12.94812863}, +{20.52,12.94315004}, +{20.53,12.93817186}, +{20.54,12.93292063}, +{20.55,12.92701037}, +{20.56,12.92203347}, +{20.57,12.91678373}, +{20.58,12.9118077}, +{20.59,12.90683207}, +{20.6,12.90185684}, +{20.61,12.89660905}, +{20.62,12.89046644}, +{20.63,12.88549235}, +{20.64,12.88051866}, +{20.65,12.87527266}, +{20.66,12.87029984}, +{20.67,12.86532743}, +{20.68,12.86008293}, +{20.69,12.8551114}, +{20.7,12.85014027}, +{20.71,12.84516956}, +{20.72,12.83992702}, +{20.73,12.83495719}, +{20.74,12.82998776}, +{20.75,12.82501875}, +{20.76,12.82005016}, +{20.77,12.8145721}, +{20.78,12.80960421}, +{20.79,12.80463674}, +{20.8,12.79966968}, +{20.81,12.79443141}, +{20.82,12.78946524}, +{20.83,12.78449948}, +{20.84,12.77953415}, +{20.85,12.77456923}, +{20.86,12.76933342}, +{20.87,12.76436939}, +{20.88,12.75940579}, +{20.89,12.75420264}, +{20.9,12.75017184}, +{20.91,12.7452093}, +{20.92,12.73997626}, +{20.93,12.73501461}, +{20.94,12.73005338}, +{20.95,12.72509257}, +{20.96,12.72013219}, +{20.97,12.71517224}, +{20.98,12.71063271}, +{20.99,12.70567346}, +{21,12.70071463}, +{21.01,12.69575622}, +{21.02,12.6907982}, +{21.03,12.68677252}, +{21.04,12.68181537}, +{21.05,12.67658858}, +{21.06,12.67163233}, +{21.07,12.66643363}, +{21.08,12.6624098}, +{21.09,12.65745464}, +{21.1,12.65249991}, +{21.11,12.64754561}, +{21.12,12.64352336}, +{21.13,12.63830036}, +{21.14,12.63334738}, +{21.15,12.62815066}, +{21.16,12.62412991}, +{21.17,12.61917802}, +{21.18,12.61422657}, +{21.19,12.61020702}, +{21.2,12.6052564}, +{21.21,12.60030623}, +{21.22,12.59604259}, +{21.23,12.59109308}, +{21.24,12.58707536}, +{21.25,12.58188098}, +{21.26,12.57786389}, +{21.27,12.57384718}, +{21.28,12.56865347}, +{21.29,12.56490597}, +{21.3,12.56064385}, +{21.31,12.55569711}, +{21.32,12.551682}, +{21.33,12.54742049}, +{21.34,12.54247484}, +{21.35,12.53846075}, +{21.36,12.53446804}, +{21.37,12.5304545}, +{21.38,12.52551029}, +{21.39,12.52124994}, +{21.4,12.51723743}, +{21.41,12.51204644}, +{21.42,12.50830244}, +{21.43,12.5042909}, +{21.44,12.50003148}, +{21.45,12.49508964}, +{21.46,12.49107914}, +{21.47,12.48682033}, +{21.48,12.48307799}, +{21.49,12.47788864}, +{21.5,12.47387935}, +{21.51,12.46987045}, +{21.52,12.46561258}, +{21.53,12.46160431}, +{21.54,12.45668327}, +{21.55,12.45267558}, +{21.56,12.44866828}, +{21.57,12.44441136}, +{21.58,12.4404047}, +{21.59,12.43548443}, +{21.6,12.43147835}, +{21.61,12.42747267}, +{21.62,12.42321669}, +{21.63,12.41947837}, +{21.64,12.41522271}, +{21.65,12.41121823}, +{21.66,12.40628365}, +{21.67,12.40202862}, +{21.68,12.39829166}, +{21.69,12.39403695}, +{21.7,12.3900341}, +{21.71,12.38603165}, +{21.72,12.38204381}, +{21.73,12.37804193}, +{21.74,12.37285783}, +{21.75,12.36885662}, +{21.76,12.36512184}, +{21.77,12.36086866}, +{21.78,12.35686841}, +{21.79,12.35261563}, +{21.8,12.34888185}, +{21.81,12.34488258}, +{21.82,12.34063037}, +{21.83,12.33663175}, +{21.84,12.33264554}, +{21.85,12.32864749}, +{21.86,12.32439601}, +{21.87,12.32039861}, +{21.88,12.316667}, +{21.89,12.31241609}, +{21.9,12.30841968}, +{21.91,12.30416917}, +{21.92,12.30043857}, +{21.93,12.29618839}, +{21.94,12.29219321}, +{21.95,12.28846345}, +{21.96,12.28421385}, +{21.97,12.28021966}, +{21.98,12.27623533}, +{21.99,12.27224173}, +{22,12.26799287}, +{22.01,12.2639999}, +{22.02,12.26001611}, +{22.03,12.25602374}, +{22.04,12.25203179}, +{22.05,12.24804842}, +{22.06,12.24405705}, +{22.07,12.23980952}, +{22.08,12.23608316}, +{22.09,12.23183596}, +{22.1,12.22784584}, +{22.11,12.22386326}, +{22.12,12.22080329}, +{22.13,12.21681415}, +{22.14,12.21283198}, +{22.15,12.20884343}, +{22.16,12.20459758}, +{22.17,12.20087358}, +{22.18,12.19662806}, +{22.19,12.19264077}, +{22.2,12.1886594}, +{22.21,12.18560208}, +{22.22,12.18135731}, +{22.23,12.17763483}, +{22.24,12.17364912}, +{22.25,12.16940496}, +{22.26,12.16568333}, +{22.27,12.16236877}, +{22.28,12.15838431}, +{22.29,12.15440414}, +{22.3,12.15042028}, +{22.31,12.14644038}, +{22.32,12.14245712}, +{22.33,12.13914354}, +{22.34,12.13542393}, +{22.35,12.13118156}, +{22.36,12.12719955}, +{22.37,12.12322045}, +{22.38,12.12016807}, +{22.39,12.11644981}, +{22.4,12.11220839}, +{22.41,12.10822799}, +{22.42,12.10517852}, +{22.43,12.1011987}, +{22.44,12.09722055}, +{22.45,12.09324134}, +{22.46,12.0899299}, +{22.47,12.08621368}, +{22.48,12.08197372}, +{22.49,12.07825803}, +{22.5,12.0749472}, +{22.51,12.07096985}, +{22.52,12.06699275}, +{22.53,12.063016}, +{22.54,12.05996787}, +{22.55,12.05599171}, +{22.56,12.05175323}, +{22.57,12.04896821}, +{22.58,12.04473009}, +{22.59,12.04101694}, +{22.6,12.03770776}, +{22.61,12.0339951}, +{22.62,12.02975769}, +{22.63,12.02671252}, +{22.64,12.022737}, +{22.65,12.01876391}, +{22.66,12.01571712}, +{22.67,12.01174463}, +{22.68,12.00776965}, +{22.69,12.00472615}, +{22.7,12.00075143}, +{22.71,11.99678015}, +{22.72,11.99373403}, +{22.73,11.98976334}, +{22.74,11.98552815}, +{22.75,11.98274735}, +{22.76,11.97851251}, +{22.77,11.97573217}, +{22.78,11.97149769}, +{22.79,11.96778965}, +{22.8,11.96448369}, +{22.81,11.96077615}, +{22.82,11.95747051}, +{22.83,11.95376347}, +{22.84,11.94953007}, +{22.85,11.94675161}, +{22.86,11.94251858}, +{22.87,11.93947411}, +{22.88,11.93550791}, +{22.89,11.93246366}, +{22.9,11.92849806}, +{22.91,11.92545404}, +{22.92,11.92148904}, +{22.93,11.91751736}, +{22.94,11.91448085}, +{22.95,11.91050943}, +{22.96,11.90747349}, +{22.97,11.90350234}, +{22.98,11.90046697}, +{22.99,11.89649608}, +{23,11.89346127}, +{23.01,11.8894906}, +{23.02,11.88645641}, +{23.03,11.88248606}, +{23.04,11.87918389}, +{23.05,11.87548231}, +{23.06,11.87218047}, +{23.07,11.86847939}, +{23.08,11.86517787}, +{23.09,11.86147731}, +{23.1,11.85817612}, +{23.11,11.85540358}, +{23.12,11.85117522}, +{23.13,11.84840314}, +{23.14,11.84417515}, +{23.15,11.84113377}, +{23.16,11.83717593}, +{23.17,11.83413478}, +{23.18,11.83043642}, +{23.19,11.82713664}, +{23.2,11.82436611}, +{23.21,11.82013934}, +{23.22,11.81736929}, +{23.23,11.81314289}, +{23.24,11.8101025}, +{23.25,11.8061473}, +{23.26,11.80310713}, +{23.27,11.80033817}, +{23.28,11.79611262}, +{23.29,11.79334414}, +{23.3,11.78911897}, +{23.31,11.78635096}, +{23.32,11.78305325}, +{23.33,11.77908676}, +{23.34,11.77631937}, +{23.35,11.77302208}, +{23.36,11.76932815}, +{23.37,11.76603119}, +{23.38,11.76326473}, +{23.39,11.75904116}, +{23.4,11.75600251}, +{23.41,11.75230977}, +{23.42,11.74901358}, +{23.43,11.74624821}, +{23.44,11.74202551}, +{23.45,11.73898739}, +{23.46,11.73622265}, +{23.47,11.73292721}, +{23.48,11.72923616}, +{23.49,11.72594107}, +{23.5,11.72317726}, +{23.51,11.71921311}, +{23.52,11.71591844}, +{23.53,11.71315525}, +{23.54,11.70893428}, +{23.55,11.70617158}, +{23.56,11.70313467}, +{23.57,11.69984077}, +{23.58,11.6961521}, +{23.59,11.6931155}, +{23.6,11.69009692}, +{23.61,11.68613405}, +{23.62,11.68311607}, +{23.63,11.68007992}, +{23.64,11.67704385}, +{23.65,11.67310019}, +{23.66,11.67006435}, +{23.67,11.66730427}, +{23.68,11.66401209}, +{23.69,11.66005021}, +{23.7,11.65729077}, +{23.71,11.65399901}, +{23.72,11.65124002}, +{23.73,11.64794862}, +{23.74,11.64398738}, +{23.75,11.64122902}, +{23.76,11.63793805}, +{23.77,11.63490335}, +{23.78,11.63214559}, +{23.79,11.62792892}, +{23.8,11.62517165}, +{23.81,11.62213741}, +{23.82,11.61884732}, +{23.83,11.61609065}, +{23.84,11.61213068}, +{23.85,11.6091187}, +{23.86,11.606085}, +{23.87,11.60305137}, +{23.88,11.60004013}, +{23.89,11.59700673}, +{23.9,11.59332571}, +{23.91,11.59003701}, +{23.92,11.58700393}, +{23.93,11.58424944}, +{23.94,11.58096119}, +{23.95,11.5779284}, +{23.96,11.57517451}, +{23.97,11.57096094}, +{23.98,11.56820755}, +{23.99,11.56517523}, +{24,11.56214297}, +{24.01,11.55913509}, +{24.02,11.55610306}, +{24.03,11.55307111}, +{24.04,11.55006398}, +{24.05,11.54610666}, +{24.06,11.54307502}, +{24.07,11.54006867}, +{24.08,11.53703727}, +{24.09,11.53428619}, +{24.1,11.53125501}, +{24.11,11.52796925}, +{24.12,11.52521878}, +{24.13,11.5221879}, +{24.14,11.5189026}, +{24.15,11.51615274}, +{24.16,11.51312217}, +{24.17,11.51009166}, +{24.18,11.50616274}, +{24.19,11.50313249}, +{24.2,11.5001023}, +{24.21,11.4970995}, +{24.22,11.49406956}, +{24.23,11.49132142}, +{24.24,11.48829169}, +{24.25,11.48500801}, +{24.26,11.48226049}, +{24.27,11.47923107}, +{24.28,11.47620172}, +{24.29,11.47320095}, +{24.3,11.47017184}, +{24.31,11.4671428}, +{24.32,11.4643965}, +{24.33,11.46111399}, +{24.34,11.45808527}, +{24.35,11.45533958}, +{24.36,11.45231108}, +{24.37,11.44902913}, +{24.38,11.44628407}, +{24.39,11.44325587}, +{24.4,11.44022775}, +{24.41,11.43722995}, +{24.42,11.43420207}, +{24.43,11.43117426}, +{24.44,11.42843043}, +{24.45,11.42514967}, +{24.46,11.42212218}, +{24.47,11.41937897}, +{24.48,11.41635169}, +{24.49,11.41332449}, +{24.5,11.41125361}, +{24.51,11.40822664}, +{24.52,11.40519974}, +{24.53,11.40245772}, +{24.54,11.39917826}, +{24.55,11.39615168}, +{24.56,11.39341029}, +{24.57,11.39038393}, +{24.58,11.38735764}, +{24.59,11.38436428}, +{24.6,11.38133823}, +{24.61,11.37831227}, +{24.62,11.37528637}, +{24.63,11.37254637}, +{24.64,11.37019274}, +{24.65,11.36716715}, +{24.66,11.36442775}, +{24.67,11.36140238}, +{24.68,11.35837708}, +{24.69,11.35538612}, +{24.7,11.35236108}, +{24.71,11.3493361}, +{24.72,11.34659795}, +{24.73,11.34449743}, +{24.74,11.34122079}, +{24.75,11.3381962}, +{24.76,11.33545879}, +{24.77,11.33243443}, +{24.78,11.32941013}, +{24.79,11.32642159}, +{24.8,11.32432166}, +{24.81,11.32129767}, +{24.82,11.31856148}, +{24.83,11.31553772}, +{24.84,11.31251404}, +{24.85,11.30923892}, +{24.86,11.30742751}, +{24.87,11.30440414}, +{24.88,11.30138084}, +{24.89,11.29864603}, +{24.9,11.29562296}, +{24.91,11.29234869}, +{24.92,11.29053838}, +{24.93,11.28751563}, +{24.94,11.28449294}, +{24.95,11.28147034}, +{24.96,11.2787369}, +{24.97,11.27663828},}; + + + arma::uvec index_E = find(LkTable<=t_disparo, 1, "last"); + + if (index_E(0)<(LkTable.n_rows-1)){ + double tdisparo1=LkTable(index_E(0),0); + double tdisparo2=LkTable(index_E(0)+1,0); + double Acc1=LkTable(index_E(0),1); + double Acc2=LkTable(index_E(0)+1,1); + + Acc=(t_disparo-tdisparo1)*(Acc2-Acc1)/(tdisparo2-tdisparo1)+Acc1; + }else{ + E=0; + } + + return Acc; +} + double Vtl_Engine::EmpujeLkTable(double t_disparo) { double E; diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index a7b081f3f..ec5db6fc9 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -107,7 +107,9 @@ private: void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); + void accelerometer(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); double EmpujeLkTable(double t_disparo); + double AccLkTable(double t_disparo); }; /** \} */ From 8760858fb042925c846d5d1f541e844756e1d162 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Tue, 14 Mar 2023 23:53:34 +0000 Subject: [PATCH 150/165] Fix: Vtl_engine bug --- src/algorithms/PVT/libs/vtl_engine.cc | 5026 +++++++++++++------------ 1 file changed, 2515 insertions(+), 2511 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 149d220d9..808dbe77d 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -474,2520 +474,2524 @@ void Vtl_Engine::accelerometer(double &acc_x, double &acc_y, double &acc_z, arma double Vtl_Engine::AccLkTable(double t_disparo) { double Acc; - arma::mat LkTable={ -{0,191.3693381}, -{0.01,391.0831124}, -{0.02,385.6263172}, -{0.03,379.2536529}, -{0.04,372.8504183}, -{0.05,366.4351054}, -{0.06,359.9487249}, -{0.07,353.4523708}, -{0.08,346.9151605}, -{0.09,340.3533742}, -{0.1,339.9823669}, -{0.11,339.649644}, -{0.12,339.3131193}, -{0.13,338.9712498}, -{0.14,338.6260923}, -{0.15,338.2779261}, -{0.16,337.9238608}, -{0.17,337.5646868}, -{0.18,337.2045652}, -{0.19,336.8363276}, -{0.2,337.6125746}, -{0.21,338.3898123}, -{0.22,339.1721974}, -{0.23,339.9508641}, -{0.24,340.7342869}, -{0.25,341.5135997}, -{0.26,342.2964879}, -{0.27,343.0777302}, -{0.28,343.8624016}, -{0.29,344.6450452}, -{0.3,345.4302226}, -{0.31,346.2163638}, -{0.32,347.003086}, -{0.33,347.7904035}, -{0.34,348.5766329}, -{0.35,349.3650418}, -{0.36,350.1543455}, -{0.37,350.9437807}, -{0.38,351.735714}, -{0.39,352.5236761}, -{0.4,353.317052}, -{0.41,354.1104116}, -{0.42,354.9032791}, -{0.43,355.6925592}, -{0.44,356.490275}, -{0.45,357.2834519}, -{0.46,358.0785073}, -{0.47,358.8713231}, -{0.48,359.668187}, -{0.49,360.4658938}, -{0.5,361.2644865}, -{0.51,362.0577138}, -{0.52,362.8553987}, -{0.53,363.6519585}, -{0.54,364.4531391}, -{0.55,365.2484945}, -{0.56,366.0474406}, -{0.57,366.8445417}, -{0.58,367.645723}, -{0.59,368.4312816}, -{0.6,369.2180327}, -{0.61,370.0057638}, -{0.62,370.7934491}, -{0.63,371.577332}, -{0.64,372.3618727}, -{0.65,373.1392343}, -{0.66,373.9223953}, -{0.67,374.6986382}, -{0.68,375.4776747}, -{0.69,376.2563757}, -{0.7,377.5615194}, -{0.71,378.8593047}, -{0.72,380.1677186}, -{0.73,381.4775822}, -{0.74,382.7850068}, -{0.75,384.0925559}, -{0.76,385.4033472}, -{0.77,386.680222}, -{0.78,387.9603486}, -{0.79,389.2414657}, -{0.8,390.5176097}, -{0.81,391.7783411}, -{0.82,393.0383665}, -{0.83,394.29121}, -{0.84,395.5451831}, -{0.85,396.779476}, -{0.86,398.0051316}, -{0.87,399.217608}, -{0.88,400.433585}, -{0.89,401.6396038}, -{0.9,402.8249609}, -{0.91,403.9998189}, -{0.92,405.1644136}, -{0.93,406.3320948}, -{0.94,407.4898264}, -{0.95,408.6386952}, -{0.96,409.7882084}, -{0.97,410.9206965}, -{0.98,412.0711172}, -{0.99,413.2273621}, -{1,414.3762955}, -{1.01,415.5172083}, -{1.02,416.6851116}, -{1.03,417.8740344}, -{1.04,419.0501524}, -{1.05,420.2265404}, -{1.06,421.4316749}, -{1.07,422.6518329}, -{1.08,423.879432}, -{1.09,425.0968591}, -{1.1,426.3705142}, -{1.11,427.6814206}, -{1.12,428.9906538}, -{1.13,430.3080079}, -{1.14,431.6231364}, -{1.15,432.9423547}, -{1.16,434.2603966}, -{1.17,435.5806367}, -{1.18,436.9038196}, -{1.19,438.2359771}, -{1.2,439.760283}, -{1.21,441.2855662}, -{1.22,442.8237912}, -{1.23,444.3543307}, -{1.24,445.8935896}, -{1.25,447.4374141}, -{1.26,449.016306}, -{1.27,450.6057334}, -{1.28,452.1975701}, -{1.29,453.7990419}, -{1.3,455.3951522}, -{1.31,457.0042091}, -{1.32,458.6230155}, -{1.33,460.2387287}, -{1.34,461.8608819}, -{1.35,463.4956492}, -{1.36,465.1298266}, -{1.37,466.7670335}, -{1.38,468.4087662}, -{1.39,470.0591738}, -{1.4,471.7211266}, -{1.41,473.4149126}, -{1.42,475.0975536}, -{1.43,476.8052568}, -{1.44,478.5125067}, -{1.45,480.2302357}, -{1.46,481.9505168}, -{1.47,483.6755965}, -{1.48,485.4085203}, -{1.49,487.1606942}, -{1.5,488.912737}, -{1.51,490.6763352}, -{1.52,492.4461934}, -{1.53,494.230761}, -{1.54,496.0142138}, -{1.55,497.8000122}, -{1.56,499.5868444}, -{1.57,501.3897915}, -{1.58,503.1978733}, -{1.59,505.0179768}, -{1.6,506.8420775}, -{1.61,508.6810082}, -{1.62,510.5317451}, -{1.63,512.3834648}, -{1.64,514.2492508}, -{1.65,516.1190989}, -{1.66,518.0053088}, -{1.67,519.9107613}, -{1.68,521.8122756}, -{1.69,523.7277473}, -{1.7,526.52496}, -{1.71,529.326074}, -{1.72,532.1521584}, -{1.73,534.9959392}, -{1.74,537.8663106}, -{1.75,532.5465503}, -{1.76,527.1860241}, -{1.77,521.7618548}, -{1.78,516.3328104}, -{1.79,510.856735}, -{1.8,505.3589191}, -{1.81,499.7424161}, -{1.82,494.1151004}, -{1.83,488.4906155}, -{1.84,482.7875458}, -{1.85,477.0777855}, -{1.86,471.3509003}, -{1.87,465.5580034}, -{1.88,459.7619075}, -{1.89,453.944495}, -{1.9,448.0865852}, -{1.91,442.2123527}, -{1.92,436.3035816}, -{1.93,430.3563548}, -{1.94,424.4002234}, -{1.95,406.4045572}, -{1.96,388.2433759}, -{1.97,370.1079284}, -{1.98,351.8103035}, -{1.99,333.4616772}, -{2,320.4688275}, -{2.01,307.452342}, -{2.02,294.3528443}, -{2.03,281.2674437}, -{2.04,268.111713}, -{2.05,254.9637706}, -{2.06,241.7924305}, -{2.07,228.5260562}, -{2.08,215.276914}, -{2.09,201.9631912}, -{2.1,188.6795429}, -{2.11,175.4304874}, -{2.12,162.00753}, -{2.13,148.7345206}, -{2.14,135.3574292}, -{2.15,126.6856641}, -{2.16,118.020273}, -{2.17,109.3739993}, -{2.18,100.6846937}, -{2.19,92.00283749}, -{2.2,83.35951249}, -{2.21,74.73486872}, -{2.22,66.0716783}, -{2.23,57.44510561}, -{2.24,48.79168522}, -{2.25,40.27062222}, -{2.26,31.7147632}, -{2.27,23.35713095}, -{2.28,15.32234163}, -{2.29,8.698553788}, -{2.3,7.708774611}, -{2.31,7.248557512}, -{2.32,7.391062373}, -{2.33,8.108155956}, -{2.34,9.264564858}, -{2.35,10.7236636}, -{2.36,12.36361185}, -{2.37,14.13412431}, -{2.38,15.9852933}, -{2.39,17.90058977}, -{2.4,105.5371052}, -{2.41,105.310299}, -{2.42,105.0834932}, -{2.43,104.8576945}, -{2.44,104.6317221}, -{2.45,104.4076761}, -{2.46,104.1836307}, -{2.47,103.9608891}, -{2.48,103.7368451}, -{2.49,103.5154945}, -{2.5,103.2936742}, -{2.51,103.0736278}, -{2.52,102.8522788}, -{2.53,102.6331529}, -{2.54,102.413108}, -{2.55,102.1952862}, -{2.56,101.9774649}, -{2.57,101.7597303}, -{2.58,101.5428295}, -{2.59,101.3269349}, -{2.6,101.1113383}, -{2.61,100.8953588}, -{2.62,100.6826079}, -{2.63,100.4688523}, -{2.64,100.2551829}, -{2.65,100.0437366}, -{2.66,99.83090244}, -{2.67,99.61945733}, -{2.68,99.4088471}, -{2.69,99.19870611}, -{2.7,98.98910215}, -{2.71,98.77988202}, -{2.72,98.57288514}, -{2.73,98.36467091}, -{2.74,98.15767517}, -{2.75,97.95168467}, -{2.76,97.74469009}, -{2.77,97.53870065}, -{2.78,97.33484982}, -{2.79,97.13016437}, -{2.8,96.92731915}, -{2.81,96.7234702}, -{2.82,96.52062617}, -{2.83,96.31816569}, -{2.84,96.11754558}, -{2.85,95.91692608}, -{2.86,95.71630718}, -{2.87,95.51699175}, -{2.88,95.31729406}, -{2.89,95.11889985}, -{2.9,94.92059003}, -{2.91,94.72349986}, -{2.92,94.52602756}, -{2.93,94.32994227}, -{2.94,94.13385751}, -{2.95,93.93768991}, -{2.96,93.7438292}, -{2.97,93.54904893}, -{2.98,93.35610946}, -{2.99,93.16225043}, -{3,92.96922916}, -{3.01,92.7776772}, -{3.02,92.58566036}, -{3.03,92.39494689}, -{3.04,92.20423405}, -{3.05,92.0148246}, -{3.06,91.8250333}, -{3.07,91.63662788}, -{3.08,91.44814059}, -{3.09,91.25973628}, -{3.1,91.0721706}, -{3.11,90.8859905}, -{3.12,90.69972888}, -{3.13,90.51354996}, -{3.14,90.32829199}, -{3.15,90.14433732}, -{3.16,89.96000096}, -{3.17,89.77512703}, -{3.18,89.59301505}, -{3.19,89.40998325}, -{3.2,89.22825475}, -{3.21,89.04614472}, -{3.22,88.86533802}, -{3.23,88.68461319}, -{3.24,88.5047283}, -{3.25,88.32530723}, -{3.26,88.14542365}, -{3.27,87.96684341}, -{3.28,87.78834468}, -{3.29,87.61168778}, -{3.3,87.43411084}, -{3.31,87.25775651}, -{3.32,87.08148342}, -{3.33,86.90613162}, -{3.34,86.73078044}, -{3.35,86.55542988}, -{3.36,86.38230327}, -{3.37,86.20695398}, -{3.38,86.03474946}, -{3.39,85.86170475}, -{3.4,85.68988324}, -{3.41,85.51768068}, -{3.42,85.34686104}, -{3.43,85.17596234}, -{3.44,85.00598522}, -{3.45,84.83516737}, -{3.46,84.6661919}, -{3.47,84.4965984}, -{3.48,84.32854516}, -{3.49,84.16087409}, -{3.5,83.99358512}, -{3.51,83.82583632}, -{3.52,83.65908817}, -{3.53,83.49272211}, -{3.54,83.32727771}, -{3.55,83.16183397}, -{3.56,82.99731197}, -{3.57,82.8328691}, -{3.58,82.66872971}, -{3.59,82.50551211}, -{3.6,82.34199214}, -{3.61,82.18007827}, -{3.62,82.01870507}, -{3.63,81.85648938}, -{3.64,81.69549878}, -{3.65,81.53458669}, -{3.66,81.37421531}, -{3.67,81.21452915}, -{3.68,81.05546147}, -{3.69,80.89547312}, -{3.7,80.73770906}, -{3.71,80.5790243}, -{3.72,80.4212615}, -{3.73,80.26442074}, -{3.74,80.10758065}, -{3.75,79.95112215}, -{3.76,79.79558572}, -{3.77,79.63966914}, -{3.78,79.48505557}, -{3.79,79.32959782}, -{3.8,79.17536637}, -{3.81,79.02167638}, -{3.82,78.8684442}, -{3.83,78.71605789}, -{3.84,78.56282692}, -{3.85,78.41143969}, -{3.86,78.25905551}, -{3.87,78.10805022}, -{3.88,77.95666494}, -{3.89,77.80650681}, -{3.9,77.6564251}, -{3.91,77.50726573}, -{3.92,77.35718532}, -{3.93,77.20840774}, -{3.94,77.06017211}, -{3.95,76.9123176}, -{3.96,76.76446376}, -{3.97,76.62527605}, -{3.98,76.48731601}, -{3.99,76.34943145}, -{4,76.21154735}, -{4.01,76.0745109}, -{4.02,75.93754972}, -{4.03,75.80096926}, -{4.04,75.66493099}, -{4.05,75.5283515}, -{4.06,75.39323622}, -{4.07,75.25757969}, -{4.08,75.12284564}, -{4.09,74.98719015}, -{4.1,74.85375922}, -{4.11,74.7199488}, -{4.12,74.58613897}, -{4.13,74.4527097}, -{4.14,74.32020305}, -{4.15,74.18677489}, -{4.16,74.05434288}, -{4.17,73.92275998}, -{4.18,73.79025558}, -{4.19,73.65874716}, -{4.2,73.52754586}, -{4.21,73.39603843}, -{4.22,73.26668262}, -{4.23,73.13647818}, -{4.24,73.00527914}, -{4.25,72.87692017}, -{4.26,72.74664453}, -{4.27,72.61828668}, -{4.28,72.48938671}, -{4.29,72.36095745}, -{4.3,72.23298084}, -{4.31,72.10500473}, -{4.32,71.97664957}, -{4.33,71.85089875}, -{4.34,71.72384656}, -{4.35,71.59672298}, -{4.36,71.47059439}, -{4.37,71.34484579}, -{4.38,71.21909773}, -{4.39,71.0938933}, -{4.4,70.96821788}, -{4.41,70.8433939}, -{4.42,70.71949301}, -{4.43,70.59467021}, -{4.44,70.4711498}, -{4.45,70.34824439}, -{4.46,70.22472508}, -{4.47,70.10174977}, -{4.48,69.97915428}, -{4.49,69.85663016}, -{4.5,69.73403583}, -{4.51,69.61243537}, -{4.52,69.4907649}, -{4.53,69.37046724}, -{4.54,69.248798}, -{4.55,69.12757867}, -{4.56,69.00683342}, -{4.57,68.88746064}, -{4.58,68.7680884}, -{4.59,68.64772398}, -{4.6,68.52927568}, -{4.61,68.40990511}, -{4.62,68.29183674}, -{4.63,68.17246724}, -{4.64,68.05433046}, -{4.65,67.93718666}, -{4.66,67.81966471}, -{4.67,67.70252207}, -{4.68,67.58537999}, -{4.69,67.4682385}, -{4.7,67.35202056}, -{4.71,67.23618183}, -{4.72,67.12003386}, -{4.73,67.00511924}, -{4.74,66.88928217}, -{4.75,66.77399025}, -{4.76,66.65907746}, -{4.77,66.54508839}, -{4.78,66.43024508}, -{4.79,66.31718035}, -{4.8,66.20357152}, -{4.81,66.08965293}, -{4.82,65.97696844}, -{4.83,65.86397411}, -{4.84,65.75129087}, -{4.85,65.63959913}, -{4.86,65.52691713}, -{4.87,65.4152265}, -{4.88,65.30346905}, -{4.89,65.19177955}, -{4.9,65.08101391}, -{4.91,64.97055983}, -{4.92,64.85979538}, -{4.93,64.74940953}, -{4.94,64.63994759}, -{4.95,64.5304195}, -{4.96,64.42058083}, -{4.97,64.31112069}, -{4.98,64.20296247}, -{4.99,64.09350346}, -{5,63.98496852}, -{5.01,63.8777355}, -{5.02,63.76920181}, -{5.03,63.66197001}, -{5.04,63.55473882}, -{5.05,63.44713053}, -{5.06,63.33990058}, -{5.07,63.23359487}, -{5.08,63.1272898}, -{5.09,63.0214284}, -{5.1,62.91512453}, -{5.11,62.809745}, -{5.12,62.70480883}, -{5.13,62.59980807}, -{5.14,62.49480796}, -{5.15,62.39041969}, -{5.16,62.28542084}, -{5.17,62.18134645}, -{5.18,62.07733733}, -{5.19,61.9745654}, -{5.2,61.8705574}, -{5.21,61.76740949}, -{5.22,61.66470377}, -{5.23,61.56162134}, -{5.24,61.45977657}, -{5.25,61.35799646}, -{5.26,61.25615307}, -{5.27,61.15437415}, -{5.28,61.05259579}, -{5.29,60.951742}, -{5.3,60.84990134}, -{5.31,60.74942582}, -{5.32,60.64949795}, -{5.33,60.54902358}, -{5.34,60.44909694}, -{5.35,60.34862373}, -{5.36,60.24999934}, -{5.37,60.15007461}, -{5.38,60.05182832}, -{5.39,59.95320583}, -{5.4,59.85365978}, -{5.41,59.75596279}, -{5.42,59.65734225}, -{5.43,59.55909902}, -{5.44,59.46140404}, -{5.45,59.36408636}, -{5.46,59.26775553}, -{5.47,59.17043906}, -{5.48,59.07404759}, -{5.49,58.97635595}, -{5.5,58.88040385}, -{5.51,58.78493873}, -{5.52,58.68854994}, -{5.53,58.5931475}, -{5.54,58.49806084}, -{5.55,58.40265956}, -{5.56,58.30757421}, -{5.57,58.21347488}, -{5.58,58.1180146}, -{5.59,58.02391646}, -{5.6,57.92975833}, -{5.61,57.83566137}, -{5.62,57.74242924}, -{5.63,57.64925811}, -{5.64,57.55646364}, -{5.65,57.46323365}, -{5.66,57.37006437}, -{5.67,57.27727167}, -{5.68,57.18540423}, -{5.69,57.09310183}, -{5.7,57.00123568}, -{5.71,56.90937012}, -{5.72,56.81842995}, -{5.73,56.7265656}, -{5.74,56.63562668}, -{5.75,56.54468839}, -{5.76,56.45505132}, -{5.77,56.3641143}, -{5.78,56.27447849}, -{5.79,56.18446768}, -{5.8,56.09483315}, -{5.81,56.00482369}, -{5.82,55.91519047}, -{5.83,55.82648287}, -{5.84,55.73685095}, -{5.85,55.6481447}, -{5.86,55.56036419}, -{5.87,55.47171714}, -{5.88,55.38338824}, -{5.89,55.29560974}, -{5.9,55.20783199}, -{5.91,55.12048763}, -{5.92,55.03271123}, -{5.93,54.94623595}, -{5.94,54.85889329}, -{5.95,54.77241935}, -{5.96,54.6856279}, -{5.97,54.59953044}, -{5.98,54.51274022}, -{5.99,54.42756936}, -{6,54.34208069}, -{6.01,54.2565362}, -{6.02,54.17142378}, -{6.03,54.08588077}, -{6.04,54.00169487}, -{6.05,53.91615335}, -{6.06,53.8319687}, -{6.07,53.74778462}, -{6.08,53.66317074}, -{6.09,53.57991343}, -{6.1,53.49573124}, -{6.11,53.41247515}, -{6.12,53.32823895}, -{6.13,53.24628434}, -{6.14,53.16303021}, -{6.15,53.07977671}, -{6.16,52.99782396}, -{6.17,52.91457174}, -{6.18,52.83354588}, -{6.19,52.75252068}, -{6.2,52.67149614}, -{6.21,52.59177232}, -{6.22,52.51172899}, -{6.23,52.43200639}, -{6.24,52.35191018}, -{6.25,52.27311468}, -{6.26,52.19344784}, -{6.27,52.11427943}, -{6.28,52.03461365}, -{6.29,51.95582072}, -{6.3,51.8770285}, -{6.31,51.7992161}, -{6.32,51.72042521}, -{6.33,51.64298797}, -{6.34,51.56512432}, -{6.35,51.4863883}, -{6.36,51.40889994}, -{6.37,51.33109103}, -{6.38,51.25453}, -{6.39,51.1767223}, -{6.4,51.10021497}, -{6.41,51.02365598}, -{6.42,50.94677613}, -{6.43,50.87027056}, -{6.44,50.79371366}, -{6.45,50.71813543}, -{6.46,50.64163163}, -{6.47,50.566003}, -{6.48,50.49042663}, -{6.49,50.41522435}, -{6.5,50.33964914}, -{6.51,50.26407455}, -{6.52,50.18974918}, -{6.53,50.11510217}, -{6.54,50.04082915}, -{6.55,49.96525709}, -{6.56,49.8919116}, -{6.57,49.81764038}, -{6.58,49.74336976}, -{6.59,49.67002613}, -{6.6,49.59575673}, -{6.61,49.52241437}, -{6.62,49.44907265}, -{6.63,49.37665805}, -{6.64,49.30331765}, -{6.65,49.2299779}, -{6.66,49.15793829}, -{6.67,49.08552639}, -{6.68,49.01218863}, -{6.69,48.94015097}, -{6.7,48.86779023}, -{6.71,48.7966804}, -{6.72,48.72464462}, -{6.73,48.65353614}, -{6.74,48.58205566}, -{6.75,48.51007046}, -{6.76,48.43896396}, -{6.77,48.3687849}, -{6.78,48.29767983}, -{6.79,48.22662358}, -{6.8,48.1568191}, -{6.81,48.08669074}, -{6.82,48.01558828}, -{6.83,47.94541339}, -{6.84,47.8756592}, -{6.85,47.80641256}, -{6.86,47.73665949}, -{6.87,47.66778656}, -{6.88,47.59858928}, -{6.89,47.5287908}, -{6.9,47.45992002}, -{6.91,47.39072463}, -{6.92,47.32282897}, -{6.93,47.25396027}, -{6.94,47.18513879}, -{6.95,47.11719857}, -{6.96,47.04967737}, -{6.97,46.98178475}, -{6.98,46.91384668}, -{6.99,46.84595533}, -{7,46.77839063}, -{7.01,46.7114277}, -{7.02,46.64353826}, -{7.03,46.57694835}, -{7.04,46.51031367}, -{7.05,46.44335335}, -{7.06,46.37676531}, -{7.07,46.31017787}, -{7.08,46.24414673}, -{7.09,46.17751571}, -{7.1,46.11185753}, -{7.11,46.04619998}, -{7.12,45.98054307}, -{7.13,45.91395938}, -{7.14,45.84960253}, -{7.15,45.78394749}, -{7.16,45.7182931}, -{7.17,45.65356684}, -{7.18,45.58921251}, -{7.19,45.52356007}, -{7.2,45.45883582}, -{7.21,45.39448341}, -{7.22,45.33013163}, -{7.23,45.26633701}, -{7.24,45.20198655}, -{7.25,45.13856437}, -{7.26,45.07477189}, -{7.27,45.01042343}, -{7.28,44.9470033}, -{7.29,44.88358387}, -{7.3,44.82016513}, -{7.31,44.75771711}, -{7.32,44.69429966}, -{7.33,44.63218143}, -{7.34,44.56969317}, -{7.35,44.50627783}, -{7.36,44.44383274}, -{7.37,44.38171718}, -{7.38,44.31923174}, -{7.39,44.25808684}, -{7.4,44.19560278}, -{7.41,44.13441787}, -{7.42,44.07197633}, -{7.43,44.01079279}, -{7.44,43.94960999}, -{7.45,43.88809827}, -{7.46,43.82691687}, -{7.47,43.76577664}, -{7.48,43.70552469}, -{7.49,43.64434543}, -{7.5,43.58413512}, -{7.51,43.52295728}, -{7.52,43.46274819}, -{7.53,43.40286997}, -{7.54,43.3426621}, -{7.55,43.28241531}, -{7.56,43.22350686}, -{7.57,43.16330081}, -{7.58,43.10398447}, -{7.59,43.04414954}, -{7.6,42.98483474}, -{7.61,42.92592934}, -{7.62,42.86702451}, -{7.63,42.80771199}, -{7.64,42.74880843}, -{7.65,42.68990543}, -{7.66,42.63059523}, -{7.67,42.57169353}, -{7.68,42.51145649}, -{7.69,42.45122031}, -{7.7,42.39102268}, -{7.71,42.332086}, -{7.72,42.27281824}, -{7.73,42.21295458}, -{7.74,42.1536882}, -{7.75,42.09475466}, -{7.76,42.03678752}, -{7.77,41.97748641}, -{7.78,41.91859202}, -{7.79,41.86059036}, -{7.8,41.80169732}, -{7.81,41.74369731}, -{7.82,41.68573437}, -{7.83,41.62684333}, -{7.84,41.56884576}, -{7.85,41.51181369}, -{7.86,41.45381785}, -{7.87,41.39585845}, -{7.88,41.33919733}, -{7.89,41.28216814}, -{7.9,41.2241756}, -{7.91,41.16751658}, -{7.92,41.11141854}, -{7.93,41.05379713}, -{7.94,40.99770067}, -{7.95,40.94104445}, -{7.96,40.88494948}, -{7.97,40.82829464}, -{7.98,40.77256952}, -{7.99,40.7168111}, -{8,40.65979014}, -{8.01,40.6049964}, -{8.02,40.5492742}, -{8.03,40.49355288}, -{8.04,40.4378322}, -{8.05,40.38304144}, -{8.06,40.32769028}, -{8.07,40.27290099}, -{8.08,40.21811247}, -{8.09,40.16332474}, -{8.1,40.10890567}, -{8.11,40.05411945}, -{8.12,39.99933402}, -{8.13,39.94491715}, -{8.14,39.89106263}, -{8.15,39.83664725}, -{8.16,39.78279435}, -{8.17,39.7293099}, -{8.18,39.67582622}, -{8.19,39.62197577}, -{8.2,39.56852494}, -{8.21,39.51504348}, -{8.22,39.46156279}, -{8.23,39.40901245}, -{8.24,39.35553335}, -{8.25,39.30301525}, -{8.26,39.24953762}, -{8.27,39.19699041}, -{8.28,39.14444404}, -{8.29,39.0919286}, -{8.3,39.04068058}, -{8.31,38.98813658}, -{8.32,38.93562314}, -{8.33,38.88344769}, -{8.34,38.83183584}, -{8.35,38.78062111}, -{8.36,38.7290109}, -{8.37,38.67683857}, -{8.38,38.62525902}, -{8.39,38.57494795}, -{8.4,38.5237365}, -{8.41,38.47213045}, -{8.42,38.42089193}, -{8.43,38.37061247}, -{8.44,38.31937553}, -{8.45,38.26909742}, -{8.46,38.21879208}, -{8.47,38.16721889}, -{8.48,38.11821167}, -{8.49,38.06700627}, -{8.5,38.01766165}, -{8.51,37.9673604}, -{8.52,37.91708712}, -{8.53,37.86771773}, -{8.54,37.81744589}, -{8.55,37.76844436}, -{8.56,37.71910415}, -{8.57,37.67013066}, -{8.58,37.61983531}, -{8.59,37.57179349}, -{8.6,37.52245614}, -{8.61,37.47345951}, -{8.62,37.42412368}, -{8.63,37.37608468}, -{8.64,37.32711599}, -{8.65,37.27868733}, -{8.66,37.2297201}, -{8.67,37.18168396}, -{8.68,37.1336485}, -{8.69,37.08558897}, -{8.7,37.03755502}, -{8.71,36.98915633}, -{8.72,36.94241975}, -{8.73,36.89438792}, -{8.74,36.84635679}, -{8.75,36.79923301}, -{8.76,36.75120345}, -{8.77,36.70410523}, -{8.78,36.65700777}, -{8.79,36.61027618}, -{8.8,36.5631802}, -{8.81,36.51608498}, -{8.82,36.46935551}, -{8.83,36.42226178}, -{8.84,36.37646449}, -{8.85,36.32937229}, -{8.86,36.28357649}, -{8.87,36.23648583}, -{8.88,36.19069152}, -{8.89,36.14489797}, -{8.9,36.0987405}, -{8.91,36.05294848}, -{8.92,36.00715721}, -{8.93,35.96136669}, -{8.94,35.91557693}, -{8.95,35.86978793}, -{8.96,35.82493069}, -{8.97,35.77916419}, -{8.98,35.7343084}, -{8.99,35.68852237}, -{9,35.64366819}, -{9.01,35.5988148}, -{9.02,35.55396228}, -{9.03,35.50913068}, -{9.04,35.46427965}, -{9.05,35.41979346}, -{9.06,35.37587524}, -{9.07,35.33102668}, -{9.08,35.28749351}, -{9.09,35.24264645}, -{9.1,35.19909533}, -{9.11,35.15424994}, -{9.12,35.11071932}, -{9.13,35.0668068}, -{9.14,35.02325881}, -{9.15,34.9793665}, -{9.16,34.93582004}, -{9.17,34.8922744}, -{9.18,34.84838415}, -{9.19,34.8057715}, -{9.2,34.76222825}, -{9.21,34.71963493}, -{9.22,34.67609322}, -{9.23,34.63350121}, -{9.24,34.59089259}, -{9.25,34.54830191}, -{9.26,34.50476332}, -{9.27,34.46215719}, -{9.28,34.4205002}, -{9.29,34.37789572}, -{9.3,34.33530849}, -{9.31,34.29270565}, -{9.32,34.25141436}, -{9.33,34.20881316}, -{9.34,34.1671604}, -{9.35,34.12549266}, -{9.36,34.08327228}, -{9.37,34.0416216}, -{9.38,34.00031895}, -{9.39,33.95866972}, -{9.4,33.91736872}, -{9.41,33.87572094}, -{9.42,33.8344216}, -{9.43,33.79277528}, -{9.44,33.75242393}, -{9.45,33.71112702}, -{9.46,33.67041482}, -{9.47,33.62913357}, -{9.48,33.58877113}, -{9.49,33.54749128}, -{9.5,33.506782}, -{9.51,33.4664221}, -{9.52,33.42607642}, -{9.53,33.38349213}, -{9.54,33.34185389}, -{9.55,33.29963327}, -{9.56,33.2579839}, -{9.57,33.21541585}, -{9.58,33.17376836}, -{9.59,33.13248356}, -{9.6,33.09085011}, -{9.61,33.04920553}, -{9.62,33.0079235}, -{9.63,32.96629251}, -{9.64,32.92501229}, -{9.65,32.88337165}, -{9.66,32.84303684}, -{9.67,32.80139814}, -{9.68,32.76106491}, -{9.69,32.71978933}, -{9.7,32.67909651}, -{9.71,32.63875528}, -{9.72,32.59841507}, -{9.73,32.55808586}, -{9.74,32.51738663}, -{9.75,32.47705906}, -{9.76,32.43673222}, -{9.77,32.39639674}, -{9.78,32.35700414}, -{9.79,32.31667062}, -{9.8,32.27634704}, -{9.81,32.23694815}, -{9.82,32.19755893}, -{9.83,32.15722936}, -{9.84,32.11820228}, -{9.85,32.07881552}, -{9.86,32.03942161}, -{9.87,32.00003659}, -{9.88,31.96101262}, -{9.89,31.9216217}, -{9.9,31.88317208}, -{9.91,31.84415051}, -{9.92,31.80476263}, -{9.93,31.76667564}, -{9.94,31.72729651}, -{9.95,31.68920457}, -{9.96,31.65076021}, -{9.97,31.61267652}, -{9.98,31.57459362}, -{9.99,31.53521874}, -{10,31.49713169}, -{10.01,31.4599844}, -{10.02,31.42190484}, -{10.03,31.38346657}, -{10.04,31.34538864}, -{10.05,31.30823975}, -{10.06,31.27016359}, -{10.07,31.23302145}, -{10.08,31.19494693}, -{10.09,31.15780647}, -{10.1,31.12066686}, -{10.11,31.08352811}, -{10.12,31.04639022}, -{10.13,31.00924954}, -{10.14,30.97211354}, -{10.15,30.9353373}, -{10.16,30.89820299}, -{10.17,30.86106955}, -{10.18,30.82487043}, -{10.19,30.78809747}, -{10.2,30.75096665}, -{10.21,30.71477021}, -{10.22,30.67893324}, -{10.23,30.64180509}, -{10.24,30.60596983}, -{10.25,30.569777}, -{10.26,30.53394347}, -{10.27,30.49775249}, -{10.28,30.4619207}, -{10.29,30.42608978}, -{10.3,30.38990248}, -{10.31,30.35407315}, -{10.32,30.31917839}, -{10.33,30.2829928}, -{10.34,30.24809987}, -{10.35,30.2122741}, -{10.36,30.17738297}, -{10.37,30.14155897}, -{10.38,30.10631156}, -{10.39,30.07142306}, -{10.4,30.03653549}, -{10.41,30.00164884}, -{10.42,29.96676312}, -{10.43,29.93187719}, -{10.44,29.89699318}, -{10.45,29.86211011}, -{10.46,29.82758532}, -{10.47,29.79363808}, -{10.48,29.75875585}, -{10.49,29.72387638}, -{10.5,29.68993191}, -{10.51,29.65634308}, -{10.52,29.6214663}, -{10.53,29.5875246}, -{10.54,29.55358391}, -{10.55,29.519998}, -{10.56,29.48512494}, -{10.57,29.45154383}, -{10.58,29.41760332}, -{10.59,29.38402392}, -{10.6,29.35102307}, -{10.61,29.31744152}, -{10.62,29.28350828}, -{10.63,29.25086245}, -{10.64,29.21693108}, -{10.65,29.18335712}, -{10.66,29.15071358}, -{10.67,29.11678509}, -{10.68,29.08414305}, -{10.69,29.05021647}, -{10.7,29.01758148}, -{10.71,28.98400733}, -{10.72,28.95137415}, -{10.73,28.91837999}, -{10.74,28.88481418}, -{10.75,28.85218387}, -{10.76,28.81954804}, -{10.77,28.78691958}, -{10.78,28.75428528}, -{10.79,28.72130305}, -{10.8,28.68960496}, -{10.81,28.65698032}, -{10.82,28.62434918}, -{10.83,28.5926611}, -{10.84,28.56003155}, -{10.85,28.52741071}, -{10.86,28.49607273}, -{10.87,28.46438852}, -{10.88,28.43176212}, -{10.89,28.40007989}, -{10.9,28.36838991}, -{10.91,28.33670971}, -{10.92,28.30537634}, -{10.93,28.27368876}, -{10.94,28.24201145}, -{10.95,28.21068035}, -{10.96,28.17900504}, -{10.97,28.14732081}, -{10.98,28.115992}, -{10.99,28.0852546}, -{11,28.05392737}, -{11.01,28.02224636}, -{11.02,27.99186643}, -{11.03,27.96018712}, -{11.04,27.92979799}, -{11.05,27.89906669}, -{11.06,27.86774413}, -{11.07,27.8373574}, -{11.08,27.80662916}, -{11.09,27.77624411}, -{11.1,27.74585982}, -{11.11,27.71512233}, -{11.12,27.6847521}, -{11.13,27.65437027}, -{11.14,27.62398919}, -{11.15,27.59325515}, -{11.16,27.56382412}, -{11.17,27.53344555}, -{11.18,27.50306776}, -{11.19,27.47362606}, -{11.2,27.44324983}, -{11.21,27.41382376}, -{11.22,27.38438464}, -{11.23,27.35401093}, -{11.24,27.32457343}, -{11.25,27.29513675}, -{11.26,27.26570089}, -{11.27,27.23626586}, -{11.28,27.20719987}, -{11.29,27.17776661}, -{11.3,27.14833418}, -{11.31,27.1189026}, -{11.32,27.08947185}, -{11.33,27.06039469}, -{11.34,27.03096558}, -{11.35,27.00247292}, -{11.36,26.97304553}, -{11.37,26.94397151}, -{11.38,26.91548144}, -{11.39,26.88642605}, -{11.4,26.85793786}, -{11.41,26.82945057}, -{11.42,26.80038071}, -{11.43,26.77189515}, -{11.44,26.74376266}, -{11.45,26.71527887}, -{11.46,26.68714803}, -{11.47,26.65901801}, -{11.48,26.63053685}, -{11.49,26.6024085}, -{11.5,26.57391023}, -{11.51,26.54578339}, -{11.52,26.51765738}, -{11.53,26.4895322}, -{11.54,26.46199216}, -{11.55,26.43386869}, -{11.56,26.40574606}, -{11.57,26.37856025}, -{11.58,26.35008791}, -{11.59,26.32290389}, -{11.6,26.29572074}, -{11.61,26.2676024}, -{11.62,26.24040005}, -{11.63,26.21321936}, -{11.64,26.18510344}, -{11.65,26.1579245}, -{11.66,26.13074645}, -{11.67,26.1035693}, -{11.68,26.07639304}, -{11.69,26.04919555}, -{11.7,26.02202092}, -{11.71,25.99578343}, -{11.72,25.96861063}, -{11.73,25.94143874}, -{11.74,25.91459531}, -{11.75,25.8883613}, -{11.76,25.86119195}, -{11.77,25.83402352}, -{11.78,25.80779235}, -{11.79,25.78095219}, -{11.8,25.75472269}, -{11.81,25.72849417}, -{11.82,25.70168028}, -{11.83,25.67542909}, -{11.84,25.64920322}, -{11.85,25.62332824}, -{11.86,25.59614278}, -{11.87,25.5702694}, -{11.88,25.54404713}, -{11.89,25.51780034}, -{11.9,25.49192951}, -{11.91,25.46570998}, -{11.92,25.4407515}, -{11.93,25.41488322}, -{11.94,25.38866641}, -{11.95,25.36277341}, -{11.96,25.33655837}, -{11.97,25.30721209}, -{11.98,25.27786728}, -{11.99,25.24849653}, -{12,25.21915441}, -{12.01,25.1898137}, -{12.02,25.16047458}, -{12.03,25.13204538}, -{12.04,25.10270896}, -{12.05,25.07337403}, -{12.06,25.04494849}, -{12.07,25.01561634}, -{12.08,24.9862857}, -{12.09,24.95821224}, -{12.1,24.92982124}, -{12.11,24.90049487}, -{12.12,24.87207673}, -{12.13,24.84403826}, -{12.14,24.81562252}, -{12.15,24.78723881}, -{12.16,24.75917343}, -{12.17,24.73079259}, -{12.18,24.70241334}, -{12.19,24.67435154}, -{12.2,24.64597519}, -{12.21,24.61885284}, -{12.22,24.59047942}, -{12.23,24.56335946}, -{12.24,24.53533627}, -{12.25,24.50787145}, -{12.26,24.47981777}, -{12.27,24.4527359}, -{12.28,24.42527482}, -{12.29,24.39725858}, -{12.3,24.37014694}, -{12.31,24.34307082}, -{12.32,24.3156149}, -{12.33,24.28850687}, -{12.34,24.26143502}, -{12.35,24.23432943}, -{12.36,24.20726048}, -{12.37,24.18109478}, -{12.38,24.15399282}, -{12.39,24.12692815}, -{12.4,24.10076615}, -{12.41,24.07366783}, -{12.42,24.04754501}, -{12.43,24.02044923}, -{12.44,23.99429215}, -{12.45,23.96817377}, -{12.46,23.94108172}, -{12.47,23.91492845}, -{12.48,23.88912194}, -{12.49,23.86300939}, -{12.5,23.8368599}, -{12.51,23.81071164}, -{12.52,23.78490986}, -{12.53,23.75876398}, -{12.54,23.73265881}, -{12.55,23.7077984}, -{12.56,23.6816564}, -{12.57,23.65551566}, -{12.58,23.62972097}, -{12.59,23.60452048}, -{12.6,23.57876888}, -{12.61,23.55357111}, -{12.62,23.52871915}, -{12.63,23.50258603}, -{12.64,23.47773645}, -{12.65,23.45288805}, -{12.66,23.42675865}, -{12.67,23.40191266}, -{12.68,23.37706784}, -{12.69,23.3518802}, -{12.7,23.32708081}, -{12.71,23.29967609}, -{12.72,23.27266003}, -{12.73,23.24564574}, -{12.74,23.21863323}, -{12.75,23.19251634}, -{12.76,23.16516372}, -{12.77,23.13909454}, -{12.78,23.112044}, -{12.79,23.08597821}, -{12.8,23.05897606}, -{12.81,23.03286817}, -{12.82,23.00680764}, -{12.83,22.97976445}, -{12.84,22.95370737}, -{12.85,22.92799483}, -{12.86,22.90189438}, -{12.87,22.87584259}, -{12.88,22.84974514}, -{12.89,22.82369687}, -{12.9,22.79854085}, -{12.91,22.77283829}, -{12.92,22.74674687}, -{12.93,22.72164409}, -{12.94,22.69589765}, -{12.95,22.67079842}, -{12.96,22.64471311}, -{12.97,22.6199592}, -{12.98,22.59387694}, -{12.99,22.56907614}, -{13,22.54398583}, -{13.01,22.519188}, -{13.02,22.49410134}, -{13.03,22.46836788}, -{13.04,22.44417194}, -{13.05,22.41943166}, -{13.06,22.39464117}, -{13.07,22.36951124}, -{13.08,22.3447761}, -{13.09,22.31999001}, -{13.1,22.29580348}, -{13.11,22.27107355}, -{13.12,22.24629192}, -{13.13,22.22245045}, -{13.14,22.19772569}, -{13.15,22.1738873}, -{13.16,22.14971031}, -{13.17,22.12493602}, -{13.18,22.1011571}, -{13.19,22.07732477}, -{13.2,22.0534939}, -{13.21,22.02966448}, -{13.22,22.00589262}, -{13.23,21.98206637}, -{13.24,21.9582416}, -{13.25,21.9344183}, -{13.26,21.91093565}, -{13.27,21.88711523}, -{13.28,21.86429292}, -{13.29,21.84047575}, -{13.3,21.81666008}, -{13.31,21.79412373}, -{13.32,21.770311}, -{13.33,21.74743888}, -{13.34,21.72362921}, -{13.35,21.70109859}, -{13.36,21.67823104}, -{13.37,21.6544259}, -{13.38,21.63195974}, -{13.39,21.60909704}, -{13.4,21.58657391}, -{13.41,21.56371429}, -{13.42,21.54119407}, -{13.43,21.51833755}, -{13.44,21.49582027}, -{13.45,21.47296687}, -{13.46,21.45045254}, -{13.47,21.42760227}, -{13.48,21.40509091}, -{13.49,21.38352034}, -{13.5,21.36067478}, -{13.51,21.33810451}, -{13.52,21.31532552}, -{13.53,21.29060603}, -{13.54,21.26689173}, -{13.55,21.24311512}, -{13.56,21.22034434}, -{13.57,21.19657128}, -{13.58,21.17286521}, -{13.59,21.1490957}, -{13.6,21.12539377}, -{13.61,21.10256736}, -{13.62,21.07886961}, -{13.63,21.05510731}, -{13.64,21.03228625}, -{13.65,21.00859467}, -{13.66,20.98611288}, -{13.67,20.96329714}, -{13.68,20.94055131}, -{13.69,20.91679968}, -{13.7,20.89432461}, -{13.71,20.87158501}, -{13.72,20.84877843}, -{13.73,20.82630851}, -{13.74,20.80357522}, -{13.75,20.78110883}, -{13.76,20.7583094}, -{13.77,20.73551176}, -{13.78,20.71312132}, -{13.79,20.69160153}, -{13.8,20.66880938}, -{13.81,20.64635316}, -{13.82,20.62457646}, -{13.83,20.60212387}, -{13.84,20.58027891}, -{13.85,20.5578297}, -{13.86,20.53632199}, -{13.87,20.51455595}, -{13.88,20.49305197}, -{13.89,20.47060979}, -{13.9,20.44910917}, -{13.91,20.42727711}, -{13.92,20.40577999}, -{13.93,20.38428458}, -{13.94,20.36279089}, -{13.95,20.34137471}, -{13.96,20.31988474}, -{13.97,20.29839651}, -{13.98,20.27785001}, -{13.99,20.25636527}, -{14,20.23488228}, -{14.01,20.2134010}, -{14.02,20.19286161}, -{14.03,20.17138391}, -{14.04,20.15084805}, -{14.05,20.12937391}, -{14.06,20.10884164}, -{14.07,20.08831118}, -{14.08,20.06684243}, -{14.09,20.0463156}, -{14.1,20.02612189}, -{14.11,20.00559862}, -{14.12,19.98413703}, -{14.13,19.96361743}, -{14.14,19.94334949}, -{14.15,19.92283321}, -{14.16,19.90231878}, -{14.17,19.88213686}, -{14.18,19.86256627}, -{14.19,19.84205738}, -{14.2,19.82188069}, -{14.21,19.80137546}, -{14.22,19.78205917}, -{14.23,19.76155735}, -{14.24,19.74138739}, -{14.25,19.72182956}, -{14.26,19.70166313}, -{14.27,19.68210906}, -{14.28,19.66280148}, -{14.29,19.64231061}, -{14.3,19.62309135}, -{14.31,19.60387385}, -{14.32,19.5833025}, -{14.33,19.56408835}, -{14.34,19.54487598}, -{14.35,19.52533651}, -{14.36,19.50604058}, -{14.37,19.48683338}, -{14.38,19.4672994}, -{14.39,19.4480959}, -{14.4,19.42880586}, -{14.41,19.40960569}, -{14.42,19.39040734}, -{14.43,19.36994231}, -{14.44,19.35041974}, -{14.45,19.33028662}, -{14.46,19.31076803}, -{14.47,19.29157915}, -{14.48,19.2711241}, -{14.49,19.25184804}, -{14.5,19.23266453}, -{14.51,19.21221515}, -{14.52,19.19303548}, -{14.53,19.1738577}, -{14.54,19.15435486}, -{14.55,19.13518095}, -{14.56,19.11600895}, -{14.57,19.09651222}, -{14.58,19.07725048}, -{14.59,19.05808401}, -{14.6,19.03986}, -{14.61,19.020371}, -{14.62,19.00121059}, -{14.63,18.98195683}, -{14.64,18.9637404}, -{14.65,18.94458535}, -{14.66,18.92543225}, -{14.67,18.90722172}, -{14.68,18.88797596}, -{14.69,18.86976908}, -{14.7,18.85156418}, -{14.71,18.83242066}, -{14.72,18.81412198}, -{14.73,18.79592278}, -{14.74,18.77678495}, -{14.75,18.75849112}, -{14.76,18.74029765}, -{14.77,18.72210621}, -{14.78,18.70391681}, -{14.79,18.68562961}, -{14.8,18.66744399}, -{14.81,18.64958453}, -{14.82,18.63130218}, -{14.83,18.61312231}, -{14.84,18.59484321}, -{14.85,18.57760787}, -{14.86,18.55975749}, -{14.87,18.54148333}, -{14.88,18.52331312}, -{14.89,18.506409}, -{14.9,18.48813982}, -{14.91,18.47091622}, -{14.92,18.45297334}, -{14.93,18.43575355}, -{14.94,18.41749095}, -{14.95,18.40059776}, -{14.96,18.38233843}, -{14.97,18.36544889}, -{14.98,18.34823896}, -{14.99,18.33030698}, -{15,18.31310094}, -{15.01,18.2961128}, -{15.02,18.27891069}, -{15.03,18.26192569}, -{15.04,18.24504922}, -{15.05,18.22774566}, -{15.06,18.21076522}, -{15.07,18.19357277}, -{15.08,18.1765955}, -{15.09,18.15972822}, -{15.1,18.14243297}, -{15.11,18.12556948}, -{15.12,18.10953931}, -{15.13,18.0925698}, -{15.14,18.07539106}, -{15.15,18.05936558}, -{15.16,18.04251136}, -{15.17,18.02554826}, -{15.18,18.0095274}, -{15.19,17.99235852}, -{15.2,17.97634095}, -{15.21,17.95938407}, -{15.22,17.94348177}, -{15.23,17.9274689}, -{15.24,17.91062964}, -{15.25,17.89462}, -{15.26,17.8786118}, -{15.27,17.86166442}, -{15.28,17.84577334}, -{15.29,17.82976996}, -{15.3,17.8137681}, -{15.31,17.79788268}, -{15.32,17.78188412}, -{15.33,17.76588709}, -{15.34,17.75000738}, -{15.35,17.73401367}, -{15.36,17.7180215}, -{15.37,17.70203087}, -{15.38,17.68615874}, -{15.39,17.67048955}, -{15.4,17.65544451}, -{15.41,17.63946025}, -{15.42,17.62347755}, -{15.43,17.6076148}, -{15.44,17.59289385}, -{15.45,17.57691603}, -{15.46,17.56093978}, -{15.47,17.5459059}, -{15.48,17.53036979}, -{15.49,17.51533925}, -{15.5,17.49936953}, -{15.51,17.48465902}, -{15.52,17.46963317}, -{15.53,17.45366815}, -{15.54,17.43896204}, -{15.55,17.42312196}, -{15.56,17.40810274}, -{15.57,17.39340133}, -{15.58,17.37838525}, -{15.59,17.36368679}, -{15.6,17.34773311}, -{15.61,17.33303762}, -{15.62,17.31802786}, -{15.63,17.30333534}, -{15.64,17.28832878}, -{15.65,17.27363925}, -{15.66,17.25876085}, -{15.67,17.24407461}, -{15.68,17.22907476}, -{15.69,17.21533227}, -{15.7,17.20065051}, -{15.71,17.18565547}, -{15.72,17.17097676}, -{15.73,17.15629954}, -{15.74,17.14225006}, -{15.75,17.12757589}, -{15.76,17.11290323}, -{15.77,17.09885862}, -{15.78,17.08418904}, -{15.79,17.06952097}, -{15.8,17.05548126}, -{15.81,17.0408163}, -{15.82,17.02709353}, -{15.83,17.01337227}, -{15.84,16.99858182}, -{15.85,16.98455001}, -{15.86,16.96989246}, -{15.87,16.9561771}, -{15.88,16.94246328}, -{15.89,16.92781034}, -{15.9,16.91409959}, -{15.91,16.90039038}, -{15.92,16.88637013}, -{15.93,16.8726641}, -{15.94,16.85895964}, -{15.95,16.8443161}, -{15.96,16.83048134}, -{15.97,16.81678126}, -{15.98,16.80308275}, -{15.99,16.78938581}, -{16,16.77569045}, -{16.01,16.7619966}, -{16.02,16.74924503}, -{16.03,16.73541903}, -{16.04,16.7217297}, -{16.05,16.70804196}, -{16.06,16.69435581}, -{16.07,16.68098232}, -{16.08,16.66810304}, -{16.09,16.6544213}, -{16.1,16.64074118}, -{16.11,16.62800319}, -{16.12,16.61432629}, -{16.13,16.60051285}, -{16.14,16.58808973}, -{16.15,16.5744173}, -{16.16,16.56168699}, -{16.17,16.54787856}, -{16.18,16.53515122}, -{16.19,16.52179485}, -{16.2,16.50907066}, -{16.21,16.49620775}, -{16.22,16.48285561}, -{16.23,16.47013594}, -{16.24,16.45741793}, -{16.25,16.44361967}, -{16.26,16.43121377}, -{16.27,16.41850033}, -{16.28,16.40564625}, -{16.29,16.39230423}, -{16.3,16.37959539}, -{16.31,16.3671968}, -{16.32,16.35434777}, -{16.33,16.34164355}, -{16.34,16.32924928}, -{16.35,16.31640408}, -{16.36,16.30401257}, -{16.37,16.29131459}, -{16.38,16.2787811}, -{16.39,16.2660861}, -{16.4,16.25355487}, -{16.41,16.24086288}, -{16.42,16.22848006}, -{16.43,16.21658521}, -{16.44,16.20420519}, -{16.45,16.19151954}, -{16.46,16.1789954}, -{16.47,16.16725301}, -{16.48,16.15473116}, -{16.49,16.14235835}, -{16.5,16.12968041}, -{16.51,16.11810231}, -{16.52,16.10573389}, -{16.53,16.09385165}, -{16.54,16.08148609}, -{16.55,16.0696064}, -{16.56,16.05724372}, -{16.57,16.04582273}, -{16.58,16.03331263}, -{16.59,16.02158869}, -{16.6,16.00908095}, -{16.61,15.99766564}, -{16.62,15.98610027}, -{16.63,15.97453592}, -{16.64,15.96391265}, -{16.65,15.95250262}, -{16.66,15.94094157}, -{16.67,15.92938153}, -{16.68,15.91797559}, -{16.69,15.90641786}, -{16.7,15.89580114}, -{16.71,15.8843993}, -{16.72,15.87284489}, -{16.73,15.86129153}, -{16.74,15.85067917}, -{16.75,15.83928272}, -{16.76,15.82803683}, -{16.77,15.8174277}, -{16.78,15.8058797}, -{16.79,15.79542848}, -{16.8,15.78388282}, -{16.81,15.7732781}, -{16.82,15.7620381}, -{16.83,15.75159221}, -{16.84,15.74005092}, -{16.85,15.72945053}, -{16.86,15.71791138}, -{16.87,15.70777403}, -{16.88,15.69623716}, -{16.89,15.68564114}, -{16.9,15.67504619}, -{16.91,15.66381524}, -{16.92,15.65338142}, -{16.93,15.64278981}, -{16.94,15.63219927}, -{16.95,15.62097241}, -{16.96,15.61038392}, -{16.97,15.59995686}, -{16.98,15.5896728}, -{16.99,15.57908768}, -{17,15.567564}, -{17.01,15.55728}, -{17.02,15.5467008}, -{17.03,15.53628186}, -{17.04,15.5260038}, -{17.05,15.51542526}, -{17.06,15.50484782}, -{17.07,15.49457266}, -{17.08,15.48399732}, -{17.09,15.47372408}, -{17.1,15.46331442}, -{17.11,15.45304336}, -{17.12,15.44247247}, -{17.13,15.43220334}, -{17.14,15.42163458}, -{17.15,15.41200639}, -{17.16,15.40174019}, -{17.17,15.39117468}, -{17.18,15.381076}, -{17.19,15.37051287}, -{17.2,15.36119022}, -{17.21,15.35062924}, -{17.22,15.34036916}, -{17.23,15.33011005}, -{17.24,15.32049157}, -{17.25,15.31023442}, -{17.26,15.29967879}, -{17.27,15.29036291}, -{17.28,15.27980946}, -{17.29,15.27049554}, -{17.3,15.26024331}, -{17.31,15.24969308}, -{17.32,15.24055107}, -{17.33,15.23000328}, -{17.34,15.2206945}, -{17.35,15.2104475}, -{17.36,15.20084211}, -{17.37,15.19059712}, -{17.38,15.18129224}, -{17.39,15.17169008}, -{17.4,15.16144808}, -{17.41,15.15214615}, -{17.42,15.14160817}, -{17.43,15.13230825}, -{17.44,15.12300929}, -{17.45,15.11277228}, -{17.46,15.10317768}, -{17.47,15.09388171}, -{17.48,15.0845867}, -{17.49,15.07435372}, -{17.5,15.06476346}, -{17.51,15.05547146}, -{17.52,15.04618045}, -{17.53,15.0368904}, -{17.54,15.02636563}, -{17.55,15.01707765}, -{17.56,15.00779064}, -{17.57,14.99850463}, -{17.58,14.98921959}, -{17.59,14.97976001}, -{17.6,14.97018043}, -{17.61,14.9608982}, -{17.62,14.95161697}, -{17.63,14.94233673}, -{17.64,14.93305749}, -{17.65,14.92377925}, -{17.66,14.914502}, -{17.67,14.90522576}, -{17.68,14.89565497}, -{17.69,14.88638082}, -{17.7,14.87710768}, -{17.71,14.86783555}, -{17.72,14.85950302}, -{17.73,14.85005401}, -{17.74,14.84078465}, -{17.75,14.83151631}, -{17.76,14.82224899}, -{17.77,14.81392119}, -{17.78,14.80465589}, -{17.79,14.79539161}, -{17.8,14.78612836}, -{17.81,14.77780457}, -{17.82,14.76836233}, -{17.83,14.7591019}, -{17.84,14.75078089}, -{17.85,14.74152251}, -{17.86,14.73226517}, -{17.87,14.72394719}, -{17.88,14.71469191}, -{17.89,14.70619329}, -{17.9,14.69693982}, -{17.91,14.68768739}, -{17.92,14.67937427}, -{17.93,14.67041721}, -{17.94,14.66192218}, -{17.95,14.65361177}, -{17.96,14.64559541}, -{17.97,14.63710245}, -{17.98,14.62879476}, -{17.99,14.62078086}, -{18,14.61228996}, -{18.01,14.603985}, -{18.02,14.59578802}, -{18.03,14.58748477}, -{18.04,14.57918256}, -{18.05,14.57098741}, -{18.06,14.56268691}, -{18.07,14.55449291}, -{18.08,14.54619414}, -{18.09,14.53789641}, -{18.1,14.52970426}, -{18.11,14.52234619}, -{18.12,14.51415519}, -{18.13,14.50586092}, -{18.14,14.49785922}, -{18.15,14.48937856}, -{18.16,14.48108708}, -{18.17,14.47383695}, -{18.18,14.46554721}, -{18.19,14.45736041}, -{18.2,14.44907243}, -{18.21,14.44107645}, -{18.22,14.43353834}, -{18.23,14.42554394}, -{18.24,14.41706943}, -{18.25,14.41001433}, -{18.26,14.40154117}, -{18.27,14.39354998}, -{18.28,14.38601581}, -{18.29,14.37802622}, -{18.3,14.36974732}, -{18.31,14.36250539}, -{18.32,14.35451831}, -{18.33,14.34698749}, -{18.34,14.33900202}, -{18.35,14.33053502}, -{18.36,14.32348868}, -{18.37,14.31502305}, -{18.38,14.3079783}, -{18.39,14.29980353}, -{18.4,14.29247089}, -{18.41,14.28429732}, -{18.42,14.27696645}, -{18.43,14.2687941}, -{18.44,14.26175406}, -{18.45,14.25329386}, -{18.46,14.24625544}, -{18.47,14.23902275}, -{18.48,14.23075982}, -{18.49,14.22352832}, -{18.5,14.21555581}, -{18.51,14.20803692}, -{18.52,14.20100324}, -{18.53,14.19283689}, -{18.54,14.18551649}, -{18.55,14.17828851}, -{18.56,14.17032089}, -{18.57,14.16280601}, -{18.58,14.15577712}, -{18.59,14.14855148}, -{18.6,14.14029932}, -{18.61,14.1330749}, -{18.62,14.12585095}, -{18.63,14.11882591}, -{18.64,14.11131564}, -{18.65,14.10335527}, -{18.66,14.09613369}, -{18.67,14.08911186}, -{18.68,14.08160428}, -{18.69,14.07458408}, -{18.7,14.06736486}, -{18.71,14.05940941}, -{18.72,14.05190453}, -{18.73,14.04468704}, -{18.74,14.03767075}, -{18.75,14.03045441}, -{18.76,14.0234397}, -{18.77,14.01593811}, -{18.78,14.00892505}, -{18.79,14.00171111}, -{18.8,13.99449767}, -{18.81,13.98748687}, -{18.82,13.97998859}, -{18.83,13.97297946}, -{18.84,13.96576844}, -{18.85,13.95855791}, -{18.86,13.95155106}, -{18.87,13.94434172}, -{18.88,13.93733647}, -{18.89,13.92984286}, -{18.9,13.92263527}, -{18.91,13.91656883}, -{18.92,13.90936242}, -{18.93,13.90236113}, -{18.94,13.89515592}, -{18.95,13.88795122}, -{18.96,13.88095224}, -{18.97,13.87374874}, -{18.98,13.8671973}, -{18.99,13.86020068}, -{19,13.85299895}, -{19.01,13.8460039}, -{19.02,13.83973967}, -{19.03,13.83253964}, -{19.04,13.82554697}, -{19.05,13.81834816}, -{19.06,13.81208601}, -{19.07,13.80509565}, -{19.08,13.79789857}, -{19.09,13.79163808}, -{19.1,13.78465005}, -{19.11,13.7774547}, -{19.12,13.77140432}, -{19.13,13.76421018}, -{19.14,13.75701657}, -{19.15,13.75096847}, -{19.16,13.74377608}, -{19.17,13.73658422}, -{19.18,13.73053841}, -{19.19,13.72334777}, -{19.2,13.7170935}, -{19.21,13.71011415}, -{19.22,13.70386106}, -{19.23,13.6966727}, -{19.24,13.69042063}, -{19.25,13.68344434}, -{19.26,13.67719346}, -{19.27,13.67000739}, -{19.28,13.66396915}, -{19.29,13.65678432}, -{19.3,13.65053567}, -{19.31,13.64356412}, -{19.32,13.63759873}, -{19.33,13.63041613}, -{19.34,13.62438244}, -{19.35,13.61813662}, -{19.36,13.61095581}, -{19.37,13.60471102}, -{19.38,13.59774489}, -{19.39,13.59150132}, -{19.4,13.58525826}, -{19.41,13.57857588}, -{19.42,13.57233396}, -{19.43,13.56609255}, -{19.44,13.55891633}, -{19.45,13.55289095}, -{19.46,13.54665128}, -{19.47,13.54069304}, -{19.48,13.53445433}, -{19.49,13.52821614}, -{19.5,13.52225917}, -{19.51,13.51623801}, -{19.52,13.51000148}, -{19.53,13.50404595}, -{19.54,13.49781038}, -{19.55,13.49157533}, -{19.56,13.48534082}, -{19.57,13.479387}, -{19.58,13.47315345}, -{19.59,13.46713797}, -{19.6,13.46118561}, -{19.61,13.45495373}, -{19.62,13.44900223}, -{19.63,13.44277133}, -{19.64,13.43654096}, -{19.65,13.43059075}, -{19.66,13.42436136}, -{19.67,13.41906737}, -{19.68,13.41311843}, -{19.69,13.40710987}, -{19.7,13.40088269}, -{19.71,13.39493524}, -{19.72,13.38870905}, -{19.73,13.38369717}, -{19.74,13.37747196}, -{19.75,13.37124729}, -{19.76,13.36530198}, -{19.77,13.3590783}, -{19.78,13.35406848}, -{19.79,13.3480669}, -{19.8,13.34184492}, -{19.81,13.33590196}, -{19.82,13.32968098}, -{19.83,13.32467339}, -{19.84,13.3184534}, -{19.85,13.31251216}, -{19.86,13.30722762}, -{19.87,13.30128723}, -{19.88,13.29506924}, -{19.89,13.28978619}, -{19.9,13.28384711}, -{19.91,13.27763066}, -{19.92,13.27262679}, -{19.93,13.26641135}, -{19.94,13.26069779}, -{19.95,13.2554178}, -{19.96,13.2494815}, -{19.97,13.24326825}, -{19.98,13.23826703}, -{19.99,13.23205479}, -{20,13.2270544}, -{20.01,13.220843}, -{20.02,13.2149095}, -{20.03,13.20963338}, -{20.04,13.20370058}, -{20.05,13.19842544}, -{20.06,13.19249351}, -{20.07,13.18721934}, -{20.08,13.18128829}, -{20.09,13.1760151}, -{20.1,13.17008493}, -{20.11,13.1650891}, -{20.12,13.15888343}, -{20.13,13.15388844}, -{20.14,13.14768379}, -{20.15,13.14268964}, -{20.16,13.13648602}, -{20.17,13.13149272}, -{20.18,13.12529013}, -{20.19,13.12029767}, -{20.2,13.11437189}, -{20.21,13.10910451}, -{20.22,13.10411329}, -{20.23,13.09791323}, -{20.24,13.09292286}, -{20.25,13.08699927}, -{20.26,13.08173433}, -{20.27,13.0767452}, -{20.28,13.07054769}, -{20.29,13.06555942}, -{20.3,13.06057154}, -{20.31,13.05437555}, -{20.32,13.04938853}, -{20.33,13.04412703}, -{20.34,13.03820743}, -{20.35,13.03322167}, -{20.36,13.02796162}, -{20.37,13.02204335}, -{20.38,13.01705885}, -{20.39,13.01180026}, -{20.4,13.00588332}, -{20.41,13.00090008}, -{20.42,12.99564296}, -{20.43,12.99066057}, -{20.44,12.9847454}, -{20.45,12.97948974}, -{20.46,12.97450862}, -{20.47,12.9695279}, -{20.48,12.96427372}, -{20.49,12.95836078}, -{20.5,12.95338133}, -{20.51,12.94812863}, -{20.52,12.94315004}, -{20.53,12.93817186}, -{20.54,12.93292063}, -{20.55,12.92701037}, -{20.56,12.92203347}, -{20.57,12.91678373}, -{20.58,12.9118077}, -{20.59,12.90683207}, -{20.6,12.90185684}, -{20.61,12.89660905}, -{20.62,12.89046644}, -{20.63,12.88549235}, -{20.64,12.88051866}, -{20.65,12.87527266}, -{20.66,12.87029984}, -{20.67,12.86532743}, -{20.68,12.86008293}, -{20.69,12.8551114}, -{20.7,12.85014027}, -{20.71,12.84516956}, -{20.72,12.83992702}, -{20.73,12.83495719}, -{20.74,12.82998776}, -{20.75,12.82501875}, -{20.76,12.82005016}, -{20.77,12.8145721}, -{20.78,12.80960421}, -{20.79,12.80463674}, -{20.8,12.79966968}, -{20.81,12.79443141}, -{20.82,12.78946524}, -{20.83,12.78449948}, -{20.84,12.77953415}, -{20.85,12.77456923}, -{20.86,12.76933342}, -{20.87,12.76436939}, -{20.88,12.75940579}, -{20.89,12.75420264}, -{20.9,12.75017184}, -{20.91,12.7452093}, -{20.92,12.73997626}, -{20.93,12.73501461}, -{20.94,12.73005338}, -{20.95,12.72509257}, -{20.96,12.72013219}, -{20.97,12.71517224}, -{20.98,12.71063271}, -{20.99,12.70567346}, -{21,12.70071463}, -{21.01,12.69575622}, -{21.02,12.6907982}, -{21.03,12.68677252}, -{21.04,12.68181537}, -{21.05,12.67658858}, -{21.06,12.67163233}, -{21.07,12.66643363}, -{21.08,12.6624098}, -{21.09,12.65745464}, -{21.1,12.65249991}, -{21.11,12.64754561}, -{21.12,12.64352336}, -{21.13,12.63830036}, -{21.14,12.63334738}, -{21.15,12.62815066}, -{21.16,12.62412991}, -{21.17,12.61917802}, -{21.18,12.61422657}, -{21.19,12.61020702}, -{21.2,12.6052564}, -{21.21,12.60030623}, -{21.22,12.59604259}, -{21.23,12.59109308}, -{21.24,12.58707536}, -{21.25,12.58188098}, -{21.26,12.57786389}, -{21.27,12.57384718}, -{21.28,12.56865347}, -{21.29,12.56490597}, -{21.3,12.56064385}, -{21.31,12.55569711}, -{21.32,12.551682}, -{21.33,12.54742049}, -{21.34,12.54247484}, -{21.35,12.53846075}, -{21.36,12.53446804}, -{21.37,12.5304545}, -{21.38,12.52551029}, -{21.39,12.52124994}, -{21.4,12.51723743}, -{21.41,12.51204644}, -{21.42,12.50830244}, -{21.43,12.5042909}, -{21.44,12.50003148}, -{21.45,12.49508964}, -{21.46,12.49107914}, -{21.47,12.48682033}, -{21.48,12.48307799}, -{21.49,12.47788864}, -{21.5,12.47387935}, -{21.51,12.46987045}, -{21.52,12.46561258}, -{21.53,12.46160431}, -{21.54,12.45668327}, -{21.55,12.45267558}, -{21.56,12.44866828}, -{21.57,12.44441136}, -{21.58,12.4404047}, -{21.59,12.43548443}, -{21.6,12.43147835}, -{21.61,12.42747267}, -{21.62,12.42321669}, -{21.63,12.41947837}, -{21.64,12.41522271}, -{21.65,12.41121823}, -{21.66,12.40628365}, -{21.67,12.40202862}, -{21.68,12.39829166}, -{21.69,12.39403695}, -{21.7,12.3900341}, -{21.71,12.38603165}, -{21.72,12.38204381}, -{21.73,12.37804193}, -{21.74,12.37285783}, -{21.75,12.36885662}, -{21.76,12.36512184}, -{21.77,12.36086866}, -{21.78,12.35686841}, -{21.79,12.35261563}, -{21.8,12.34888185}, -{21.81,12.34488258}, -{21.82,12.34063037}, -{21.83,12.33663175}, -{21.84,12.33264554}, -{21.85,12.32864749}, -{21.86,12.32439601}, -{21.87,12.32039861}, -{21.88,12.316667}, -{21.89,12.31241609}, -{21.9,12.30841968}, -{21.91,12.30416917}, -{21.92,12.30043857}, -{21.93,12.29618839}, -{21.94,12.29219321}, -{21.95,12.28846345}, -{21.96,12.28421385}, -{21.97,12.28021966}, -{21.98,12.27623533}, -{21.99,12.27224173}, -{22,12.26799287}, -{22.01,12.2639999}, -{22.02,12.26001611}, -{22.03,12.25602374}, -{22.04,12.25203179}, -{22.05,12.24804842}, -{22.06,12.24405705}, -{22.07,12.23980952}, -{22.08,12.23608316}, -{22.09,12.23183596}, -{22.1,12.22784584}, -{22.11,12.22386326}, -{22.12,12.22080329}, -{22.13,12.21681415}, -{22.14,12.21283198}, -{22.15,12.20884343}, -{22.16,12.20459758}, -{22.17,12.20087358}, -{22.18,12.19662806}, -{22.19,12.19264077}, -{22.2,12.1886594}, -{22.21,12.18560208}, -{22.22,12.18135731}, -{22.23,12.17763483}, -{22.24,12.17364912}, -{22.25,12.16940496}, -{22.26,12.16568333}, -{22.27,12.16236877}, -{22.28,12.15838431}, -{22.29,12.15440414}, -{22.3,12.15042028}, -{22.31,12.14644038}, -{22.32,12.14245712}, -{22.33,12.13914354}, -{22.34,12.13542393}, -{22.35,12.13118156}, -{22.36,12.12719955}, -{22.37,12.12322045}, -{22.38,12.12016807}, -{22.39,12.11644981}, -{22.4,12.11220839}, -{22.41,12.10822799}, -{22.42,12.10517852}, -{22.43,12.1011987}, -{22.44,12.09722055}, -{22.45,12.09324134}, -{22.46,12.0899299}, -{22.47,12.08621368}, -{22.48,12.08197372}, -{22.49,12.07825803}, -{22.5,12.0749472}, -{22.51,12.07096985}, -{22.52,12.06699275}, -{22.53,12.063016}, -{22.54,12.05996787}, -{22.55,12.05599171}, -{22.56,12.05175323}, -{22.57,12.04896821}, -{22.58,12.04473009}, -{22.59,12.04101694}, -{22.6,12.03770776}, -{22.61,12.0339951}, -{22.62,12.02975769}, -{22.63,12.02671252}, -{22.64,12.022737}, -{22.65,12.01876391}, -{22.66,12.01571712}, -{22.67,12.01174463}, -{22.68,12.00776965}, -{22.69,12.00472615}, -{22.7,12.00075143}, -{22.71,11.99678015}, -{22.72,11.99373403}, -{22.73,11.98976334}, -{22.74,11.98552815}, -{22.75,11.98274735}, -{22.76,11.97851251}, -{22.77,11.97573217}, -{22.78,11.97149769}, -{22.79,11.96778965}, -{22.8,11.96448369}, -{22.81,11.96077615}, -{22.82,11.95747051}, -{22.83,11.95376347}, -{22.84,11.94953007}, -{22.85,11.94675161}, -{22.86,11.94251858}, -{22.87,11.93947411}, -{22.88,11.93550791}, -{22.89,11.93246366}, -{22.9,11.92849806}, -{22.91,11.92545404}, -{22.92,11.92148904}, -{22.93,11.91751736}, -{22.94,11.91448085}, -{22.95,11.91050943}, -{22.96,11.90747349}, -{22.97,11.90350234}, -{22.98,11.90046697}, -{22.99,11.89649608}, -{23,11.89346127}, -{23.01,11.8894906}, -{23.02,11.88645641}, -{23.03,11.88248606}, -{23.04,11.87918389}, -{23.05,11.87548231}, -{23.06,11.87218047}, -{23.07,11.86847939}, -{23.08,11.86517787}, -{23.09,11.86147731}, -{23.1,11.85817612}, -{23.11,11.85540358}, -{23.12,11.85117522}, -{23.13,11.84840314}, -{23.14,11.84417515}, -{23.15,11.84113377}, -{23.16,11.83717593}, -{23.17,11.83413478}, -{23.18,11.83043642}, -{23.19,11.82713664}, -{23.2,11.82436611}, -{23.21,11.82013934}, -{23.22,11.81736929}, -{23.23,11.81314289}, -{23.24,11.8101025}, -{23.25,11.8061473}, -{23.26,11.80310713}, -{23.27,11.80033817}, -{23.28,11.79611262}, -{23.29,11.79334414}, -{23.3,11.78911897}, -{23.31,11.78635096}, -{23.32,11.78305325}, -{23.33,11.77908676}, -{23.34,11.77631937}, -{23.35,11.77302208}, -{23.36,11.76932815}, -{23.37,11.76603119}, -{23.38,11.76326473}, -{23.39,11.75904116}, -{23.4,11.75600251}, -{23.41,11.75230977}, -{23.42,11.74901358}, -{23.43,11.74624821}, -{23.44,11.74202551}, -{23.45,11.73898739}, -{23.46,11.73622265}, -{23.47,11.73292721}, -{23.48,11.72923616}, -{23.49,11.72594107}, -{23.5,11.72317726}, -{23.51,11.71921311}, -{23.52,11.71591844}, -{23.53,11.71315525}, -{23.54,11.70893428}, -{23.55,11.70617158}, -{23.56,11.70313467}, -{23.57,11.69984077}, -{23.58,11.6961521}, -{23.59,11.6931155}, -{23.6,11.69009692}, -{23.61,11.68613405}, -{23.62,11.68311607}, -{23.63,11.68007992}, -{23.64,11.67704385}, -{23.65,11.67310019}, -{23.66,11.67006435}, -{23.67,11.66730427}, -{23.68,11.66401209}, -{23.69,11.66005021}, -{23.7,11.65729077}, -{23.71,11.65399901}, -{23.72,11.65124002}, -{23.73,11.64794862}, -{23.74,11.64398738}, -{23.75,11.64122902}, -{23.76,11.63793805}, -{23.77,11.63490335}, -{23.78,11.63214559}, -{23.79,11.62792892}, -{23.8,11.62517165}, -{23.81,11.62213741}, -{23.82,11.61884732}, -{23.83,11.61609065}, -{23.84,11.61213068}, -{23.85,11.6091187}, -{23.86,11.606085}, -{23.87,11.60305137}, -{23.88,11.60004013}, -{23.89,11.59700673}, -{23.9,11.59332571}, -{23.91,11.59003701}, -{23.92,11.58700393}, -{23.93,11.58424944}, -{23.94,11.58096119}, -{23.95,11.5779284}, -{23.96,11.57517451}, -{23.97,11.57096094}, -{23.98,11.56820755}, -{23.99,11.56517523}, -{24,11.56214297}, -{24.01,11.55913509}, -{24.02,11.55610306}, -{24.03,11.55307111}, -{24.04,11.55006398}, -{24.05,11.54610666}, -{24.06,11.54307502}, -{24.07,11.54006867}, -{24.08,11.53703727}, -{24.09,11.53428619}, -{24.1,11.53125501}, -{24.11,11.52796925}, -{24.12,11.52521878}, -{24.13,11.5221879}, -{24.14,11.5189026}, -{24.15,11.51615274}, -{24.16,11.51312217}, -{24.17,11.51009166}, -{24.18,11.50616274}, -{24.19,11.50313249}, -{24.2,11.5001023}, -{24.21,11.4970995}, -{24.22,11.49406956}, -{24.23,11.49132142}, -{24.24,11.48829169}, -{24.25,11.48500801}, -{24.26,11.48226049}, -{24.27,11.47923107}, -{24.28,11.47620172}, -{24.29,11.47320095}, -{24.3,11.47017184}, -{24.31,11.4671428}, -{24.32,11.4643965}, -{24.33,11.46111399}, -{24.34,11.45808527}, -{24.35,11.45533958}, -{24.36,11.45231108}, -{24.37,11.44902913}, -{24.38,11.44628407}, -{24.39,11.44325587}, -{24.4,11.44022775}, -{24.41,11.43722995}, -{24.42,11.43420207}, -{24.43,11.43117426}, -{24.44,11.42843043}, -{24.45,11.42514967}, -{24.46,11.42212218}, -{24.47,11.41937897}, -{24.48,11.41635169}, -{24.49,11.41332449}, -{24.5,11.41125361}, -{24.51,11.40822664}, -{24.52,11.40519974}, -{24.53,11.40245772}, -{24.54,11.39917826}, -{24.55,11.39615168}, -{24.56,11.39341029}, -{24.57,11.39038393}, -{24.58,11.38735764}, -{24.59,11.38436428}, -{24.6,11.38133823}, -{24.61,11.37831227}, -{24.62,11.37528637}, -{24.63,11.37254637}, -{24.64,11.37019274}, -{24.65,11.36716715}, -{24.66,11.36442775}, -{24.67,11.36140238}, -{24.68,11.35837708}, -{24.69,11.35538612}, -{24.7,11.35236108}, -{24.71,11.3493361}, -{24.72,11.34659795}, -{24.73,11.34449743}, -{24.74,11.34122079}, -{24.75,11.3381962}, -{24.76,11.33545879}, -{24.77,11.33243443}, -{24.78,11.32941013}, -{24.79,11.32642159}, -{24.8,11.32432166}, -{24.81,11.32129767}, -{24.82,11.31856148}, -{24.83,11.31553772}, -{24.84,11.31251404}, -{24.85,11.30923892}, -{24.86,11.30742751}, -{24.87,11.30440414}, -{24.88,11.30138084}, -{24.89,11.29864603}, -{24.9,11.29562296}, -{24.91,11.29234869}, -{24.92,11.29053838}, -{24.93,11.28751563}, -{24.94,11.28449294}, -{24.95,11.28147034}, -{24.96,11.2787369}, -{24.97,11.27663828},}; + arma::mat LkTable = { + {0, 191.3693381}, + {0.01, 391.0831124}, + {0.02, 385.6263172}, + {0.03, 379.2536529}, + {0.04, 372.8504183}, + {0.05, 366.4351054}, + {0.06, 359.9487249}, + {0.07, 353.4523708}, + {0.08, 346.9151605}, + {0.09, 340.3533742}, + {0.1, 339.9823669}, + {0.11, 339.649644}, + {0.12, 339.3131193}, + {0.13, 338.9712498}, + {0.14, 338.6260923}, + {0.15, 338.2779261}, + {0.16, 337.9238608}, + {0.17, 337.5646868}, + {0.18, 337.2045652}, + {0.19, 336.8363276}, + {0.2, 337.6125746}, + {0.21, 338.3898123}, + {0.22, 339.1721974}, + {0.23, 339.9508641}, + {0.24, 340.7342869}, + {0.25, 341.5135997}, + {0.26, 342.2964879}, + {0.27, 343.0777302}, + {0.28, 343.8624016}, + {0.29, 344.6450452}, + {0.3, 345.4302226}, + {0.31, 346.2163638}, + {0.32, 347.003086}, + {0.33, 347.7904035}, + {0.34, 348.5766329}, + {0.35, 349.3650418}, + {0.36, 350.1543455}, + {0.37, 350.9437807}, + {0.38, 351.735714}, + {0.39, 352.5236761}, + {0.4, 353.317052}, + {0.41, 354.1104116}, + {0.42, 354.9032791}, + {0.43, 355.6925592}, + {0.44, 356.490275}, + {0.45, 357.2834519}, + {0.46, 358.0785073}, + {0.47, 358.8713231}, + {0.48, 359.668187}, + {0.49, 360.4658938}, + {0.5, 361.2644865}, + {0.51, 362.0577138}, + {0.52, 362.8553987}, + {0.53, 363.6519585}, + {0.54, 364.4531391}, + {0.55, 365.2484945}, + {0.56, 366.0474406}, + {0.57, 366.8445417}, + {0.58, 367.645723}, + {0.59, 368.4312816}, + {0.6, 369.2180327}, + {0.61, 370.0057638}, + {0.62, 370.7934491}, + {0.63, 371.577332}, + {0.64, 372.3618727}, + {0.65, 373.1392343}, + {0.66, 373.9223953}, + {0.67, 374.6986382}, + {0.68, 375.4776747}, + {0.69, 376.2563757}, + {0.7, 377.5615194}, + {0.71, 378.8593047}, + {0.72, 380.1677186}, + {0.73, 381.4775822}, + {0.74, 382.7850068}, + {0.75, 384.0925559}, + {0.76, 385.4033472}, + {0.77, 386.680222}, + {0.78, 387.9603486}, + {0.79, 389.2414657}, + {0.8, 390.5176097}, + {0.81, 391.7783411}, + {0.82, 393.0383665}, + {0.83, 394.29121}, + {0.84, 395.5451831}, + {0.85, 396.779476}, + {0.86, 398.0051316}, + {0.87, 399.217608}, + {0.88, 400.433585}, + {0.89, 401.6396038}, + {0.9, 402.8249609}, + {0.91, 403.9998189}, + {0.92, 405.1644136}, + {0.93, 406.3320948}, + {0.94, 407.4898264}, + {0.95, 408.6386952}, + {0.96, 409.7882084}, + {0.97, 410.9206965}, + {0.98, 412.0711172}, + {0.99, 413.2273621}, + {1, 414.3762955}, + {1.01, 415.5172083}, + {1.02, 416.6851116}, + {1.03, 417.8740344}, + {1.04, 419.0501524}, + {1.05, 420.2265404}, + {1.06, 421.4316749}, + {1.07, 422.6518329}, + {1.08, 423.879432}, + {1.09, 425.0968591}, + {1.1, 426.3705142}, + {1.11, 427.6814206}, + {1.12, 428.9906538}, + {1.13, 430.3080079}, + {1.14, 431.6231364}, + {1.15, 432.9423547}, + {1.16, 434.2603966}, + {1.17, 435.5806367}, + {1.18, 436.9038196}, + {1.19, 438.2359771}, + {1.2, 439.760283}, + {1.21, 441.2855662}, + {1.22, 442.8237912}, + {1.23, 444.3543307}, + {1.24, 445.8935896}, + {1.25, 447.4374141}, + {1.26, 449.016306}, + {1.27, 450.6057334}, + {1.28, 452.1975701}, + {1.29, 453.7990419}, + {1.3, 455.3951522}, + {1.31, 457.0042091}, + {1.32, 458.6230155}, + {1.33, 460.2387287}, + {1.34, 461.8608819}, + {1.35, 463.4956492}, + {1.36, 465.1298266}, + {1.37, 466.7670335}, + {1.38, 468.4087662}, + {1.39, 470.0591738}, + {1.4, 471.7211266}, + {1.41, 473.4149126}, + {1.42, 475.0975536}, + {1.43, 476.8052568}, + {1.44, 478.5125067}, + {1.45, 480.2302357}, + {1.46, 481.9505168}, + {1.47, 483.6755965}, + {1.48, 485.4085203}, + {1.49, 487.1606942}, + {1.5, 488.912737}, + {1.51, 490.6763352}, + {1.52, 492.4461934}, + {1.53, 494.230761}, + {1.54, 496.0142138}, + {1.55, 497.8000122}, + {1.56, 499.5868444}, + {1.57, 501.3897915}, + {1.58, 503.1978733}, + {1.59, 505.0179768}, + {1.6, 506.8420775}, + {1.61, 508.6810082}, + {1.62, 510.5317451}, + {1.63, 512.3834648}, + {1.64, 514.2492508}, + {1.65, 516.1190989}, + {1.66, 518.0053088}, + {1.67, 519.9107613}, + {1.68, 521.8122756}, + {1.69, 523.7277473}, + {1.7, 526.52496}, + {1.71, 529.326074}, + {1.72, 532.1521584}, + {1.73, 534.9959392}, + {1.74, 537.8663106}, + {1.75, 532.5465503}, + {1.76, 527.1860241}, + {1.77, 521.7618548}, + {1.78, 516.3328104}, + {1.79, 510.856735}, + {1.8, 505.3589191}, + {1.81, 499.7424161}, + {1.82, 494.1151004}, + {1.83, 488.4906155}, + {1.84, 482.7875458}, + {1.85, 477.0777855}, + {1.86, 471.3509003}, + {1.87, 465.5580034}, + {1.88, 459.7619075}, + {1.89, 453.944495}, + {1.9, 448.0865852}, + {1.91, 442.2123527}, + {1.92, 436.3035816}, + {1.93, 430.3563548}, + {1.94, 424.4002234}, + {1.95, 406.4045572}, + {1.96, 388.2433759}, + {1.97, 370.1079284}, + {1.98, 351.8103035}, + {1.99, 333.4616772}, + {2, 320.4688275}, + {2.01, 307.452342}, + {2.02, 294.3528443}, + {2.03, 281.2674437}, + {2.04, 268.111713}, + {2.05, 254.9637706}, + {2.06, 241.7924305}, + {2.07, 228.5260562}, + {2.08, 215.276914}, + {2.09, 201.9631912}, + {2.1, 188.6795429}, + {2.11, 175.4304874}, + {2.12, 162.00753}, + {2.13, 148.7345206}, + {2.14, 135.3574292}, + {2.15, 126.6856641}, + {2.16, 118.020273}, + {2.17, 109.3739993}, + {2.18, 100.6846937}, + {2.19, 92.00283749}, + {2.2, 83.35951249}, + {2.21, 74.73486872}, + {2.22, 66.0716783}, + {2.23, 57.44510561}, + {2.24, 48.79168522}, + {2.25, 40.27062222}, + {2.26, 31.7147632}, + {2.27, 23.35713095}, + {2.28, 15.32234163}, + {2.29, 8.698553788}, + {2.3, 7.708774611}, + {2.31, 7.248557512}, + {2.32, 7.391062373}, + {2.33, 8.108155956}, + {2.34, 9.264564858}, + {2.35, 10.7236636}, + {2.36, 12.36361185}, + {2.37, 14.13412431}, + {2.38, 15.9852933}, + {2.39, 17.90058977}, + {2.4, 105.5371052}, + {2.41, 105.310299}, + {2.42, 105.0834932}, + {2.43, 104.8576945}, + {2.44, 104.6317221}, + {2.45, 104.4076761}, + {2.46, 104.1836307}, + {2.47, 103.9608891}, + {2.48, 103.7368451}, + {2.49, 103.5154945}, + {2.5, 103.2936742}, + {2.51, 103.0736278}, + {2.52, 102.8522788}, + {2.53, 102.6331529}, + {2.54, 102.413108}, + {2.55, 102.1952862}, + {2.56, 101.9774649}, + {2.57, 101.7597303}, + {2.58, 101.5428295}, + {2.59, 101.3269349}, + {2.6, 101.1113383}, + {2.61, 100.8953588}, + {2.62, 100.6826079}, + {2.63, 100.4688523}, + {2.64, 100.2551829}, + {2.65, 100.0437366}, + {2.66, 99.83090244}, + {2.67, 99.61945733}, + {2.68, 99.4088471}, + {2.69, 99.19870611}, + {2.7, 98.98910215}, + {2.71, 98.77988202}, + {2.72, 98.57288514}, + {2.73, 98.36467091}, + {2.74, 98.15767517}, + {2.75, 97.95168467}, + {2.76, 97.74469009}, + {2.77, 97.53870065}, + {2.78, 97.33484982}, + {2.79, 97.13016437}, + {2.8, 96.92731915}, + {2.81, 96.7234702}, + {2.82, 96.52062617}, + {2.83, 96.31816569}, + {2.84, 96.11754558}, + {2.85, 95.91692608}, + {2.86, 95.71630718}, + {2.87, 95.51699175}, + {2.88, 95.31729406}, + {2.89, 95.11889985}, + {2.9, 94.92059003}, + {2.91, 94.72349986}, + {2.92, 94.52602756}, + {2.93, 94.32994227}, + {2.94, 94.13385751}, + {2.95, 93.93768991}, + {2.96, 93.7438292}, + {2.97, 93.54904893}, + {2.98, 93.35610946}, + {2.99, 93.16225043}, + {3, 92.96922916}, + {3.01, 92.7776772}, + {3.02, 92.58566036}, + {3.03, 92.39494689}, + {3.04, 92.20423405}, + {3.05, 92.0148246}, + {3.06, 91.8250333}, + {3.07, 91.63662788}, + {3.08, 91.44814059}, + {3.09, 91.25973628}, + {3.1, 91.0721706}, + {3.11, 90.8859905}, + {3.12, 90.69972888}, + {3.13, 90.51354996}, + {3.14, 90.32829199}, + {3.15, 90.14433732}, + {3.16, 89.96000096}, + {3.17, 89.77512703}, + {3.18, 89.59301505}, + {3.19, 89.40998325}, + {3.2, 89.22825475}, + {3.21, 89.04614472}, + {3.22, 88.86533802}, + {3.23, 88.68461319}, + {3.24, 88.5047283}, + {3.25, 88.32530723}, + {3.26, 88.14542365}, + {3.27, 87.96684341}, + {3.28, 87.78834468}, + {3.29, 87.61168778}, + {3.3, 87.43411084}, + {3.31, 87.25775651}, + {3.32, 87.08148342}, + {3.33, 86.90613162}, + {3.34, 86.73078044}, + {3.35, 86.55542988}, + {3.36, 86.38230327}, + {3.37, 86.20695398}, + {3.38, 86.03474946}, + {3.39, 85.86170475}, + {3.4, 85.68988324}, + {3.41, 85.51768068}, + {3.42, 85.34686104}, + {3.43, 85.17596234}, + {3.44, 85.00598522}, + {3.45, 84.83516737}, + {3.46, 84.6661919}, + {3.47, 84.4965984}, + {3.48, 84.32854516}, + {3.49, 84.16087409}, + {3.5, 83.99358512}, + {3.51, 83.82583632}, + {3.52, 83.65908817}, + {3.53, 83.49272211}, + {3.54, 83.32727771}, + {3.55, 83.16183397}, + {3.56, 82.99731197}, + {3.57, 82.8328691}, + {3.58, 82.66872971}, + {3.59, 82.50551211}, + {3.6, 82.34199214}, + {3.61, 82.18007827}, + {3.62, 82.01870507}, + {3.63, 81.85648938}, + {3.64, 81.69549878}, + {3.65, 81.53458669}, + {3.66, 81.37421531}, + {3.67, 81.21452915}, + {3.68, 81.05546147}, + {3.69, 80.89547312}, + {3.7, 80.73770906}, + {3.71, 80.5790243}, + {3.72, 80.4212615}, + {3.73, 80.26442074}, + {3.74, 80.10758065}, + {3.75, 79.95112215}, + {3.76, 79.79558572}, + {3.77, 79.63966914}, + {3.78, 79.48505557}, + {3.79, 79.32959782}, + {3.8, 79.17536637}, + {3.81, 79.02167638}, + {3.82, 78.8684442}, + {3.83, 78.71605789}, + {3.84, 78.56282692}, + {3.85, 78.41143969}, + {3.86, 78.25905551}, + {3.87, 78.10805022}, + {3.88, 77.95666494}, + {3.89, 77.80650681}, + {3.9, 77.6564251}, + {3.91, 77.50726573}, + {3.92, 77.35718532}, + {3.93, 77.20840774}, + {3.94, 77.06017211}, + {3.95, 76.9123176}, + {3.96, 76.76446376}, + {3.97, 76.62527605}, + {3.98, 76.48731601}, + {3.99, 76.34943145}, + {4, 76.21154735}, + {4.01, 76.0745109}, + {4.02, 75.93754972}, + {4.03, 75.80096926}, + {4.04, 75.66493099}, + {4.05, 75.5283515}, + {4.06, 75.39323622}, + {4.07, 75.25757969}, + {4.08, 75.12284564}, + {4.09, 74.98719015}, + {4.1, 74.85375922}, + {4.11, 74.7199488}, + {4.12, 74.58613897}, + {4.13, 74.4527097}, + {4.14, 74.32020305}, + {4.15, 74.18677489}, + {4.16, 74.05434288}, + {4.17, 73.92275998}, + {4.18, 73.79025558}, + {4.19, 73.65874716}, + {4.2, 73.52754586}, + {4.21, 73.39603843}, + {4.22, 73.26668262}, + {4.23, 73.13647818}, + {4.24, 73.00527914}, + {4.25, 72.87692017}, + {4.26, 72.74664453}, + {4.27, 72.61828668}, + {4.28, 72.48938671}, + {4.29, 72.36095745}, + {4.3, 72.23298084}, + {4.31, 72.10500473}, + {4.32, 71.97664957}, + {4.33, 71.85089875}, + {4.34, 71.72384656}, + {4.35, 71.59672298}, + {4.36, 71.47059439}, + {4.37, 71.34484579}, + {4.38, 71.21909773}, + {4.39, 71.0938933}, + {4.4, 70.96821788}, + {4.41, 70.8433939}, + {4.42, 70.71949301}, + {4.43, 70.59467021}, + {4.44, 70.4711498}, + {4.45, 70.34824439}, + {4.46, 70.22472508}, + {4.47, 70.10174977}, + {4.48, 69.97915428}, + {4.49, 69.85663016}, + {4.5, 69.73403583}, + {4.51, 69.61243537}, + {4.52, 69.4907649}, + {4.53, 69.37046724}, + {4.54, 69.248798}, + {4.55, 69.12757867}, + {4.56, 69.00683342}, + {4.57, 68.88746064}, + {4.58, 68.7680884}, + {4.59, 68.64772398}, + {4.6, 68.52927568}, + {4.61, 68.40990511}, + {4.62, 68.29183674}, + {4.63, 68.17246724}, + {4.64, 68.05433046}, + {4.65, 67.93718666}, + {4.66, 67.81966471}, + {4.67, 67.70252207}, + {4.68, 67.58537999}, + {4.69, 67.4682385}, + {4.7, 67.35202056}, + {4.71, 67.23618183}, + {4.72, 67.12003386}, + {4.73, 67.00511924}, + {4.74, 66.88928217}, + {4.75, 66.77399025}, + {4.76, 66.65907746}, + {4.77, 66.54508839}, + {4.78, 66.43024508}, + {4.79, 66.31718035}, + {4.8, 66.20357152}, + {4.81, 66.08965293}, + {4.82, 65.97696844}, + {4.83, 65.86397411}, + {4.84, 65.75129087}, + {4.85, 65.63959913}, + {4.86, 65.52691713}, + {4.87, 65.4152265}, + {4.88, 65.30346905}, + {4.89, 65.19177955}, + {4.9, 65.08101391}, + {4.91, 64.97055983}, + {4.92, 64.85979538}, + {4.93, 64.74940953}, + {4.94, 64.63994759}, + {4.95, 64.5304195}, + {4.96, 64.42058083}, + {4.97, 64.31112069}, + {4.98, 64.20296247}, + {4.99, 64.09350346}, + {5, 63.98496852}, + {5.01, 63.8777355}, + {5.02, 63.76920181}, + {5.03, 63.66197001}, + {5.04, 63.55473882}, + {5.05, 63.44713053}, + {5.06, 63.33990058}, + {5.07, 63.23359487}, + {5.08, 63.1272898}, + {5.09, 63.0214284}, + {5.1, 62.91512453}, + {5.11, 62.809745}, + {5.12, 62.70480883}, + {5.13, 62.59980807}, + {5.14, 62.49480796}, + {5.15, 62.39041969}, + {5.16, 62.28542084}, + {5.17, 62.18134645}, + {5.18, 62.07733733}, + {5.19, 61.9745654}, + {5.2, 61.8705574}, + {5.21, 61.76740949}, + {5.22, 61.66470377}, + {5.23, 61.56162134}, + {5.24, 61.45977657}, + {5.25, 61.35799646}, + {5.26, 61.25615307}, + {5.27, 61.15437415}, + {5.28, 61.05259579}, + {5.29, 60.951742}, + {5.3, 60.84990134}, + {5.31, 60.74942582}, + {5.32, 60.64949795}, + {5.33, 60.54902358}, + {5.34, 60.44909694}, + {5.35, 60.34862373}, + {5.36, 60.24999934}, + {5.37, 60.15007461}, + {5.38, 60.05182832}, + {5.39, 59.95320583}, + {5.4, 59.85365978}, + {5.41, 59.75596279}, + {5.42, 59.65734225}, + {5.43, 59.55909902}, + {5.44, 59.46140404}, + {5.45, 59.36408636}, + {5.46, 59.26775553}, + {5.47, 59.17043906}, + {5.48, 59.07404759}, + {5.49, 58.97635595}, + {5.5, 58.88040385}, + {5.51, 58.78493873}, + {5.52, 58.68854994}, + {5.53, 58.5931475}, + {5.54, 58.49806084}, + {5.55, 58.40265956}, + {5.56, 58.30757421}, + {5.57, 58.21347488}, + {5.58, 58.1180146}, + {5.59, 58.02391646}, + {5.6, 57.92975833}, + {5.61, 57.83566137}, + {5.62, 57.74242924}, + {5.63, 57.64925811}, + {5.64, 57.55646364}, + {5.65, 57.46323365}, + {5.66, 57.37006437}, + {5.67, 57.27727167}, + {5.68, 57.18540423}, + {5.69, 57.09310183}, + {5.7, 57.00123568}, + {5.71, 56.90937012}, + {5.72, 56.81842995}, + {5.73, 56.7265656}, + {5.74, 56.63562668}, + {5.75, 56.54468839}, + {5.76, 56.45505132}, + {5.77, 56.3641143}, + {5.78, 56.27447849}, + {5.79, 56.18446768}, + {5.8, 56.09483315}, + {5.81, 56.00482369}, + {5.82, 55.91519047}, + {5.83, 55.82648287}, + {5.84, 55.73685095}, + {5.85, 55.6481447}, + {5.86, 55.56036419}, + {5.87, 55.47171714}, + {5.88, 55.38338824}, + {5.89, 55.29560974}, + {5.9, 55.20783199}, + {5.91, 55.12048763}, + {5.92, 55.03271123}, + {5.93, 54.94623595}, + {5.94, 54.85889329}, + {5.95, 54.77241935}, + {5.96, 54.6856279}, + {5.97, 54.59953044}, + {5.98, 54.51274022}, + {5.99, 54.42756936}, + {6, 54.34208069}, + {6.01, 54.2565362}, + {6.02, 54.17142378}, + {6.03, 54.08588077}, + {6.04, 54.00169487}, + {6.05, 53.91615335}, + {6.06, 53.8319687}, + {6.07, 53.74778462}, + {6.08, 53.66317074}, + {6.09, 53.57991343}, + {6.1, 53.49573124}, + {6.11, 53.41247515}, + {6.12, 53.32823895}, + {6.13, 53.24628434}, + {6.14, 53.16303021}, + {6.15, 53.07977671}, + {6.16, 52.99782396}, + {6.17, 52.91457174}, + {6.18, 52.83354588}, + {6.19, 52.75252068}, + {6.2, 52.67149614}, + {6.21, 52.59177232}, + {6.22, 52.51172899}, + {6.23, 52.43200639}, + {6.24, 52.35191018}, + {6.25, 52.27311468}, + {6.26, 52.19344784}, + {6.27, 52.11427943}, + {6.28, 52.03461365}, + {6.29, 51.95582072}, + {6.3, 51.8770285}, + {6.31, 51.7992161}, + {6.32, 51.72042521}, + {6.33, 51.64298797}, + {6.34, 51.56512432}, + {6.35, 51.4863883}, + {6.36, 51.40889994}, + {6.37, 51.33109103}, + {6.38, 51.25453}, + {6.39, 51.1767223}, + {6.4, 51.10021497}, + {6.41, 51.02365598}, + {6.42, 50.94677613}, + {6.43, 50.87027056}, + {6.44, 50.79371366}, + {6.45, 50.71813543}, + {6.46, 50.64163163}, + {6.47, 50.566003}, + {6.48, 50.49042663}, + {6.49, 50.41522435}, + {6.5, 50.33964914}, + {6.51, 50.26407455}, + {6.52, 50.18974918}, + {6.53, 50.11510217}, + {6.54, 50.04082915}, + {6.55, 49.96525709}, + {6.56, 49.8919116}, + {6.57, 49.81764038}, + {6.58, 49.74336976}, + {6.59, 49.67002613}, + {6.6, 49.59575673}, + {6.61, 49.52241437}, + {6.62, 49.44907265}, + {6.63, 49.37665805}, + {6.64, 49.30331765}, + {6.65, 49.2299779}, + {6.66, 49.15793829}, + {6.67, 49.08552639}, + {6.68, 49.01218863}, + {6.69, 48.94015097}, + {6.7, 48.86779023}, + {6.71, 48.7966804}, + {6.72, 48.72464462}, + {6.73, 48.65353614}, + {6.74, 48.58205566}, + {6.75, 48.51007046}, + {6.76, 48.43896396}, + {6.77, 48.3687849}, + {6.78, 48.29767983}, + {6.79, 48.22662358}, + {6.8, 48.1568191}, + {6.81, 48.08669074}, + {6.82, 48.01558828}, + {6.83, 47.94541339}, + {6.84, 47.8756592}, + {6.85, 47.80641256}, + {6.86, 47.73665949}, + {6.87, 47.66778656}, + {6.88, 47.59858928}, + {6.89, 47.5287908}, + {6.9, 47.45992002}, + {6.91, 47.39072463}, + {6.92, 47.32282897}, + {6.93, 47.25396027}, + {6.94, 47.18513879}, + {6.95, 47.11719857}, + {6.96, 47.04967737}, + {6.97, 46.98178475}, + {6.98, 46.91384668}, + {6.99, 46.84595533}, + {7, 46.77839063}, + {7.01, 46.7114277}, + {7.02, 46.64353826}, + {7.03, 46.57694835}, + {7.04, 46.51031367}, + {7.05, 46.44335335}, + {7.06, 46.37676531}, + {7.07, 46.31017787}, + {7.08, 46.24414673}, + {7.09, 46.17751571}, + {7.1, 46.11185753}, + {7.11, 46.04619998}, + {7.12, 45.98054307}, + {7.13, 45.91395938}, + {7.14, 45.84960253}, + {7.15, 45.78394749}, + {7.16, 45.7182931}, + {7.17, 45.65356684}, + {7.18, 45.58921251}, + {7.19, 45.52356007}, + {7.2, 45.45883582}, + {7.21, 45.39448341}, + {7.22, 45.33013163}, + {7.23, 45.26633701}, + {7.24, 45.20198655}, + {7.25, 45.13856437}, + {7.26, 45.07477189}, + {7.27, 45.01042343}, + {7.28, 44.9470033}, + {7.29, 44.88358387}, + {7.3, 44.82016513}, + {7.31, 44.75771711}, + {7.32, 44.69429966}, + {7.33, 44.63218143}, + {7.34, 44.56969317}, + {7.35, 44.50627783}, + {7.36, 44.44383274}, + {7.37, 44.38171718}, + {7.38, 44.31923174}, + {7.39, 44.25808684}, + {7.4, 44.19560278}, + {7.41, 44.13441787}, + {7.42, 44.07197633}, + {7.43, 44.01079279}, + {7.44, 43.94960999}, + {7.45, 43.88809827}, + {7.46, 43.82691687}, + {7.47, 43.76577664}, + {7.48, 43.70552469}, + {7.49, 43.64434543}, + {7.5, 43.58413512}, + {7.51, 43.52295728}, + {7.52, 43.46274819}, + {7.53, 43.40286997}, + {7.54, 43.3426621}, + {7.55, 43.28241531}, + {7.56, 43.22350686}, + {7.57, 43.16330081}, + {7.58, 43.10398447}, + {7.59, 43.04414954}, + {7.6, 42.98483474}, + {7.61, 42.92592934}, + {7.62, 42.86702451}, + {7.63, 42.80771199}, + {7.64, 42.74880843}, + {7.65, 42.68990543}, + {7.66, 42.63059523}, + {7.67, 42.57169353}, + {7.68, 42.51145649}, + {7.69, 42.45122031}, + {7.7, 42.39102268}, + {7.71, 42.332086}, + {7.72, 42.27281824}, + {7.73, 42.21295458}, + {7.74, 42.1536882}, + {7.75, 42.09475466}, + {7.76, 42.03678752}, + {7.77, 41.97748641}, + {7.78, 41.91859202}, + {7.79, 41.86059036}, + {7.8, 41.80169732}, + {7.81, 41.74369731}, + {7.82, 41.68573437}, + {7.83, 41.62684333}, + {7.84, 41.56884576}, + {7.85, 41.51181369}, + {7.86, 41.45381785}, + {7.87, 41.39585845}, + {7.88, 41.33919733}, + {7.89, 41.28216814}, + {7.9, 41.2241756}, + {7.91, 41.16751658}, + {7.92, 41.11141854}, + {7.93, 41.05379713}, + {7.94, 40.99770067}, + {7.95, 40.94104445}, + {7.96, 40.88494948}, + {7.97, 40.82829464}, + {7.98, 40.77256952}, + {7.99, 40.7168111}, + {8, 40.65979014}, + {8.01, 40.6049964}, + {8.02, 40.5492742}, + {8.03, 40.49355288}, + {8.04, 40.4378322}, + {8.05, 40.38304144}, + {8.06, 40.32769028}, + {8.07, 40.27290099}, + {8.08, 40.21811247}, + {8.09, 40.16332474}, + {8.1, 40.10890567}, + {8.11, 40.05411945}, + {8.12, 39.99933402}, + {8.13, 39.94491715}, + {8.14, 39.89106263}, + {8.15, 39.83664725}, + {8.16, 39.78279435}, + {8.17, 39.7293099}, + {8.18, 39.67582622}, + {8.19, 39.62197577}, + {8.2, 39.56852494}, + {8.21, 39.51504348}, + {8.22, 39.46156279}, + {8.23, 39.40901245}, + {8.24, 39.35553335}, + {8.25, 39.30301525}, + {8.26, 39.24953762}, + {8.27, 39.19699041}, + {8.28, 39.14444404}, + {8.29, 39.0919286}, + {8.3, 39.04068058}, + {8.31, 38.98813658}, + {8.32, 38.93562314}, + {8.33, 38.88344769}, + {8.34, 38.83183584}, + {8.35, 38.78062111}, + {8.36, 38.7290109}, + {8.37, 38.67683857}, + {8.38, 38.62525902}, + {8.39, 38.57494795}, + {8.4, 38.5237365}, + {8.41, 38.47213045}, + {8.42, 38.42089193}, + {8.43, 38.37061247}, + {8.44, 38.31937553}, + {8.45, 38.26909742}, + {8.46, 38.21879208}, + {8.47, 38.16721889}, + {8.48, 38.11821167}, + {8.49, 38.06700627}, + {8.5, 38.01766165}, + {8.51, 37.9673604}, + {8.52, 37.91708712}, + {8.53, 37.86771773}, + {8.54, 37.81744589}, + {8.55, 37.76844436}, + {8.56, 37.71910415}, + {8.57, 37.67013066}, + {8.58, 37.61983531}, + {8.59, 37.57179349}, + {8.6, 37.52245614}, + {8.61, 37.47345951}, + {8.62, 37.42412368}, + {8.63, 37.37608468}, + {8.64, 37.32711599}, + {8.65, 37.27868733}, + {8.66, 37.2297201}, + {8.67, 37.18168396}, + {8.68, 37.1336485}, + {8.69, 37.08558897}, + {8.7, 37.03755502}, + {8.71, 36.98915633}, + {8.72, 36.94241975}, + {8.73, 36.89438792}, + {8.74, 36.84635679}, + {8.75, 36.79923301}, + {8.76, 36.75120345}, + {8.77, 36.70410523}, + {8.78, 36.65700777}, + {8.79, 36.61027618}, + {8.8, 36.5631802}, + {8.81, 36.51608498}, + {8.82, 36.46935551}, + {8.83, 36.42226178}, + {8.84, 36.37646449}, + {8.85, 36.32937229}, + {8.86, 36.28357649}, + {8.87, 36.23648583}, + {8.88, 36.19069152}, + {8.89, 36.14489797}, + {8.9, 36.0987405}, + {8.91, 36.05294848}, + {8.92, 36.00715721}, + {8.93, 35.96136669}, + {8.94, 35.91557693}, + {8.95, 35.86978793}, + {8.96, 35.82493069}, + {8.97, 35.77916419}, + {8.98, 35.7343084}, + {8.99, 35.68852237}, + {9, 35.64366819}, + {9.01, 35.5988148}, + {9.02, 35.55396228}, + {9.03, 35.50913068}, + {9.04, 35.46427965}, + {9.05, 35.41979346}, + {9.06, 35.37587524}, + {9.07, 35.33102668}, + {9.08, 35.28749351}, + {9.09, 35.24264645}, + {9.1, 35.19909533}, + {9.11, 35.15424994}, + {9.12, 35.11071932}, + {9.13, 35.0668068}, + {9.14, 35.02325881}, + {9.15, 34.9793665}, + {9.16, 34.93582004}, + {9.17, 34.8922744}, + {9.18, 34.84838415}, + {9.19, 34.8057715}, + {9.2, 34.76222825}, + {9.21, 34.71963493}, + {9.22, 34.67609322}, + {9.23, 34.63350121}, + {9.24, 34.59089259}, + {9.25, 34.54830191}, + {9.26, 34.50476332}, + {9.27, 34.46215719}, + {9.28, 34.4205002}, + {9.29, 34.37789572}, + {9.3, 34.33530849}, + {9.31, 34.29270565}, + {9.32, 34.25141436}, + {9.33, 34.20881316}, + {9.34, 34.1671604}, + {9.35, 34.12549266}, + {9.36, 34.08327228}, + {9.37, 34.0416216}, + {9.38, 34.00031895}, + {9.39, 33.95866972}, + {9.4, 33.91736872}, + {9.41, 33.87572094}, + {9.42, 33.8344216}, + {9.43, 33.79277528}, + {9.44, 33.75242393}, + {9.45, 33.71112702}, + {9.46, 33.67041482}, + {9.47, 33.62913357}, + {9.48, 33.58877113}, + {9.49, 33.54749128}, + {9.5, 33.506782}, + {9.51, 33.4664221}, + {9.52, 33.42607642}, + {9.53, 33.38349213}, + {9.54, 33.34185389}, + {9.55, 33.29963327}, + {9.56, 33.2579839}, + {9.57, 33.21541585}, + {9.58, 33.17376836}, + {9.59, 33.13248356}, + {9.6, 33.09085011}, + {9.61, 33.04920553}, + {9.62, 33.0079235}, + {9.63, 32.96629251}, + {9.64, 32.92501229}, + {9.65, 32.88337165}, + {9.66, 32.84303684}, + {9.67, 32.80139814}, + {9.68, 32.76106491}, + {9.69, 32.71978933}, + {9.7, 32.67909651}, + {9.71, 32.63875528}, + {9.72, 32.59841507}, + {9.73, 32.55808586}, + {9.74, 32.51738663}, + {9.75, 32.47705906}, + {9.76, 32.43673222}, + {9.77, 32.39639674}, + {9.78, 32.35700414}, + {9.79, 32.31667062}, + {9.8, 32.27634704}, + {9.81, 32.23694815}, + {9.82, 32.19755893}, + {9.83, 32.15722936}, + {9.84, 32.11820228}, + {9.85, 32.07881552}, + {9.86, 32.03942161}, + {9.87, 32.00003659}, + {9.88, 31.96101262}, + {9.89, 31.9216217}, + {9.9, 31.88317208}, + {9.91, 31.84415051}, + {9.92, 31.80476263}, + {9.93, 31.76667564}, + {9.94, 31.72729651}, + {9.95, 31.68920457}, + {9.96, 31.65076021}, + {9.97, 31.61267652}, + {9.98, 31.57459362}, + {9.99, 31.53521874}, + {10, 31.49713169}, + {10.01, 31.4599844}, + {10.02, 31.42190484}, + {10.03, 31.38346657}, + {10.04, 31.34538864}, + {10.05, 31.30823975}, + {10.06, 31.27016359}, + {10.07, 31.23302145}, + {10.08, 31.19494693}, + {10.09, 31.15780647}, + {10.1, 31.12066686}, + {10.11, 31.08352811}, + {10.12, 31.04639022}, + {10.13, 31.00924954}, + {10.14, 30.97211354}, + {10.15, 30.9353373}, + {10.16, 30.89820299}, + {10.17, 30.86106955}, + {10.18, 30.82487043}, + {10.19, 30.78809747}, + {10.2, 30.75096665}, + {10.21, 30.71477021}, + {10.22, 30.67893324}, + {10.23, 30.64180509}, + {10.24, 30.60596983}, + {10.25, 30.569777}, + {10.26, 30.53394347}, + {10.27, 30.49775249}, + {10.28, 30.4619207}, + {10.29, 30.42608978}, + {10.3, 30.38990248}, + {10.31, 30.35407315}, + {10.32, 30.31917839}, + {10.33, 30.2829928}, + {10.34, 30.24809987}, + {10.35, 30.2122741}, + {10.36, 30.17738297}, + {10.37, 30.14155897}, + {10.38, 30.10631156}, + {10.39, 30.07142306}, + {10.4, 30.03653549}, + {10.41, 30.00164884}, + {10.42, 29.96676312}, + {10.43, 29.93187719}, + {10.44, 29.89699318}, + {10.45, 29.86211011}, + {10.46, 29.82758532}, + {10.47, 29.79363808}, + {10.48, 29.75875585}, + {10.49, 29.72387638}, + {10.5, 29.68993191}, + {10.51, 29.65634308}, + {10.52, 29.6214663}, + {10.53, 29.5875246}, + {10.54, 29.55358391}, + {10.55, 29.519998}, + {10.56, 29.48512494}, + {10.57, 29.45154383}, + {10.58, 29.41760332}, + {10.59, 29.38402392}, + {10.6, 29.35102307}, + {10.61, 29.31744152}, + {10.62, 29.28350828}, + {10.63, 29.25086245}, + {10.64, 29.21693108}, + {10.65, 29.18335712}, + {10.66, 29.15071358}, + {10.67, 29.11678509}, + {10.68, 29.08414305}, + {10.69, 29.05021647}, + {10.7, 29.01758148}, + {10.71, 28.98400733}, + {10.72, 28.95137415}, + {10.73, 28.91837999}, + {10.74, 28.88481418}, + {10.75, 28.85218387}, + {10.76, 28.81954804}, + {10.77, 28.78691958}, + {10.78, 28.75428528}, + {10.79, 28.72130305}, + {10.8, 28.68960496}, + {10.81, 28.65698032}, + {10.82, 28.62434918}, + {10.83, 28.5926611}, + {10.84, 28.56003155}, + {10.85, 28.52741071}, + {10.86, 28.49607273}, + {10.87, 28.46438852}, + {10.88, 28.43176212}, + {10.89, 28.40007989}, + {10.9, 28.36838991}, + {10.91, 28.33670971}, + {10.92, 28.30537634}, + {10.93, 28.27368876}, + {10.94, 28.24201145}, + {10.95, 28.21068035}, + {10.96, 28.17900504}, + {10.97, 28.14732081}, + {10.98, 28.115992}, + {10.99, 28.0852546}, + {11, 28.05392737}, + {11.01, 28.02224636}, + {11.02, 27.99186643}, + {11.03, 27.96018712}, + {11.04, 27.92979799}, + {11.05, 27.89906669}, + {11.06, 27.86774413}, + {11.07, 27.8373574}, + {11.08, 27.80662916}, + {11.09, 27.77624411}, + {11.1, 27.74585982}, + {11.11, 27.71512233}, + {11.12, 27.6847521}, + {11.13, 27.65437027}, + {11.14, 27.62398919}, + {11.15, 27.59325515}, + {11.16, 27.56382412}, + {11.17, 27.53344555}, + {11.18, 27.50306776}, + {11.19, 27.47362606}, + {11.2, 27.44324983}, + {11.21, 27.41382376}, + {11.22, 27.38438464}, + {11.23, 27.35401093}, + {11.24, 27.32457343}, + {11.25, 27.29513675}, + {11.26, 27.26570089}, + {11.27, 27.23626586}, + {11.28, 27.20719987}, + {11.29, 27.17776661}, + {11.3, 27.14833418}, + {11.31, 27.1189026}, + {11.32, 27.08947185}, + {11.33, 27.06039469}, + {11.34, 27.03096558}, + {11.35, 27.00247292}, + {11.36, 26.97304553}, + {11.37, 26.94397151}, + {11.38, 26.91548144}, + {11.39, 26.88642605}, + {11.4, 26.85793786}, + {11.41, 26.82945057}, + {11.42, 26.80038071}, + {11.43, 26.77189515}, + {11.44, 26.74376266}, + {11.45, 26.71527887}, + {11.46, 26.68714803}, + {11.47, 26.65901801}, + {11.48, 26.63053685}, + {11.49, 26.6024085}, + {11.5, 26.57391023}, + {11.51, 26.54578339}, + {11.52, 26.51765738}, + {11.53, 26.4895322}, + {11.54, 26.46199216}, + {11.55, 26.43386869}, + {11.56, 26.40574606}, + {11.57, 26.37856025}, + {11.58, 26.35008791}, + {11.59, 26.32290389}, + {11.6, 26.29572074}, + {11.61, 26.2676024}, + {11.62, 26.24040005}, + {11.63, 26.21321936}, + {11.64, 26.18510344}, + {11.65, 26.1579245}, + {11.66, 26.13074645}, + {11.67, 26.1035693}, + {11.68, 26.07639304}, + {11.69, 26.04919555}, + {11.7, 26.02202092}, + {11.71, 25.99578343}, + {11.72, 25.96861063}, + {11.73, 25.94143874}, + {11.74, 25.91459531}, + {11.75, 25.8883613}, + {11.76, 25.86119195}, + {11.77, 25.83402352}, + {11.78, 25.80779235}, + {11.79, 25.78095219}, + {11.8, 25.75472269}, + {11.81, 25.72849417}, + {11.82, 25.70168028}, + {11.83, 25.67542909}, + {11.84, 25.64920322}, + {11.85, 25.62332824}, + {11.86, 25.59614278}, + {11.87, 25.5702694}, + {11.88, 25.54404713}, + {11.89, 25.51780034}, + {11.9, 25.49192951}, + {11.91, 25.46570998}, + {11.92, 25.4407515}, + {11.93, 25.41488322}, + {11.94, 25.38866641}, + {11.95, 25.36277341}, + {11.96, 25.33655837}, + {11.97, 25.30721209}, + {11.98, 25.27786728}, + {11.99, 25.24849653}, + {12, 25.21915441}, + {12.01, 25.1898137}, + {12.02, 25.16047458}, + {12.03, 25.13204538}, + {12.04, 25.10270896}, + {12.05, 25.07337403}, + {12.06, 25.04494849}, + {12.07, 25.01561634}, + {12.08, 24.9862857}, + {12.09, 24.95821224}, + {12.1, 24.92982124}, + {12.11, 24.90049487}, + {12.12, 24.87207673}, + {12.13, 24.84403826}, + {12.14, 24.81562252}, + {12.15, 24.78723881}, + {12.16, 24.75917343}, + {12.17, 24.73079259}, + {12.18, 24.70241334}, + {12.19, 24.67435154}, + {12.2, 24.64597519}, + {12.21, 24.61885284}, + {12.22, 24.59047942}, + {12.23, 24.56335946}, + {12.24, 24.53533627}, + {12.25, 24.50787145}, + {12.26, 24.47981777}, + {12.27, 24.4527359}, + {12.28, 24.42527482}, + {12.29, 24.39725858}, + {12.3, 24.37014694}, + {12.31, 24.34307082}, + {12.32, 24.3156149}, + {12.33, 24.28850687}, + {12.34, 24.26143502}, + {12.35, 24.23432943}, + {12.36, 24.20726048}, + {12.37, 24.18109478}, + {12.38, 24.15399282}, + {12.39, 24.12692815}, + {12.4, 24.10076615}, + {12.41, 24.07366783}, + {12.42, 24.04754501}, + {12.43, 24.02044923}, + {12.44, 23.99429215}, + {12.45, 23.96817377}, + {12.46, 23.94108172}, + {12.47, 23.91492845}, + {12.48, 23.88912194}, + {12.49, 23.86300939}, + {12.5, 23.8368599}, + {12.51, 23.81071164}, + {12.52, 23.78490986}, + {12.53, 23.75876398}, + {12.54, 23.73265881}, + {12.55, 23.7077984}, + {12.56, 23.6816564}, + {12.57, 23.65551566}, + {12.58, 23.62972097}, + {12.59, 23.60452048}, + {12.6, 23.57876888}, + {12.61, 23.55357111}, + {12.62, 23.52871915}, + {12.63, 23.50258603}, + {12.64, 23.47773645}, + {12.65, 23.45288805}, + {12.66, 23.42675865}, + {12.67, 23.40191266}, + {12.68, 23.37706784}, + {12.69, 23.3518802}, + {12.7, 23.32708081}, + {12.71, 23.29967609}, + {12.72, 23.27266003}, + {12.73, 23.24564574}, + {12.74, 23.21863323}, + {12.75, 23.19251634}, + {12.76, 23.16516372}, + {12.77, 23.13909454}, + {12.78, 23.112044}, + {12.79, 23.08597821}, + {12.8, 23.05897606}, + {12.81, 23.03286817}, + {12.82, 23.00680764}, + {12.83, 22.97976445}, + {12.84, 22.95370737}, + {12.85, 22.92799483}, + {12.86, 22.90189438}, + {12.87, 22.87584259}, + {12.88, 22.84974514}, + {12.89, 22.82369687}, + {12.9, 22.79854085}, + {12.91, 22.77283829}, + {12.92, 22.74674687}, + {12.93, 22.72164409}, + {12.94, 22.69589765}, + {12.95, 22.67079842}, + {12.96, 22.64471311}, + {12.97, 22.6199592}, + {12.98, 22.59387694}, + {12.99, 22.56907614}, + {13, 22.54398583}, + {13.01, 22.519188}, + {13.02, 22.49410134}, + {13.03, 22.46836788}, + {13.04, 22.44417194}, + {13.05, 22.41943166}, + {13.06, 22.39464117}, + {13.07, 22.36951124}, + {13.08, 22.3447761}, + {13.09, 22.31999001}, + {13.1, 22.29580348}, + {13.11, 22.27107355}, + {13.12, 22.24629192}, + {13.13, 22.22245045}, + {13.14, 22.19772569}, + {13.15, 22.1738873}, + {13.16, 22.14971031}, + {13.17, 22.12493602}, + {13.18, 22.1011571}, + {13.19, 22.07732477}, + {13.2, 22.0534939}, + {13.21, 22.02966448}, + {13.22, 22.00589262}, + {13.23, 21.98206637}, + {13.24, 21.9582416}, + {13.25, 21.9344183}, + {13.26, 21.91093565}, + {13.27, 21.88711523}, + {13.28, 21.86429292}, + {13.29, 21.84047575}, + {13.3, 21.81666008}, + {13.31, 21.79412373}, + {13.32, 21.770311}, + {13.33, 21.74743888}, + {13.34, 21.72362921}, + {13.35, 21.70109859}, + {13.36, 21.67823104}, + {13.37, 21.6544259}, + {13.38, 21.63195974}, + {13.39, 21.60909704}, + {13.4, 21.58657391}, + {13.41, 21.56371429}, + {13.42, 21.54119407}, + {13.43, 21.51833755}, + {13.44, 21.49582027}, + {13.45, 21.47296687}, + {13.46, 21.45045254}, + {13.47, 21.42760227}, + {13.48, 21.40509091}, + {13.49, 21.38352034}, + {13.5, 21.36067478}, + {13.51, 21.33810451}, + {13.52, 21.31532552}, + {13.53, 21.29060603}, + {13.54, 21.26689173}, + {13.55, 21.24311512}, + {13.56, 21.22034434}, + {13.57, 21.19657128}, + {13.58, 21.17286521}, + {13.59, 21.1490957}, + {13.6, 21.12539377}, + {13.61, 21.10256736}, + {13.62, 21.07886961}, + {13.63, 21.05510731}, + {13.64, 21.03228625}, + {13.65, 21.00859467}, + {13.66, 20.98611288}, + {13.67, 20.96329714}, + {13.68, 20.94055131}, + {13.69, 20.91679968}, + {13.7, 20.89432461}, + {13.71, 20.87158501}, + {13.72, 20.84877843}, + {13.73, 20.82630851}, + {13.74, 20.80357522}, + {13.75, 20.78110883}, + {13.76, 20.7583094}, + {13.77, 20.73551176}, + {13.78, 20.71312132}, + {13.79, 20.69160153}, + {13.8, 20.66880938}, + {13.81, 20.64635316}, + {13.82, 20.62457646}, + {13.83, 20.60212387}, + {13.84, 20.58027891}, + {13.85, 20.5578297}, + {13.86, 20.53632199}, + {13.87, 20.51455595}, + {13.88, 20.49305197}, + {13.89, 20.47060979}, + {13.9, 20.44910917}, + {13.91, 20.42727711}, + {13.92, 20.40577999}, + {13.93, 20.38428458}, + {13.94, 20.36279089}, + {13.95, 20.34137471}, + {13.96, 20.31988474}, + {13.97, 20.29839651}, + {13.98, 20.27785001}, + {13.99, 20.25636527}, + {14, 20.23488228}, + {14.01, 20.2134010}, + {14.02, 20.19286161}, + {14.03, 20.17138391}, + {14.04, 20.15084805}, + {14.05, 20.12937391}, + {14.06, 20.10884164}, + {14.07, 20.08831118}, + {14.08, 20.06684243}, + {14.09, 20.0463156}, + {14.1, 20.02612189}, + {14.11, 20.00559862}, + {14.12, 19.98413703}, + {14.13, 19.96361743}, + {14.14, 19.94334949}, + {14.15, 19.92283321}, + {14.16, 19.90231878}, + {14.17, 19.88213686}, + {14.18, 19.86256627}, + {14.19, 19.84205738}, + {14.2, 19.82188069}, + {14.21, 19.80137546}, + {14.22, 19.78205917}, + {14.23, 19.76155735}, + {14.24, 19.74138739}, + {14.25, 19.72182956}, + {14.26, 19.70166313}, + {14.27, 19.68210906}, + {14.28, 19.66280148}, + {14.29, 19.64231061}, + {14.3, 19.62309135}, + {14.31, 19.60387385}, + {14.32, 19.5833025}, + {14.33, 19.56408835}, + {14.34, 19.54487598}, + {14.35, 19.52533651}, + {14.36, 19.50604058}, + {14.37, 19.48683338}, + {14.38, 19.4672994}, + {14.39, 19.4480959}, + {14.4, 19.42880586}, + {14.41, 19.40960569}, + {14.42, 19.39040734}, + {14.43, 19.36994231}, + {14.44, 19.35041974}, + {14.45, 19.33028662}, + {14.46, 19.31076803}, + {14.47, 19.29157915}, + {14.48, 19.2711241}, + {14.49, 19.25184804}, + {14.5, 19.23266453}, + {14.51, 19.21221515}, + {14.52, 19.19303548}, + {14.53, 19.1738577}, + {14.54, 19.15435486}, + {14.55, 19.13518095}, + {14.56, 19.11600895}, + {14.57, 19.09651222}, + {14.58, 19.07725048}, + {14.59, 19.05808401}, + {14.6, 19.03986}, + {14.61, 19.020371}, + {14.62, 19.00121059}, + {14.63, 18.98195683}, + {14.64, 18.9637404}, + {14.65, 18.94458535}, + {14.66, 18.92543225}, + {14.67, 18.90722172}, + {14.68, 18.88797596}, + {14.69, 18.86976908}, + {14.7, 18.85156418}, + {14.71, 18.83242066}, + {14.72, 18.81412198}, + {14.73, 18.79592278}, + {14.74, 18.77678495}, + {14.75, 18.75849112}, + {14.76, 18.74029765}, + {14.77, 18.72210621}, + {14.78, 18.70391681}, + {14.79, 18.68562961}, + {14.8, 18.66744399}, + {14.81, 18.64958453}, + {14.82, 18.63130218}, + {14.83, 18.61312231}, + {14.84, 18.59484321}, + {14.85, 18.57760787}, + {14.86, 18.55975749}, + {14.87, 18.54148333}, + {14.88, 18.52331312}, + {14.89, 18.506409}, + {14.9, 18.48813982}, + {14.91, 18.47091622}, + {14.92, 18.45297334}, + {14.93, 18.43575355}, + {14.94, 18.41749095}, + {14.95, 18.40059776}, + {14.96, 18.38233843}, + {14.97, 18.36544889}, + {14.98, 18.34823896}, + {14.99, 18.33030698}, + {15, 18.31310094}, + {15.01, 18.2961128}, + {15.02, 18.27891069}, + {15.03, 18.26192569}, + {15.04, 18.24504922}, + {15.05, 18.22774566}, + {15.06, 18.21076522}, + {15.07, 18.19357277}, + {15.08, 18.1765955}, + {15.09, 18.15972822}, + {15.1, 18.14243297}, + {15.11, 18.12556948}, + {15.12, 18.10953931}, + {15.13, 18.0925698}, + {15.14, 18.07539106}, + {15.15, 18.05936558}, + {15.16, 18.04251136}, + {15.17, 18.02554826}, + {15.18, 18.0095274}, + {15.19, 17.99235852}, + {15.2, 17.97634095}, + {15.21, 17.95938407}, + {15.22, 17.94348177}, + {15.23, 17.9274689}, + {15.24, 17.91062964}, + {15.25, 17.89462}, + {15.26, 17.8786118}, + {15.27, 17.86166442}, + {15.28, 17.84577334}, + {15.29, 17.82976996}, + {15.3, 17.8137681}, + {15.31, 17.79788268}, + {15.32, 17.78188412}, + {15.33, 17.76588709}, + {15.34, 17.75000738}, + {15.35, 17.73401367}, + {15.36, 17.7180215}, + {15.37, 17.70203087}, + {15.38, 17.68615874}, + {15.39, 17.67048955}, + {15.4, 17.65544451}, + {15.41, 17.63946025}, + {15.42, 17.62347755}, + {15.43, 17.6076148}, + {15.44, 17.59289385}, + {15.45, 17.57691603}, + {15.46, 17.56093978}, + {15.47, 17.5459059}, + {15.48, 17.53036979}, + {15.49, 17.51533925}, + {15.5, 17.49936953}, + {15.51, 17.48465902}, + {15.52, 17.46963317}, + {15.53, 17.45366815}, + {15.54, 17.43896204}, + {15.55, 17.42312196}, + {15.56, 17.40810274}, + {15.57, 17.39340133}, + {15.58, 17.37838525}, + {15.59, 17.36368679}, + {15.6, 17.34773311}, + {15.61, 17.33303762}, + {15.62, 17.31802786}, + {15.63, 17.30333534}, + {15.64, 17.28832878}, + {15.65, 17.27363925}, + {15.66, 17.25876085}, + {15.67, 17.24407461}, + {15.68, 17.22907476}, + {15.69, 17.21533227}, + {15.7, 17.20065051}, + {15.71, 17.18565547}, + {15.72, 17.17097676}, + {15.73, 17.15629954}, + {15.74, 17.14225006}, + {15.75, 17.12757589}, + {15.76, 17.11290323}, + {15.77, 17.09885862}, + {15.78, 17.08418904}, + {15.79, 17.06952097}, + {15.8, 17.05548126}, + {15.81, 17.0408163}, + {15.82, 17.02709353}, + {15.83, 17.01337227}, + {15.84, 16.99858182}, + {15.85, 16.98455001}, + {15.86, 16.96989246}, + {15.87, 16.9561771}, + {15.88, 16.94246328}, + {15.89, 16.92781034}, + {15.9, 16.91409959}, + {15.91, 16.90039038}, + {15.92, 16.88637013}, + {15.93, 16.8726641}, + {15.94, 16.85895964}, + {15.95, 16.8443161}, + {15.96, 16.83048134}, + {15.97, 16.81678126}, + {15.98, 16.80308275}, + {15.99, 16.78938581}, + {16, 16.77569045}, + {16.01, 16.7619966}, + {16.02, 16.74924503}, + {16.03, 16.73541903}, + {16.04, 16.7217297}, + {16.05, 16.70804196}, + {16.06, 16.69435581}, + {16.07, 16.68098232}, + {16.08, 16.66810304}, + {16.09, 16.6544213}, + {16.1, 16.64074118}, + {16.11, 16.62800319}, + {16.12, 16.61432629}, + {16.13, 16.60051285}, + {16.14, 16.58808973}, + {16.15, 16.5744173}, + {16.16, 16.56168699}, + {16.17, 16.54787856}, + {16.18, 16.53515122}, + {16.19, 16.52179485}, + {16.2, 16.50907066}, + {16.21, 16.49620775}, + {16.22, 16.48285561}, + {16.23, 16.47013594}, + {16.24, 16.45741793}, + {16.25, 16.44361967}, + {16.26, 16.43121377}, + {16.27, 16.41850033}, + {16.28, 16.40564625}, + {16.29, 16.39230423}, + {16.3, 16.37959539}, + {16.31, 16.3671968}, + {16.32, 16.35434777}, + {16.33, 16.34164355}, + {16.34, 16.32924928}, + {16.35, 16.31640408}, + {16.36, 16.30401257}, + {16.37, 16.29131459}, + {16.38, 16.2787811}, + {16.39, 16.2660861}, + {16.4, 16.25355487}, + {16.41, 16.24086288}, + {16.42, 16.22848006}, + {16.43, 16.21658521}, + {16.44, 16.20420519}, + {16.45, 16.19151954}, + {16.46, 16.1789954}, + {16.47, 16.16725301}, + {16.48, 16.15473116}, + {16.49, 16.14235835}, + {16.5, 16.12968041}, + {16.51, 16.11810231}, + {16.52, 16.10573389}, + {16.53, 16.09385165}, + {16.54, 16.08148609}, + {16.55, 16.0696064}, + {16.56, 16.05724372}, + {16.57, 16.04582273}, + {16.58, 16.03331263}, + {16.59, 16.02158869}, + {16.6, 16.00908095}, + {16.61, 15.99766564}, + {16.62, 15.98610027}, + {16.63, 15.97453592}, + {16.64, 15.96391265}, + {16.65, 15.95250262}, + {16.66, 15.94094157}, + {16.67, 15.92938153}, + {16.68, 15.91797559}, + {16.69, 15.90641786}, + {16.7, 15.89580114}, + {16.71, 15.8843993}, + {16.72, 15.87284489}, + {16.73, 15.86129153}, + {16.74, 15.85067917}, + {16.75, 15.83928272}, + {16.76, 15.82803683}, + {16.77, 15.8174277}, + {16.78, 15.8058797}, + {16.79, 15.79542848}, + {16.8, 15.78388282}, + {16.81, 15.7732781}, + {16.82, 15.7620381}, + {16.83, 15.75159221}, + {16.84, 15.74005092}, + {16.85, 15.72945053}, + {16.86, 15.71791138}, + {16.87, 15.70777403}, + {16.88, 15.69623716}, + {16.89, 15.68564114}, + {16.9, 15.67504619}, + {16.91, 15.66381524}, + {16.92, 15.65338142}, + {16.93, 15.64278981}, + {16.94, 15.63219927}, + {16.95, 15.62097241}, + {16.96, 15.61038392}, + {16.97, 15.59995686}, + {16.98, 15.5896728}, + {16.99, 15.57908768}, + {17, 15.567564}, + {17.01, 15.55728}, + {17.02, 15.5467008}, + {17.03, 15.53628186}, + {17.04, 15.5260038}, + {17.05, 15.51542526}, + {17.06, 15.50484782}, + {17.07, 15.49457266}, + {17.08, 15.48399732}, + {17.09, 15.47372408}, + {17.1, 15.46331442}, + {17.11, 15.45304336}, + {17.12, 15.44247247}, + {17.13, 15.43220334}, + {17.14, 15.42163458}, + {17.15, 15.41200639}, + {17.16, 15.40174019}, + {17.17, 15.39117468}, + {17.18, 15.381076}, + {17.19, 15.37051287}, + {17.2, 15.36119022}, + {17.21, 15.35062924}, + {17.22, 15.34036916}, + {17.23, 15.33011005}, + {17.24, 15.32049157}, + {17.25, 15.31023442}, + {17.26, 15.29967879}, + {17.27, 15.29036291}, + {17.28, 15.27980946}, + {17.29, 15.27049554}, + {17.3, 15.26024331}, + {17.31, 15.24969308}, + {17.32, 15.24055107}, + {17.33, 15.23000328}, + {17.34, 15.2206945}, + {17.35, 15.2104475}, + {17.36, 15.20084211}, + {17.37, 15.19059712}, + {17.38, 15.18129224}, + {17.39, 15.17169008}, + {17.4, 15.16144808}, + {17.41, 15.15214615}, + {17.42, 15.14160817}, + {17.43, 15.13230825}, + {17.44, 15.12300929}, + {17.45, 15.11277228}, + {17.46, 15.10317768}, + {17.47, 15.09388171}, + {17.48, 15.0845867}, + {17.49, 15.07435372}, + {17.5, 15.06476346}, + {17.51, 15.05547146}, + {17.52, 15.04618045}, + {17.53, 15.0368904}, + {17.54, 15.02636563}, + {17.55, 15.01707765}, + {17.56, 15.00779064}, + {17.57, 14.99850463}, + {17.58, 14.98921959}, + {17.59, 14.97976001}, + {17.6, 14.97018043}, + {17.61, 14.9608982}, + {17.62, 14.95161697}, + {17.63, 14.94233673}, + {17.64, 14.93305749}, + {17.65, 14.92377925}, + {17.66, 14.914502}, + {17.67, 14.90522576}, + {17.68, 14.89565497}, + {17.69, 14.88638082}, + {17.7, 14.87710768}, + {17.71, 14.86783555}, + {17.72, 14.85950302}, + {17.73, 14.85005401}, + {17.74, 14.84078465}, + {17.75, 14.83151631}, + {17.76, 14.82224899}, + {17.77, 14.81392119}, + {17.78, 14.80465589}, + {17.79, 14.79539161}, + {17.8, 14.78612836}, + {17.81, 14.77780457}, + {17.82, 14.76836233}, + {17.83, 14.7591019}, + {17.84, 14.75078089}, + {17.85, 14.74152251}, + {17.86, 14.73226517}, + {17.87, 14.72394719}, + {17.88, 14.71469191}, + {17.89, 14.70619329}, + {17.9, 14.69693982}, + {17.91, 14.68768739}, + {17.92, 14.67937427}, + {17.93, 14.67041721}, + {17.94, 14.66192218}, + {17.95, 14.65361177}, + {17.96, 14.64559541}, + {17.97, 14.63710245}, + {17.98, 14.62879476}, + {17.99, 14.62078086}, + {18, 14.61228996}, + {18.01, 14.603985}, + {18.02, 14.59578802}, + {18.03, 14.58748477}, + {18.04, 14.57918256}, + {18.05, 14.57098741}, + {18.06, 14.56268691}, + {18.07, 14.55449291}, + {18.08, 14.54619414}, + {18.09, 14.53789641}, + {18.1, 14.52970426}, + {18.11, 14.52234619}, + {18.12, 14.51415519}, + {18.13, 14.50586092}, + {18.14, 14.49785922}, + {18.15, 14.48937856}, + {18.16, 14.48108708}, + {18.17, 14.47383695}, + {18.18, 14.46554721}, + {18.19, 14.45736041}, + {18.2, 14.44907243}, + {18.21, 14.44107645}, + {18.22, 14.43353834}, + {18.23, 14.42554394}, + {18.24, 14.41706943}, + {18.25, 14.41001433}, + {18.26, 14.40154117}, + {18.27, 14.39354998}, + {18.28, 14.38601581}, + {18.29, 14.37802622}, + {18.3, 14.36974732}, + {18.31, 14.36250539}, + {18.32, 14.35451831}, + {18.33, 14.34698749}, + {18.34, 14.33900202}, + {18.35, 14.33053502}, + {18.36, 14.32348868}, + {18.37, 14.31502305}, + {18.38, 14.3079783}, + {18.39, 14.29980353}, + {18.4, 14.29247089}, + {18.41, 14.28429732}, + {18.42, 14.27696645}, + {18.43, 14.2687941}, + {18.44, 14.26175406}, + {18.45, 14.25329386}, + {18.46, 14.24625544}, + {18.47, 14.23902275}, + {18.48, 14.23075982}, + {18.49, 14.22352832}, + {18.5, 14.21555581}, + {18.51, 14.20803692}, + {18.52, 14.20100324}, + {18.53, 14.19283689}, + {18.54, 14.18551649}, + {18.55, 14.17828851}, + {18.56, 14.17032089}, + {18.57, 14.16280601}, + {18.58, 14.15577712}, + {18.59, 14.14855148}, + {18.6, 14.14029932}, + {18.61, 14.1330749}, + {18.62, 14.12585095}, + {18.63, 14.11882591}, + {18.64, 14.11131564}, + {18.65, 14.10335527}, + {18.66, 14.09613369}, + {18.67, 14.08911186}, + {18.68, 14.08160428}, + {18.69, 14.07458408}, + {18.7, 14.06736486}, + {18.71, 14.05940941}, + {18.72, 14.05190453}, + {18.73, 14.04468704}, + {18.74, 14.03767075}, + {18.75, 14.03045441}, + {18.76, 14.0234397}, + {18.77, 14.01593811}, + {18.78, 14.00892505}, + {18.79, 14.00171111}, + {18.8, 13.99449767}, + {18.81, 13.98748687}, + {18.82, 13.97998859}, + {18.83, 13.97297946}, + {18.84, 13.96576844}, + {18.85, 13.95855791}, + {18.86, 13.95155106}, + {18.87, 13.94434172}, + {18.88, 13.93733647}, + {18.89, 13.92984286}, + {18.9, 13.92263527}, + {18.91, 13.91656883}, + {18.92, 13.90936242}, + {18.93, 13.90236113}, + {18.94, 13.89515592}, + {18.95, 13.88795122}, + {18.96, 13.88095224}, + {18.97, 13.87374874}, + {18.98, 13.8671973}, + {18.99, 13.86020068}, + {19, 13.85299895}, + {19.01, 13.8460039}, + {19.02, 13.83973967}, + {19.03, 13.83253964}, + {19.04, 13.82554697}, + {19.05, 13.81834816}, + {19.06, 13.81208601}, + {19.07, 13.80509565}, + {19.08, 13.79789857}, + {19.09, 13.79163808}, + {19.1, 13.78465005}, + {19.11, 13.7774547}, + {19.12, 13.77140432}, + {19.13, 13.76421018}, + {19.14, 13.75701657}, + {19.15, 13.75096847}, + {19.16, 13.74377608}, + {19.17, 13.73658422}, + {19.18, 13.73053841}, + {19.19, 13.72334777}, + {19.2, 13.7170935}, + {19.21, 13.71011415}, + {19.22, 13.70386106}, + {19.23, 13.6966727}, + {19.24, 13.69042063}, + {19.25, 13.68344434}, + {19.26, 13.67719346}, + {19.27, 13.67000739}, + {19.28, 13.66396915}, + {19.29, 13.65678432}, + {19.3, 13.65053567}, + {19.31, 13.64356412}, + {19.32, 13.63759873}, + {19.33, 13.63041613}, + {19.34, 13.62438244}, + {19.35, 13.61813662}, + {19.36, 13.61095581}, + {19.37, 13.60471102}, + {19.38, 13.59774489}, + {19.39, 13.59150132}, + {19.4, 13.58525826}, + {19.41, 13.57857588}, + {19.42, 13.57233396}, + {19.43, 13.56609255}, + {19.44, 13.55891633}, + {19.45, 13.55289095}, + {19.46, 13.54665128}, + {19.47, 13.54069304}, + {19.48, 13.53445433}, + {19.49, 13.52821614}, + {19.5, 13.52225917}, + {19.51, 13.51623801}, + {19.52, 13.51000148}, + {19.53, 13.50404595}, + {19.54, 13.49781038}, + {19.55, 13.49157533}, + {19.56, 13.48534082}, + {19.57, 13.479387}, + {19.58, 13.47315345}, + {19.59, 13.46713797}, + {19.6, 13.46118561}, + {19.61, 13.45495373}, + {19.62, 13.44900223}, + {19.63, 13.44277133}, + {19.64, 13.43654096}, + {19.65, 13.43059075}, + {19.66, 13.42436136}, + {19.67, 13.41906737}, + {19.68, 13.41311843}, + {19.69, 13.40710987}, + {19.7, 13.40088269}, + {19.71, 13.39493524}, + {19.72, 13.38870905}, + {19.73, 13.38369717}, + {19.74, 13.37747196}, + {19.75, 13.37124729}, + {19.76, 13.36530198}, + {19.77, 13.3590783}, + {19.78, 13.35406848}, + {19.79, 13.3480669}, + {19.8, 13.34184492}, + {19.81, 13.33590196}, + {19.82, 13.32968098}, + {19.83, 13.32467339}, + {19.84, 13.3184534}, + {19.85, 13.31251216}, + {19.86, 13.30722762}, + {19.87, 13.30128723}, + {19.88, 13.29506924}, + {19.89, 13.28978619}, + {19.9, 13.28384711}, + {19.91, 13.27763066}, + {19.92, 13.27262679}, + {19.93, 13.26641135}, + {19.94, 13.26069779}, + {19.95, 13.2554178}, + {19.96, 13.2494815}, + {19.97, 13.24326825}, + {19.98, 13.23826703}, + {19.99, 13.23205479}, + {20, 13.2270544}, + {20.01, 13.220843}, + {20.02, 13.2149095}, + {20.03, 13.20963338}, + {20.04, 13.20370058}, + {20.05, 13.19842544}, + {20.06, 13.19249351}, + {20.07, 13.18721934}, + {20.08, 13.18128829}, + {20.09, 13.1760151}, + {20.1, 13.17008493}, + {20.11, 13.1650891}, + {20.12, 13.15888343}, + {20.13, 13.15388844}, + {20.14, 13.14768379}, + {20.15, 13.14268964}, + {20.16, 13.13648602}, + {20.17, 13.13149272}, + {20.18, 13.12529013}, + {20.19, 13.12029767}, + {20.2, 13.11437189}, + {20.21, 13.10910451}, + {20.22, 13.10411329}, + {20.23, 13.09791323}, + {20.24, 13.09292286}, + {20.25, 13.08699927}, + {20.26, 13.08173433}, + {20.27, 13.0767452}, + {20.28, 13.07054769}, + {20.29, 13.06555942}, + {20.3, 13.06057154}, + {20.31, 13.05437555}, + {20.32, 13.04938853}, + {20.33, 13.04412703}, + {20.34, 13.03820743}, + {20.35, 13.03322167}, + {20.36, 13.02796162}, + {20.37, 13.02204335}, + {20.38, 13.01705885}, + {20.39, 13.01180026}, + {20.4, 13.00588332}, + {20.41, 13.00090008}, + {20.42, 12.99564296}, + {20.43, 12.99066057}, + {20.44, 12.9847454}, + {20.45, 12.97948974}, + {20.46, 12.97450862}, + {20.47, 12.9695279}, + {20.48, 12.96427372}, + {20.49, 12.95836078}, + {20.5, 12.95338133}, + {20.51, 12.94812863}, + {20.52, 12.94315004}, + {20.53, 12.93817186}, + {20.54, 12.93292063}, + {20.55, 12.92701037}, + {20.56, 12.92203347}, + {20.57, 12.91678373}, + {20.58, 12.9118077}, + {20.59, 12.90683207}, + {20.6, 12.90185684}, + {20.61, 12.89660905}, + {20.62, 12.89046644}, + {20.63, 12.88549235}, + {20.64, 12.88051866}, + {20.65, 12.87527266}, + {20.66, 12.87029984}, + {20.67, 12.86532743}, + {20.68, 12.86008293}, + {20.69, 12.8551114}, + {20.7, 12.85014027}, + {20.71, 12.84516956}, + {20.72, 12.83992702}, + {20.73, 12.83495719}, + {20.74, 12.82998776}, + {20.75, 12.82501875}, + {20.76, 12.82005016}, + {20.77, 12.8145721}, + {20.78, 12.80960421}, + {20.79, 12.80463674}, + {20.8, 12.79966968}, + {20.81, 12.79443141}, + {20.82, 12.78946524}, + {20.83, 12.78449948}, + {20.84, 12.77953415}, + {20.85, 12.77456923}, + {20.86, 12.76933342}, + {20.87, 12.76436939}, + {20.88, 12.75940579}, + {20.89, 12.75420264}, + {20.9, 12.75017184}, + {20.91, 12.7452093}, + {20.92, 12.73997626}, + {20.93, 12.73501461}, + {20.94, 12.73005338}, + {20.95, 12.72509257}, + {20.96, 12.72013219}, + {20.97, 12.71517224}, + {20.98, 12.71063271}, + {20.99, 12.70567346}, + {21, 12.70071463}, + {21.01, 12.69575622}, + {21.02, 12.6907982}, + {21.03, 12.68677252}, + {21.04, 12.68181537}, + {21.05, 12.67658858}, + {21.06, 12.67163233}, + {21.07, 12.66643363}, + {21.08, 12.6624098}, + {21.09, 12.65745464}, + {21.1, 12.65249991}, + {21.11, 12.64754561}, + {21.12, 12.64352336}, + {21.13, 12.63830036}, + {21.14, 12.63334738}, + {21.15, 12.62815066}, + {21.16, 12.62412991}, + {21.17, 12.61917802}, + {21.18, 12.61422657}, + {21.19, 12.61020702}, + {21.2, 12.6052564}, + {21.21, 12.60030623}, + {21.22, 12.59604259}, + {21.23, 12.59109308}, + {21.24, 12.58707536}, + {21.25, 12.58188098}, + {21.26, 12.57786389}, + {21.27, 12.57384718}, + {21.28, 12.56865347}, + {21.29, 12.56490597}, + {21.3, 12.56064385}, + {21.31, 12.55569711}, + {21.32, 12.551682}, + {21.33, 12.54742049}, + {21.34, 12.54247484}, + {21.35, 12.53846075}, + {21.36, 12.53446804}, + {21.37, 12.5304545}, + {21.38, 12.52551029}, + {21.39, 12.52124994}, + {21.4, 12.51723743}, + {21.41, 12.51204644}, + {21.42, 12.50830244}, + {21.43, 12.5042909}, + {21.44, 12.50003148}, + {21.45, 12.49508964}, + {21.46, 12.49107914}, + {21.47, 12.48682033}, + {21.48, 12.48307799}, + {21.49, 12.47788864}, + {21.5, 12.47387935}, + {21.51, 12.46987045}, + {21.52, 12.46561258}, + {21.53, 12.46160431}, + {21.54, 12.45668327}, + {21.55, 12.45267558}, + {21.56, 12.44866828}, + {21.57, 12.44441136}, + {21.58, 12.4404047}, + {21.59, 12.43548443}, + {21.6, 12.43147835}, + {21.61, 12.42747267}, + {21.62, 12.42321669}, + {21.63, 12.41947837}, + {21.64, 12.41522271}, + {21.65, 12.41121823}, + {21.66, 12.40628365}, + {21.67, 12.40202862}, + {21.68, 12.39829166}, + {21.69, 12.39403695}, + {21.7, 12.3900341}, + {21.71, 12.38603165}, + {21.72, 12.38204381}, + {21.73, 12.37804193}, + {21.74, 12.37285783}, + {21.75, 12.36885662}, + {21.76, 12.36512184}, + {21.77, 12.36086866}, + {21.78, 12.35686841}, + {21.79, 12.35261563}, + {21.8, 12.34888185}, + {21.81, 12.34488258}, + {21.82, 12.34063037}, + {21.83, 12.33663175}, + {21.84, 12.33264554}, + {21.85, 12.32864749}, + {21.86, 12.32439601}, + {21.87, 12.32039861}, + {21.88, 12.316667}, + {21.89, 12.31241609}, + {21.9, 12.30841968}, + {21.91, 12.30416917}, + {21.92, 12.30043857}, + {21.93, 12.29618839}, + {21.94, 12.29219321}, + {21.95, 12.28846345}, + {21.96, 12.28421385}, + {21.97, 12.28021966}, + {21.98, 12.27623533}, + {21.99, 12.27224173}, + {22, 12.26799287}, + {22.01, 12.2639999}, + {22.02, 12.26001611}, + {22.03, 12.25602374}, + {22.04, 12.25203179}, + {22.05, 12.24804842}, + {22.06, 12.24405705}, + {22.07, 12.23980952}, + {22.08, 12.23608316}, + {22.09, 12.23183596}, + {22.1, 12.22784584}, + {22.11, 12.22386326}, + {22.12, 12.22080329}, + {22.13, 12.21681415}, + {22.14, 12.21283198}, + {22.15, 12.20884343}, + {22.16, 12.20459758}, + {22.17, 12.20087358}, + {22.18, 12.19662806}, + {22.19, 12.19264077}, + {22.2, 12.1886594}, + {22.21, 12.18560208}, + {22.22, 12.18135731}, + {22.23, 12.17763483}, + {22.24, 12.17364912}, + {22.25, 12.16940496}, + {22.26, 12.16568333}, + {22.27, 12.16236877}, + {22.28, 12.15838431}, + {22.29, 12.15440414}, + {22.3, 12.15042028}, + {22.31, 12.14644038}, + {22.32, 12.14245712}, + {22.33, 12.13914354}, + {22.34, 12.13542393}, + {22.35, 12.13118156}, + {22.36, 12.12719955}, + {22.37, 12.12322045}, + {22.38, 12.12016807}, + {22.39, 12.11644981}, + {22.4, 12.11220839}, + {22.41, 12.10822799}, + {22.42, 12.10517852}, + {22.43, 12.1011987}, + {22.44, 12.09722055}, + {22.45, 12.09324134}, + {22.46, 12.0899299}, + {22.47, 12.08621368}, + {22.48, 12.08197372}, + {22.49, 12.07825803}, + {22.5, 12.0749472}, + {22.51, 12.07096985}, + {22.52, 12.06699275}, + {22.53, 12.063016}, + {22.54, 12.05996787}, + {22.55, 12.05599171}, + {22.56, 12.05175323}, + {22.57, 12.04896821}, + {22.58, 12.04473009}, + {22.59, 12.04101694}, + {22.6, 12.03770776}, + {22.61, 12.0339951}, + {22.62, 12.02975769}, + {22.63, 12.02671252}, + {22.64, 12.022737}, + {22.65, 12.01876391}, + {22.66, 12.01571712}, + {22.67, 12.01174463}, + {22.68, 12.00776965}, + {22.69, 12.00472615}, + {22.7, 12.00075143}, + {22.71, 11.99678015}, + {22.72, 11.99373403}, + {22.73, 11.98976334}, + {22.74, 11.98552815}, + {22.75, 11.98274735}, + {22.76, 11.97851251}, + {22.77, 11.97573217}, + {22.78, 11.97149769}, + {22.79, 11.96778965}, + {22.8, 11.96448369}, + {22.81, 11.96077615}, + {22.82, 11.95747051}, + {22.83, 11.95376347}, + {22.84, 11.94953007}, + {22.85, 11.94675161}, + {22.86, 11.94251858}, + {22.87, 11.93947411}, + {22.88, 11.93550791}, + {22.89, 11.93246366}, + {22.9, 11.92849806}, + {22.91, 11.92545404}, + {22.92, 11.92148904}, + {22.93, 11.91751736}, + {22.94, 11.91448085}, + {22.95, 11.91050943}, + {22.96, 11.90747349}, + {22.97, 11.90350234}, + {22.98, 11.90046697}, + {22.99, 11.89649608}, + {23, 11.89346127}, + {23.01, 11.8894906}, + {23.02, 11.88645641}, + {23.03, 11.88248606}, + {23.04, 11.87918389}, + {23.05, 11.87548231}, + {23.06, 11.87218047}, + {23.07, 11.86847939}, + {23.08, 11.86517787}, + {23.09, 11.86147731}, + {23.1, 11.85817612}, + {23.11, 11.85540358}, + {23.12, 11.85117522}, + {23.13, 11.84840314}, + {23.14, 11.84417515}, + {23.15, 11.84113377}, + {23.16, 11.83717593}, + {23.17, 11.83413478}, + {23.18, 11.83043642}, + {23.19, 11.82713664}, + {23.2, 11.82436611}, + {23.21, 11.82013934}, + {23.22, 11.81736929}, + {23.23, 11.81314289}, + {23.24, 11.8101025}, + {23.25, 11.8061473}, + {23.26, 11.80310713}, + {23.27, 11.80033817}, + {23.28, 11.79611262}, + {23.29, 11.79334414}, + {23.3, 11.78911897}, + {23.31, 11.78635096}, + {23.32, 11.78305325}, + {23.33, 11.77908676}, + {23.34, 11.77631937}, + {23.35, 11.77302208}, + {23.36, 11.76932815}, + {23.37, 11.76603119}, + {23.38, 11.76326473}, + {23.39, 11.75904116}, + {23.4, 11.75600251}, + {23.41, 11.75230977}, + {23.42, 11.74901358}, + {23.43, 11.74624821}, + {23.44, 11.74202551}, + {23.45, 11.73898739}, + {23.46, 11.73622265}, + {23.47, 11.73292721}, + {23.48, 11.72923616}, + {23.49, 11.72594107}, + {23.5, 11.72317726}, + {23.51, 11.71921311}, + {23.52, 11.71591844}, + {23.53, 11.71315525}, + {23.54, 11.70893428}, + {23.55, 11.70617158}, + {23.56, 11.70313467}, + {23.57, 11.69984077}, + {23.58, 11.6961521}, + {23.59, 11.6931155}, + {23.6, 11.69009692}, + {23.61, 11.68613405}, + {23.62, 11.68311607}, + {23.63, 11.68007992}, + {23.64, 11.67704385}, + {23.65, 11.67310019}, + {23.66, 11.67006435}, + {23.67, 11.66730427}, + {23.68, 11.66401209}, + {23.69, 11.66005021}, + {23.7, 11.65729077}, + {23.71, 11.65399901}, + {23.72, 11.65124002}, + {23.73, 11.64794862}, + {23.74, 11.64398738}, + {23.75, 11.64122902}, + {23.76, 11.63793805}, + {23.77, 11.63490335}, + {23.78, 11.63214559}, + {23.79, 11.62792892}, + {23.8, 11.62517165}, + {23.81, 11.62213741}, + {23.82, 11.61884732}, + {23.83, 11.61609065}, + {23.84, 11.61213068}, + {23.85, 11.6091187}, + {23.86, 11.606085}, + {23.87, 11.60305137}, + {23.88, 11.60004013}, + {23.89, 11.59700673}, + {23.9, 11.59332571}, + {23.91, 11.59003701}, + {23.92, 11.58700393}, + {23.93, 11.58424944}, + {23.94, 11.58096119}, + {23.95, 11.5779284}, + {23.96, 11.57517451}, + {23.97, 11.57096094}, + {23.98, 11.56820755}, + {23.99, 11.56517523}, + {24, 11.56214297}, + {24.01, 11.55913509}, + {24.02, 11.55610306}, + {24.03, 11.55307111}, + {24.04, 11.55006398}, + {24.05, 11.54610666}, + {24.06, 11.54307502}, + {24.07, 11.54006867}, + {24.08, 11.53703727}, + {24.09, 11.53428619}, + {24.1, 11.53125501}, + {24.11, 11.52796925}, + {24.12, 11.52521878}, + {24.13, 11.5221879}, + {24.14, 11.5189026}, + {24.15, 11.51615274}, + {24.16, 11.51312217}, + {24.17, 11.51009166}, + {24.18, 11.50616274}, + {24.19, 11.50313249}, + {24.2, 11.5001023}, + {24.21, 11.4970995}, + {24.22, 11.49406956}, + {24.23, 11.49132142}, + {24.24, 11.48829169}, + {24.25, 11.48500801}, + {24.26, 11.48226049}, + {24.27, 11.47923107}, + {24.28, 11.47620172}, + {24.29, 11.47320095}, + {24.3, 11.47017184}, + {24.31, 11.4671428}, + {24.32, 11.4643965}, + {24.33, 11.46111399}, + {24.34, 11.45808527}, + {24.35, 11.45533958}, + {24.36, 11.45231108}, + {24.37, 11.44902913}, + {24.38, 11.44628407}, + {24.39, 11.44325587}, + {24.4, 11.44022775}, + {24.41, 11.43722995}, + {24.42, 11.43420207}, + {24.43, 11.43117426}, + {24.44, 11.42843043}, + {24.45, 11.42514967}, + {24.46, 11.42212218}, + {24.47, 11.41937897}, + {24.48, 11.41635169}, + {24.49, 11.41332449}, + {24.5, 11.41125361}, + {24.51, 11.40822664}, + {24.52, 11.40519974}, + {24.53, 11.40245772}, + {24.54, 11.39917826}, + {24.55, 11.39615168}, + {24.56, 11.39341029}, + {24.57, 11.39038393}, + {24.58, 11.38735764}, + {24.59, 11.38436428}, + {24.6, 11.38133823}, + {24.61, 11.37831227}, + {24.62, 11.37528637}, + {24.63, 11.37254637}, + {24.64, 11.37019274}, + {24.65, 11.36716715}, + {24.66, 11.36442775}, + {24.67, 11.36140238}, + {24.68, 11.35837708}, + {24.69, 11.35538612}, + {24.7, 11.35236108}, + {24.71, 11.3493361}, + {24.72, 11.34659795}, + {24.73, 11.34449743}, + {24.74, 11.34122079}, + {24.75, 11.3381962}, + {24.76, 11.33545879}, + {24.77, 11.33243443}, + {24.78, 11.32941013}, + {24.79, 11.32642159}, + {24.8, 11.32432166}, + {24.81, 11.32129767}, + {24.82, 11.31856148}, + {24.83, 11.31553772}, + {24.84, 11.31251404}, + {24.85, 11.30923892}, + {24.86, 11.30742751}, + {24.87, 11.30440414}, + {24.88, 11.30138084}, + {24.89, 11.29864603}, + {24.9, 11.29562296}, + {24.91, 11.29234869}, + {24.92, 11.29053838}, + {24.93, 11.28751563}, + {24.94, 11.28449294}, + {24.95, 11.28147034}, + {24.96, 11.2787369}, + {24.97, 11.27663828}, + }; - arma::uvec index_E = find(LkTable<=t_disparo, 1, "last"); + arma::uvec index_E = find(LkTable <= t_disparo, 1, "last"); + + if (index_E(0) < (LkTable.n_rows - 1)) + { + double tdisparo1 = LkTable(index_E(0), 0); + double tdisparo2 = LkTable(index_E(0) + 1, 0); + double Acc1 = LkTable(index_E(0), 1); + double Acc2 = LkTable(index_E(0) + 1, 1); + + Acc = (t_disparo - tdisparo1) * (Acc2 - Acc1) / (tdisparo2 - tdisparo1) + Acc1; + } + else + { + Acc = 9.81; + } - if (index_E(0)<(LkTable.n_rows-1)){ - double tdisparo1=LkTable(index_E(0),0); - double tdisparo2=LkTable(index_E(0)+1,0); - double Acc1=LkTable(index_E(0),1); - double Acc2=LkTable(index_E(0)+1,1); - - Acc=(t_disparo-tdisparo1)*(Acc2-Acc1)/(tdisparo2-tdisparo1)+Acc1; - }else{ - E=0; - } - return Acc; } From 2f17b5c5e0a371efe311186f7c86dc7573872224 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 5 Apr 2023 12:06:17 +0200 Subject: [PATCH 151/165] ADD: user clock drift to PVT_raw dump file --- src/algorithms/PVT/libs/rtklib_solver.cc | 12 ++++++++++-- src/algorithms/PVT/libs/vtl_engine.cc | 8 ++++++++ src/algorithms/PVT/libs/vtl_engine.h | 1 + 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 7dbc2f711..2af55de3e 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -236,7 +236,7 @@ bool Rtklib_Solver::save_vtl_matfile() const { // READ DUMP FILE const std::string dump_filename = d_vtl_dump_filename; - const int32_t number_of_double_vars = 27; + const int32_t number_of_double_vars = 28; const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 1; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + @@ -271,6 +271,7 @@ bool Rtklib_Solver::save_vtl_matfile() const auto week = std::vector(num_epoch); auto RX_time = std::vector(num_epoch); auto user_clk_offset = std::vector(num_epoch); + auto user_clk_offset_drift = std::vector(num_epoch); auto pos_x = std::vector(num_epoch); auto pos_y = std::vector(num_epoch); auto pos_z = std::vector(num_epoch); @@ -308,6 +309,7 @@ bool Rtklib_Solver::save_vtl_matfile() const dump_file.read(reinterpret_cast(&week[i]), sizeof(uint32_t)); dump_file.read(reinterpret_cast(&RX_time[i]), sizeof(double)); dump_file.read(reinterpret_cast(&user_clk_offset[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&user_clk_offset_drift[i]), sizeof(double)); dump_file.read(reinterpret_cast(&pos_x[i]), sizeof(double)); dump_file.read(reinterpret_cast(&pos_y[i]), sizeof(double)); dump_file.read(reinterpret_cast(&pos_z[i]), sizeof(double)); @@ -370,6 +372,10 @@ bool Rtklib_Solver::save_vtl_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("user_clk_offset_drift", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), user_clk_offset_drift.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_x.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -2097,9 +2103,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = gnss_observables_map.cbegin()->second.RX_time; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // User clock offset [s] - //tmp_double = rx_position_and_time[3]; tmp_double = vtl_engine.get_user_clock_offset_s(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // User clock offset drift[s/s] + tmp_double = vtl_engine.get_user_clock_offset_drift_s_s(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double) std::vector p_vec_m = vtl_engine.get_position_ecef_m(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 808dbe77d..14f08b1d3 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -3276,5 +3276,13 @@ double Vtl_Engine::get_user_clock_offset_s() double temp = 0; temp = kf_x[9]; + return temp; +} + +double Vtl_Engine::get_user_clock_offset_drift_s_s() +{ + double temp = 0; + temp = kf_x[10]; + return temp; } \ No newline at end of file diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index ec5db6fc9..98ad23931 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -57,6 +57,7 @@ public: double get_longitude(); // get_longitude double get_height(); // get_height double get_user_clock_offset_s(); // get_user_clock_offset_s; + double get_user_clock_offset_drift_s_s(); // get_user_clock_offset_drift_s/s; private: Vtl_Conf config; From adfa9db282700a2c1656a176c51b249e2cc58c8f Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 5 Apr 2023 15:35:24 +0200 Subject: [PATCH 152/165] ADD: rtklib usr_clk_drift to pvv_raw_vtl dump file --- src/algorithms/PVT/libs/rtklib_solver.cc | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 2af55de3e..bfd8a5e6b 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -236,7 +236,7 @@ bool Rtklib_Solver::save_vtl_matfile() const { // READ DUMP FILE const std::string dump_filename = d_vtl_dump_filename; - const int32_t number_of_double_vars = 28; + const int32_t number_of_double_vars = 29; const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 1; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + @@ -272,6 +272,7 @@ bool Rtklib_Solver::save_vtl_matfile() const auto RX_time = std::vector(num_epoch); auto user_clk_offset = std::vector(num_epoch); auto user_clk_offset_drift = std::vector(num_epoch); + auto rtklib_user_clk_offset_drift = std::vector(num_epoch); auto pos_x = std::vector(num_epoch); auto pos_y = std::vector(num_epoch); auto pos_z = std::vector(num_epoch); @@ -310,6 +311,7 @@ bool Rtklib_Solver::save_vtl_matfile() const dump_file.read(reinterpret_cast(&RX_time[i]), sizeof(double)); dump_file.read(reinterpret_cast(&user_clk_offset[i]), sizeof(double)); dump_file.read(reinterpret_cast(&user_clk_offset_drift[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&rtklib_user_clk_offset_drift[i]), sizeof(double)); dump_file.read(reinterpret_cast(&pos_x[i]), sizeof(double)); dump_file.read(reinterpret_cast(&pos_y[i]), sizeof(double)); dump_file.read(reinterpret_cast(&pos_z[i]), sizeof(double)); @@ -376,6 +378,10 @@ bool Rtklib_Solver::save_vtl_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("rtklib_user_clk_offset_drift", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), rtklib_user_clk_offset_drift.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_x.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -2102,13 +2108,17 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // PVT GPS time tmp_double = gnss_observables_map.cbegin()->second.RX_time; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // User clock offset [s] + // VTL filtered User clock offset [s] tmp_double = vtl_engine.get_user_clock_offset_s(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // User clock offset drift[s/s] + // VTL filtered User clock offset drift[s/s] tmp_double = vtl_engine.get_user_clock_offset_drift_s_s(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // rtklib User clock offset drift[s/s] + tmp_double = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double) std::vector p_vec_m = vtl_engine.get_position_ecef_m(); tmp_double = p_vec_m[0]; From 948406acfe218769873f33387f1328f4af4047d5 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 24 May 2023 17:07:27 +0200 Subject: [PATCH 153/165] FIX: doppler rate calculation error --- src/algorithms/PVT/libs/vtl_engine.cc | 16 ++++++++-------- src/algorithms/PVT/libs/vtl_engine.h | 2 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 2 +- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 14f08b1d3..80e5c6165 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -162,7 +162,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; } - obsv_calc(rho_pri, rhoDot_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); + obsv_calc(rho_pri, rhoDot_pri, rhoDot2_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction //************************************** @@ -171,7 +171,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt); - kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri * 0, new_data.pr_m, new_data.doppler_hz, kf_x); + kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot2_pri, new_data.pr_m, new_data.doppler_hz, kf_x); //************************************** // Kalman filter update step //************************************** @@ -190,7 +190,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // Geometric Transformation //************************* - obsv_calc(rho_pri, rhoDot_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); + obsv_calc(rho_pri, rhoDot_pri, rhoDot2_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt); // Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x @@ -315,7 +315,7 @@ void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt) kf_F(9, 10) = kf_dt; } -void Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x) +void Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x) { for (int32_t i = 0; i < sat_number; i++) // neccesary quantities { @@ -333,7 +333,7 @@ void Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colv az(i) = -(sat_p(i, 2) - kf_x(2)) / d(i); // compute pseudorange rate estimation OUTPUT rhoDot_pri(i) = (sat_v(i, 0) - kf_x(3)) * a_x(i) + (sat_v(i, 1) - kf_x(4)) * a_y(i) + (sat_v(i, 2) - kf_x(5)) * a_z(i); - // rhoDot_pri(i) = rhoDot_pri(i) + a_x(i)*xDot2_u*kf_dt+a_y(i)*yDot2_u*kf_dt+a_z(i)*zDot2_u*kf_dt; + rhoDot2_pri(i) = (kf_x(6)) * a_x(i) + (kf_x(7)) * a_y(i) + (kf_x(8)) * a_z(i); } } @@ -375,7 +375,7 @@ void Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma if (t_disparo < .2) { - u_dir = {.90828, -.13984, -.388756}; + u_dir = {.90828, -.13984, -.388756}; // launching attitude } else { @@ -439,11 +439,11 @@ void Vtl_Engine::accelerometer(double &acc_x, double &acc_y, double &acc_z, arma if (t_disparo < .2) { - u_dir = {.90828, -.13984, -.388756}; + u_dir = {.90828, -.13984, -.388756}; // launching attitude } else { - u_dir = u_vec / u; + u_dir = u_vec / u; } accelerometer = AccLkTable(t_disparo); diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 98ad23931..5cc664bb3 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -105,7 +105,7 @@ private: void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor void kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor - void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation + void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); void accelerometer(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 1c67a3f5f..ec233df9f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -673,7 +673,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // << " \n"; } d_x_old_old(2) = tmp_x(2); // replace DOPPLER - // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE + d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE } else { From b68d8c5c7c329e15e614dfc5a59c7a9ce308c95a Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 24 May 2023 17:23:25 +0200 Subject: [PATCH 154/165] FIX: doppler feedback --- src/algorithms/PVT/libs/vtl_engine.cc | 2 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 41 +++---------------- 2 files changed, 7 insertions(+), 36 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 80e5c6165..6c0b28f0e 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -111,7 +111,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { kf_R(i, i) = 80.0; kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; - kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; + kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 40.0; if (i == 6) { diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index ec233df9f..00095ac12 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -657,46 +657,17 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) double old_code_phase_chips = d_x_old_old(0); if (cmd->enable_carrier_nco_cmd) - { - if (abs(d_x_old_old(2) - tmp_x(2)) > 50) - { - // std::cout << "channel: " << this->d_channel - // << " tracking_cmd TOO FAR: " - // << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" - // << " \n"; - } - else - { - // std::cout << "channel: " << this->d_channel - // << " tracking_cmd NEAR: " - // << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" - // << " \n"; - } - d_x_old_old(2) = tmp_x(2); // replace DOPPLER - d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE - } + { + d_x_old_old(2) = tmp_x(2); // replace DOPPLER + } else - { - // std::cout << "correction not applied" << std::endl; - } + { + // std::cout << "correction not applied" << std::endl; + } // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; d_vtl_cmd_samplestamp = cmd->sample_counter; - // std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " - // << static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in - // << " SampleCounter origin: " << cmd->sample_counter - // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" - // << " [s]\n"; - // if(cmd->channel_id ==0) - // { - // std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " - // << delta_t_s << "[s]" - // << " SampleCounter origin: " << cmd->sample_counter - // << " code phase new state: " << x_tmp(0) << " vs. trk state: " << old_code_phase_chips << " [chips]" - // << "\n"; - // std::cout << "use count " < Date: Sun, 18 Jun 2023 22:48:02 +0200 Subject: [PATCH 155/165] ADD: 3DoF model linearization in F matrix --- src/algorithms/PVT/libs/vtl_engine.cc | 23 +++++++++++++++++++---- src/algorithms/PVT/libs/vtl_engine.h | 2 +- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 6c0b28f0e..05e0ed083 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -124,8 +124,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //************************************** // Kalman state prediction (time update) //************************************** - kf_F_fill(kf_F, kf_dt); - + if (counter < closure_point) { // receiver solution from rtklib_solver @@ -161,7 +160,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; // kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; } - + kf_F_fill(kf_F, kf_dt, kf_x); obsv_calc(rho_pri, rhoDot_pri, rhoDot2_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction @@ -299,8 +298,20 @@ void Vtl_Engine::kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arm } } -void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt) +void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x) { + + double densidad = 1.0; + double ballistic_coef = 0.007; + double diam_cohete = 120.0e-3; // 120 mm + double mass_rocket = 50.0; // 50Kg + double beta = (GNSS_PI * densidad * diam_cohete * diam_cohete / 8) * ballistic_coef; + // modulo de la velocidad + double vx = kf_x(3); + double vy = kf_x(4); + double vz = kf_x(5); + double u = norm(kf_x.rows(3, 5), 2); + kf_F(0, 3) = kf_dt; kf_F(0, 6) = kf_dt * kf_dt / 2; kf_F(1, 4) = kf_dt; @@ -312,6 +323,10 @@ void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt) kf_F(4, 7) = kf_dt; kf_F(5, 8) = kf_dt; + kf_F(6, 3) = -beta*(vx*vx/u+u); + kf_F(7, 4) = -beta*(vy*vy/u+u); + kf_F(8, 5) = -beta*(vz*vz/u+u); + kf_F(9, 10) = kf_dt; } diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 5cc664bb3..d1977df72 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -104,7 +104,7 @@ private: double delta_t_cmd = 0; void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor - void kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor + void kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); From 3f33a0fdd44f61374ec2d0009c4e3486e89fd0d6 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Sun, 18 Jun 2023 23:47:24 +0200 Subject: [PATCH 156/165] FORMAT: clang --- src/algorithms/PVT/libs/vtl_engine.cc | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 05e0ed083..dbf3358a6 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -124,7 +124,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //************************************** // Kalman state prediction (time update) //************************************** - + if (counter < closure_point) { // receiver solution from rtklib_solver @@ -300,7 +300,6 @@ void Vtl_Engine::kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arm void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x) { - double densidad = 1.0; double ballistic_coef = 0.007; double diam_cohete = 120.0e-3; // 120 mm @@ -323,14 +322,14 @@ void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x) kf_F(4, 7) = kf_dt; kf_F(5, 8) = kf_dt; - kf_F(6, 3) = -beta*(vx*vx/u+u); - kf_F(7, 4) = -beta*(vy*vy/u+u); - kf_F(8, 5) = -beta*(vz*vz/u+u); + kf_F(6, 3) = -beta * (vx * vx / u + u); + kf_F(7, 4) = -beta * (vy * vy / u + u); + kf_F(8, 5) = -beta * (vz * vz / u + u); kf_F(9, 10) = kf_dt; } -void Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x) +void Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x) { for (int32_t i = 0; i < sat_number; i++) // neccesary quantities { @@ -390,7 +389,7 @@ void Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma if (t_disparo < .2) { - u_dir = {.90828, -.13984, -.388756}; // launching attitude + u_dir = {.90828, -.13984, -.388756}; // launching attitude } else { @@ -454,11 +453,11 @@ void Vtl_Engine::accelerometer(double &acc_x, double &acc_y, double &acc_z, arma if (t_disparo < .2) { - u_dir = {.90828, -.13984, -.388756}; // launching attitude + u_dir = {.90828, -.13984, -.388756}; // launching attitude } else { - u_dir = u_vec / u; + u_dir = u_vec / u; } accelerometer = AccLkTable(t_disparo); From e53f2a14beb2412762881b785576a5f05562b80a Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Tue, 16 Jan 2024 19:45:41 +0100 Subject: [PATCH 157/165] MOD vtl_engine --- src/algorithms/PVT/libs/vtl_engine.cc | 2881 +------------------------ src/algorithms/PVT/libs/vtl_engine.h | 9 +- 2 files changed, 30 insertions(+), 2860 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index dbf3358a6..56e3fb876 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -142,7 +142,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; kf_dx = kf_x - kf_dx; - kf_dx = kf_F * kf_dx; // state prediction + kf_dx = kf_F * kf_dx; // state prediction/* */ } else { @@ -152,7 +152,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) double acc_x = 0; double acc_y = 0; double acc_z = 0; - model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter); kf_x(6) = acc_x; kf_x(7) = acc_y; kf_x(8) = acc_z; @@ -297,13 +296,37 @@ void Vtl_Engine::kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arm kf_H(i + 2 * sat_number, 10) = kf_dt; } } - void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x) +{ + + // modulo de la velocidad + double vx = kf_x(3); + double vy = kf_x(4); + double vz = kf_x(5); + double u = norm(kf_x.rows(3, 5), 2); + + kf_F(0, 3) = kf_dt; + kf_F(0, 6) = kf_dt * kf_dt / 2; + kf_F(1, 4) = kf_dt; + kf_F(1, 7) = kf_dt * kf_dt / 2; + kf_F(2, 5) = kf_dt; + kf_F(2, 8) = kf_dt * kf_dt / 2; + + kf_F(3, 6) = kf_dt; + kf_F(4, 7) = kf_dt; + kf_F(5, 8) = kf_dt; + + kf_F(6, 3) = (vx * vx / u + u); + kf_F(7, 4) = (vy * vy / u + u); + kf_F(8, 5) = (vz * vz / u + u); + + kf_F(9, 10) = kf_dt; +} +void Vtl_Engine::kf_F_fill_rocket(arma::mat &kf_F, double kf_dt, arma::mat &kf_x) { double densidad = 1.0; double ballistic_coef = 0.007; double diam_cohete = 120.0e-3; // 120 mm - double mass_rocket = 50.0; // 50Kg double beta = (GNSS_PI * densidad * diam_cohete * diam_cohete / 8) * ballistic_coef; // modulo de la velocidad double vx = kf_x(3); @@ -361,2856 +384,6 @@ void Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r } } -void Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter) -{ - arma::colvec u_vec; - arma::colvec acc_vec; - arma::colvec u_dir; - arma::colvec gravity_ECEF = {-7.826024, 0.8616969, -5.833042}; // lat=36.533333 lon=-6.283333 - static double t_disparo = 0; - double Empuje; - double densidad = 1.0; - double ballistic_coef = 0.007; - // vector velocidad - - u_vec = kf_x.rows(3, 5); - - // modulo de la velocidad - double u = norm(u_vec, 2); - - if (counter > 1500) - { - if (u > 6) - { - t_disparo = t_disparo + dt; - // std::cout << "u : " << u << endl; - double diam_cohete = 120.0e-3; // 120 mm - double mass_rocket = 50.0; // 50Kg - - if (t_disparo < .2) - { - u_dir = {.90828, -.13984, -.388756}; // launching attitude - } - else - { - u_dir = u_vec / u; - } - // u_dir.print("u_dir"); - // lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]); - // [T, sound_v, P, densidad] = atmosisa(lla(3)); - // sound_v=320;% @ 5km and -17.5C - // Mach=u/sound_v; - // CD0 = Cd0_M_LookTable(Mach); - // % ballistic_coef is Cd0/mass_rocket; - // ballistic_coef=CD0/mass_rocket; - Empuje = EmpujeLkTable(t_disparo); - // cout<<"Empuje: "< 1500) - { - if (u > 6) - { - t_disparo = t_disparo + dt; - - if (t_disparo < .2) - { - u_dir = {.90828, -.13984, -.388756}; // launching attitude - } - else - { - u_dir = u_vec / u; - } - - accelerometer = AccLkTable(t_disparo); - acc_vec = accelerometer * u_dir; - - acc_x = acc_vec(0); - acc_y = acc_vec(1); - acc_z = acc_vec(2); - } - else - { - t_disparo = 0; - // % return - acc_x = 0; - acc_y = 0; - acc_z = 0; - } - } - else - { - // % return - acc_x = 0; - acc_y = 0; - acc_z = 0; - } -} - -double Vtl_Engine::AccLkTable(double t_disparo) -{ - double Acc; - arma::mat LkTable = { - {0, 191.3693381}, - {0.01, 391.0831124}, - {0.02, 385.6263172}, - {0.03, 379.2536529}, - {0.04, 372.8504183}, - {0.05, 366.4351054}, - {0.06, 359.9487249}, - {0.07, 353.4523708}, - {0.08, 346.9151605}, - {0.09, 340.3533742}, - {0.1, 339.9823669}, - {0.11, 339.649644}, - {0.12, 339.3131193}, - {0.13, 338.9712498}, - {0.14, 338.6260923}, - {0.15, 338.2779261}, - {0.16, 337.9238608}, - {0.17, 337.5646868}, - {0.18, 337.2045652}, - {0.19, 336.8363276}, - {0.2, 337.6125746}, - {0.21, 338.3898123}, - {0.22, 339.1721974}, - {0.23, 339.9508641}, - {0.24, 340.7342869}, - {0.25, 341.5135997}, - {0.26, 342.2964879}, - {0.27, 343.0777302}, - {0.28, 343.8624016}, - {0.29, 344.6450452}, - {0.3, 345.4302226}, - {0.31, 346.2163638}, - {0.32, 347.003086}, - {0.33, 347.7904035}, - {0.34, 348.5766329}, - {0.35, 349.3650418}, - {0.36, 350.1543455}, - {0.37, 350.9437807}, - {0.38, 351.735714}, - {0.39, 352.5236761}, - {0.4, 353.317052}, - {0.41, 354.1104116}, - {0.42, 354.9032791}, - {0.43, 355.6925592}, - {0.44, 356.490275}, - {0.45, 357.2834519}, - {0.46, 358.0785073}, - {0.47, 358.8713231}, - {0.48, 359.668187}, - {0.49, 360.4658938}, - {0.5, 361.2644865}, - {0.51, 362.0577138}, - {0.52, 362.8553987}, - {0.53, 363.6519585}, - {0.54, 364.4531391}, - {0.55, 365.2484945}, - {0.56, 366.0474406}, - {0.57, 366.8445417}, - {0.58, 367.645723}, - {0.59, 368.4312816}, - {0.6, 369.2180327}, - {0.61, 370.0057638}, - {0.62, 370.7934491}, - {0.63, 371.577332}, - {0.64, 372.3618727}, - {0.65, 373.1392343}, - {0.66, 373.9223953}, - {0.67, 374.6986382}, - {0.68, 375.4776747}, - {0.69, 376.2563757}, - {0.7, 377.5615194}, - {0.71, 378.8593047}, - {0.72, 380.1677186}, - {0.73, 381.4775822}, - {0.74, 382.7850068}, - {0.75, 384.0925559}, - {0.76, 385.4033472}, - {0.77, 386.680222}, - {0.78, 387.9603486}, - {0.79, 389.2414657}, - {0.8, 390.5176097}, - {0.81, 391.7783411}, - {0.82, 393.0383665}, - {0.83, 394.29121}, - {0.84, 395.5451831}, - {0.85, 396.779476}, - {0.86, 398.0051316}, - {0.87, 399.217608}, - {0.88, 400.433585}, - {0.89, 401.6396038}, - {0.9, 402.8249609}, - {0.91, 403.9998189}, - {0.92, 405.1644136}, - {0.93, 406.3320948}, - {0.94, 407.4898264}, - {0.95, 408.6386952}, - {0.96, 409.7882084}, - {0.97, 410.9206965}, - {0.98, 412.0711172}, - {0.99, 413.2273621}, - {1, 414.3762955}, - {1.01, 415.5172083}, - {1.02, 416.6851116}, - {1.03, 417.8740344}, - {1.04, 419.0501524}, - {1.05, 420.2265404}, - {1.06, 421.4316749}, - {1.07, 422.6518329}, - {1.08, 423.879432}, - {1.09, 425.0968591}, - {1.1, 426.3705142}, - {1.11, 427.6814206}, - {1.12, 428.9906538}, - {1.13, 430.3080079}, - {1.14, 431.6231364}, - {1.15, 432.9423547}, - {1.16, 434.2603966}, - {1.17, 435.5806367}, - {1.18, 436.9038196}, - {1.19, 438.2359771}, - {1.2, 439.760283}, - {1.21, 441.2855662}, - {1.22, 442.8237912}, - {1.23, 444.3543307}, - {1.24, 445.8935896}, - {1.25, 447.4374141}, - {1.26, 449.016306}, - {1.27, 450.6057334}, - {1.28, 452.1975701}, - {1.29, 453.7990419}, - {1.3, 455.3951522}, - {1.31, 457.0042091}, - {1.32, 458.6230155}, - {1.33, 460.2387287}, - {1.34, 461.8608819}, - {1.35, 463.4956492}, - {1.36, 465.1298266}, - {1.37, 466.7670335}, - {1.38, 468.4087662}, - {1.39, 470.0591738}, - {1.4, 471.7211266}, - {1.41, 473.4149126}, - {1.42, 475.0975536}, - {1.43, 476.8052568}, - {1.44, 478.5125067}, - {1.45, 480.2302357}, - {1.46, 481.9505168}, - {1.47, 483.6755965}, - {1.48, 485.4085203}, - {1.49, 487.1606942}, - {1.5, 488.912737}, - {1.51, 490.6763352}, - {1.52, 492.4461934}, - {1.53, 494.230761}, - {1.54, 496.0142138}, - {1.55, 497.8000122}, - {1.56, 499.5868444}, - {1.57, 501.3897915}, - {1.58, 503.1978733}, - {1.59, 505.0179768}, - {1.6, 506.8420775}, - {1.61, 508.6810082}, - {1.62, 510.5317451}, - {1.63, 512.3834648}, - {1.64, 514.2492508}, - {1.65, 516.1190989}, - {1.66, 518.0053088}, - {1.67, 519.9107613}, - {1.68, 521.8122756}, - {1.69, 523.7277473}, - {1.7, 526.52496}, - {1.71, 529.326074}, - {1.72, 532.1521584}, - {1.73, 534.9959392}, - {1.74, 537.8663106}, - {1.75, 532.5465503}, - {1.76, 527.1860241}, - {1.77, 521.7618548}, - {1.78, 516.3328104}, - {1.79, 510.856735}, - {1.8, 505.3589191}, - {1.81, 499.7424161}, - {1.82, 494.1151004}, - {1.83, 488.4906155}, - {1.84, 482.7875458}, - {1.85, 477.0777855}, - {1.86, 471.3509003}, - {1.87, 465.5580034}, - {1.88, 459.7619075}, - {1.89, 453.944495}, - {1.9, 448.0865852}, - {1.91, 442.2123527}, - {1.92, 436.3035816}, - {1.93, 430.3563548}, - {1.94, 424.4002234}, - {1.95, 406.4045572}, - {1.96, 388.2433759}, - {1.97, 370.1079284}, - {1.98, 351.8103035}, - {1.99, 333.4616772}, - {2, 320.4688275}, - {2.01, 307.452342}, - {2.02, 294.3528443}, - {2.03, 281.2674437}, - {2.04, 268.111713}, - {2.05, 254.9637706}, - {2.06, 241.7924305}, - {2.07, 228.5260562}, - {2.08, 215.276914}, - {2.09, 201.9631912}, - {2.1, 188.6795429}, - {2.11, 175.4304874}, - {2.12, 162.00753}, - {2.13, 148.7345206}, - {2.14, 135.3574292}, - {2.15, 126.6856641}, - {2.16, 118.020273}, - {2.17, 109.3739993}, - {2.18, 100.6846937}, - {2.19, 92.00283749}, - {2.2, 83.35951249}, - {2.21, 74.73486872}, - {2.22, 66.0716783}, - {2.23, 57.44510561}, - {2.24, 48.79168522}, - {2.25, 40.27062222}, - {2.26, 31.7147632}, - {2.27, 23.35713095}, - {2.28, 15.32234163}, - {2.29, 8.698553788}, - {2.3, 7.708774611}, - {2.31, 7.248557512}, - {2.32, 7.391062373}, - {2.33, 8.108155956}, - {2.34, 9.264564858}, - {2.35, 10.7236636}, - {2.36, 12.36361185}, - {2.37, 14.13412431}, - {2.38, 15.9852933}, - {2.39, 17.90058977}, - {2.4, 105.5371052}, - {2.41, 105.310299}, - {2.42, 105.0834932}, - {2.43, 104.8576945}, - {2.44, 104.6317221}, - {2.45, 104.4076761}, - {2.46, 104.1836307}, - {2.47, 103.9608891}, - {2.48, 103.7368451}, - {2.49, 103.5154945}, - {2.5, 103.2936742}, - {2.51, 103.0736278}, - {2.52, 102.8522788}, - {2.53, 102.6331529}, - {2.54, 102.413108}, - {2.55, 102.1952862}, - {2.56, 101.9774649}, - {2.57, 101.7597303}, - {2.58, 101.5428295}, - {2.59, 101.3269349}, - {2.6, 101.1113383}, - {2.61, 100.8953588}, - {2.62, 100.6826079}, - {2.63, 100.4688523}, - {2.64, 100.2551829}, - {2.65, 100.0437366}, - {2.66, 99.83090244}, - {2.67, 99.61945733}, - {2.68, 99.4088471}, - {2.69, 99.19870611}, - {2.7, 98.98910215}, - {2.71, 98.77988202}, - {2.72, 98.57288514}, - {2.73, 98.36467091}, - {2.74, 98.15767517}, - {2.75, 97.95168467}, - {2.76, 97.74469009}, - {2.77, 97.53870065}, - {2.78, 97.33484982}, - {2.79, 97.13016437}, - {2.8, 96.92731915}, - {2.81, 96.7234702}, - {2.82, 96.52062617}, - {2.83, 96.31816569}, - {2.84, 96.11754558}, - {2.85, 95.91692608}, - {2.86, 95.71630718}, - {2.87, 95.51699175}, - {2.88, 95.31729406}, - {2.89, 95.11889985}, - {2.9, 94.92059003}, - {2.91, 94.72349986}, - {2.92, 94.52602756}, - {2.93, 94.32994227}, - {2.94, 94.13385751}, - {2.95, 93.93768991}, - {2.96, 93.7438292}, - {2.97, 93.54904893}, - {2.98, 93.35610946}, - {2.99, 93.16225043}, - {3, 92.96922916}, - {3.01, 92.7776772}, - {3.02, 92.58566036}, - {3.03, 92.39494689}, - {3.04, 92.20423405}, - {3.05, 92.0148246}, - {3.06, 91.8250333}, - {3.07, 91.63662788}, - {3.08, 91.44814059}, - {3.09, 91.25973628}, - {3.1, 91.0721706}, - {3.11, 90.8859905}, - {3.12, 90.69972888}, - {3.13, 90.51354996}, - {3.14, 90.32829199}, - {3.15, 90.14433732}, - {3.16, 89.96000096}, - {3.17, 89.77512703}, - {3.18, 89.59301505}, - {3.19, 89.40998325}, - {3.2, 89.22825475}, - {3.21, 89.04614472}, - {3.22, 88.86533802}, - {3.23, 88.68461319}, - {3.24, 88.5047283}, - {3.25, 88.32530723}, - {3.26, 88.14542365}, - {3.27, 87.96684341}, - {3.28, 87.78834468}, - {3.29, 87.61168778}, - {3.3, 87.43411084}, - {3.31, 87.25775651}, - {3.32, 87.08148342}, - {3.33, 86.90613162}, - {3.34, 86.73078044}, - {3.35, 86.55542988}, - {3.36, 86.38230327}, - {3.37, 86.20695398}, - {3.38, 86.03474946}, - {3.39, 85.86170475}, - {3.4, 85.68988324}, - {3.41, 85.51768068}, - {3.42, 85.34686104}, - {3.43, 85.17596234}, - {3.44, 85.00598522}, - {3.45, 84.83516737}, - {3.46, 84.6661919}, - {3.47, 84.4965984}, - {3.48, 84.32854516}, - {3.49, 84.16087409}, - {3.5, 83.99358512}, - {3.51, 83.82583632}, - {3.52, 83.65908817}, - {3.53, 83.49272211}, - {3.54, 83.32727771}, - {3.55, 83.16183397}, - {3.56, 82.99731197}, - {3.57, 82.8328691}, - {3.58, 82.66872971}, - {3.59, 82.50551211}, - {3.6, 82.34199214}, - {3.61, 82.18007827}, - {3.62, 82.01870507}, - {3.63, 81.85648938}, - {3.64, 81.69549878}, - {3.65, 81.53458669}, - {3.66, 81.37421531}, - {3.67, 81.21452915}, - {3.68, 81.05546147}, - {3.69, 80.89547312}, - {3.7, 80.73770906}, - {3.71, 80.5790243}, - {3.72, 80.4212615}, - {3.73, 80.26442074}, - {3.74, 80.10758065}, - {3.75, 79.95112215}, - {3.76, 79.79558572}, - {3.77, 79.63966914}, - {3.78, 79.48505557}, - {3.79, 79.32959782}, - {3.8, 79.17536637}, - {3.81, 79.02167638}, - {3.82, 78.8684442}, - {3.83, 78.71605789}, - {3.84, 78.56282692}, - {3.85, 78.41143969}, - {3.86, 78.25905551}, - {3.87, 78.10805022}, - {3.88, 77.95666494}, - {3.89, 77.80650681}, - {3.9, 77.6564251}, - {3.91, 77.50726573}, - {3.92, 77.35718532}, - {3.93, 77.20840774}, - {3.94, 77.06017211}, - {3.95, 76.9123176}, - {3.96, 76.76446376}, - {3.97, 76.62527605}, - {3.98, 76.48731601}, - {3.99, 76.34943145}, - {4, 76.21154735}, - {4.01, 76.0745109}, - {4.02, 75.93754972}, - {4.03, 75.80096926}, - {4.04, 75.66493099}, - {4.05, 75.5283515}, - {4.06, 75.39323622}, - {4.07, 75.25757969}, - {4.08, 75.12284564}, - {4.09, 74.98719015}, - {4.1, 74.85375922}, - {4.11, 74.7199488}, - {4.12, 74.58613897}, - {4.13, 74.4527097}, - {4.14, 74.32020305}, - {4.15, 74.18677489}, - {4.16, 74.05434288}, - {4.17, 73.92275998}, - {4.18, 73.79025558}, - {4.19, 73.65874716}, - {4.2, 73.52754586}, - {4.21, 73.39603843}, - {4.22, 73.26668262}, - {4.23, 73.13647818}, - {4.24, 73.00527914}, - {4.25, 72.87692017}, - {4.26, 72.74664453}, - {4.27, 72.61828668}, - {4.28, 72.48938671}, - {4.29, 72.36095745}, - {4.3, 72.23298084}, - {4.31, 72.10500473}, - {4.32, 71.97664957}, - {4.33, 71.85089875}, - {4.34, 71.72384656}, - {4.35, 71.59672298}, - {4.36, 71.47059439}, - {4.37, 71.34484579}, - {4.38, 71.21909773}, - {4.39, 71.0938933}, - {4.4, 70.96821788}, - {4.41, 70.8433939}, - {4.42, 70.71949301}, - {4.43, 70.59467021}, - {4.44, 70.4711498}, - {4.45, 70.34824439}, - {4.46, 70.22472508}, - {4.47, 70.10174977}, - {4.48, 69.97915428}, - {4.49, 69.85663016}, - {4.5, 69.73403583}, - {4.51, 69.61243537}, - {4.52, 69.4907649}, - {4.53, 69.37046724}, - {4.54, 69.248798}, - {4.55, 69.12757867}, - {4.56, 69.00683342}, - {4.57, 68.88746064}, - {4.58, 68.7680884}, - {4.59, 68.64772398}, - {4.6, 68.52927568}, - {4.61, 68.40990511}, - {4.62, 68.29183674}, - {4.63, 68.17246724}, - {4.64, 68.05433046}, - {4.65, 67.93718666}, - {4.66, 67.81966471}, - {4.67, 67.70252207}, - {4.68, 67.58537999}, - {4.69, 67.4682385}, - {4.7, 67.35202056}, - {4.71, 67.23618183}, - {4.72, 67.12003386}, - {4.73, 67.00511924}, - {4.74, 66.88928217}, - {4.75, 66.77399025}, - {4.76, 66.65907746}, - {4.77, 66.54508839}, - {4.78, 66.43024508}, - {4.79, 66.31718035}, - {4.8, 66.20357152}, - {4.81, 66.08965293}, - {4.82, 65.97696844}, - {4.83, 65.86397411}, - {4.84, 65.75129087}, - {4.85, 65.63959913}, - {4.86, 65.52691713}, - {4.87, 65.4152265}, - {4.88, 65.30346905}, - {4.89, 65.19177955}, - {4.9, 65.08101391}, - {4.91, 64.97055983}, - {4.92, 64.85979538}, - {4.93, 64.74940953}, - {4.94, 64.63994759}, - {4.95, 64.5304195}, - {4.96, 64.42058083}, - {4.97, 64.31112069}, - {4.98, 64.20296247}, - {4.99, 64.09350346}, - {5, 63.98496852}, - {5.01, 63.8777355}, - {5.02, 63.76920181}, - {5.03, 63.66197001}, - {5.04, 63.55473882}, - {5.05, 63.44713053}, - {5.06, 63.33990058}, - {5.07, 63.23359487}, - {5.08, 63.1272898}, - {5.09, 63.0214284}, - {5.1, 62.91512453}, - {5.11, 62.809745}, - {5.12, 62.70480883}, - {5.13, 62.59980807}, - {5.14, 62.49480796}, - {5.15, 62.39041969}, - {5.16, 62.28542084}, - {5.17, 62.18134645}, - {5.18, 62.07733733}, - {5.19, 61.9745654}, - {5.2, 61.8705574}, - {5.21, 61.76740949}, - {5.22, 61.66470377}, - {5.23, 61.56162134}, - {5.24, 61.45977657}, - {5.25, 61.35799646}, - {5.26, 61.25615307}, - {5.27, 61.15437415}, - {5.28, 61.05259579}, - {5.29, 60.951742}, - {5.3, 60.84990134}, - {5.31, 60.74942582}, - {5.32, 60.64949795}, - {5.33, 60.54902358}, - {5.34, 60.44909694}, - {5.35, 60.34862373}, - {5.36, 60.24999934}, - {5.37, 60.15007461}, - {5.38, 60.05182832}, - {5.39, 59.95320583}, - {5.4, 59.85365978}, - {5.41, 59.75596279}, - {5.42, 59.65734225}, - {5.43, 59.55909902}, - {5.44, 59.46140404}, - {5.45, 59.36408636}, - {5.46, 59.26775553}, - {5.47, 59.17043906}, - {5.48, 59.07404759}, - {5.49, 58.97635595}, - {5.5, 58.88040385}, - {5.51, 58.78493873}, - {5.52, 58.68854994}, - {5.53, 58.5931475}, - {5.54, 58.49806084}, - {5.55, 58.40265956}, - {5.56, 58.30757421}, - {5.57, 58.21347488}, - {5.58, 58.1180146}, - {5.59, 58.02391646}, - {5.6, 57.92975833}, - {5.61, 57.83566137}, - {5.62, 57.74242924}, - {5.63, 57.64925811}, - {5.64, 57.55646364}, - {5.65, 57.46323365}, - {5.66, 57.37006437}, - {5.67, 57.27727167}, - {5.68, 57.18540423}, - {5.69, 57.09310183}, - {5.7, 57.00123568}, - {5.71, 56.90937012}, - {5.72, 56.81842995}, - {5.73, 56.7265656}, - {5.74, 56.63562668}, - {5.75, 56.54468839}, - {5.76, 56.45505132}, - {5.77, 56.3641143}, - {5.78, 56.27447849}, - {5.79, 56.18446768}, - {5.8, 56.09483315}, - {5.81, 56.00482369}, - {5.82, 55.91519047}, - {5.83, 55.82648287}, - {5.84, 55.73685095}, - {5.85, 55.6481447}, - {5.86, 55.56036419}, - {5.87, 55.47171714}, - {5.88, 55.38338824}, - {5.89, 55.29560974}, - {5.9, 55.20783199}, - {5.91, 55.12048763}, - {5.92, 55.03271123}, - {5.93, 54.94623595}, - {5.94, 54.85889329}, - {5.95, 54.77241935}, - {5.96, 54.6856279}, - {5.97, 54.59953044}, - {5.98, 54.51274022}, - {5.99, 54.42756936}, - {6, 54.34208069}, - {6.01, 54.2565362}, - {6.02, 54.17142378}, - {6.03, 54.08588077}, - {6.04, 54.00169487}, - {6.05, 53.91615335}, - {6.06, 53.8319687}, - {6.07, 53.74778462}, - {6.08, 53.66317074}, - {6.09, 53.57991343}, - {6.1, 53.49573124}, - {6.11, 53.41247515}, - {6.12, 53.32823895}, - {6.13, 53.24628434}, - {6.14, 53.16303021}, - {6.15, 53.07977671}, - {6.16, 52.99782396}, - {6.17, 52.91457174}, - {6.18, 52.83354588}, - {6.19, 52.75252068}, - {6.2, 52.67149614}, - {6.21, 52.59177232}, - {6.22, 52.51172899}, - {6.23, 52.43200639}, - {6.24, 52.35191018}, - {6.25, 52.27311468}, - {6.26, 52.19344784}, - {6.27, 52.11427943}, - {6.28, 52.03461365}, - {6.29, 51.95582072}, - {6.3, 51.8770285}, - {6.31, 51.7992161}, - {6.32, 51.72042521}, - {6.33, 51.64298797}, - {6.34, 51.56512432}, - {6.35, 51.4863883}, - {6.36, 51.40889994}, - {6.37, 51.33109103}, - {6.38, 51.25453}, - {6.39, 51.1767223}, - {6.4, 51.10021497}, - {6.41, 51.02365598}, - {6.42, 50.94677613}, - {6.43, 50.87027056}, - {6.44, 50.79371366}, - {6.45, 50.71813543}, - {6.46, 50.64163163}, - {6.47, 50.566003}, - {6.48, 50.49042663}, - {6.49, 50.41522435}, - {6.5, 50.33964914}, - {6.51, 50.26407455}, - {6.52, 50.18974918}, - {6.53, 50.11510217}, - {6.54, 50.04082915}, - {6.55, 49.96525709}, - {6.56, 49.8919116}, - {6.57, 49.81764038}, - {6.58, 49.74336976}, - {6.59, 49.67002613}, - {6.6, 49.59575673}, - {6.61, 49.52241437}, - {6.62, 49.44907265}, - {6.63, 49.37665805}, - {6.64, 49.30331765}, - {6.65, 49.2299779}, - {6.66, 49.15793829}, - {6.67, 49.08552639}, - {6.68, 49.01218863}, - {6.69, 48.94015097}, - {6.7, 48.86779023}, - {6.71, 48.7966804}, - {6.72, 48.72464462}, - {6.73, 48.65353614}, - {6.74, 48.58205566}, - {6.75, 48.51007046}, - {6.76, 48.43896396}, - {6.77, 48.3687849}, - {6.78, 48.29767983}, - {6.79, 48.22662358}, - {6.8, 48.1568191}, - {6.81, 48.08669074}, - {6.82, 48.01558828}, - {6.83, 47.94541339}, - {6.84, 47.8756592}, - {6.85, 47.80641256}, - {6.86, 47.73665949}, - {6.87, 47.66778656}, - {6.88, 47.59858928}, - {6.89, 47.5287908}, - {6.9, 47.45992002}, - {6.91, 47.39072463}, - {6.92, 47.32282897}, - {6.93, 47.25396027}, - {6.94, 47.18513879}, - {6.95, 47.11719857}, - {6.96, 47.04967737}, - {6.97, 46.98178475}, - {6.98, 46.91384668}, - {6.99, 46.84595533}, - {7, 46.77839063}, - {7.01, 46.7114277}, - {7.02, 46.64353826}, - {7.03, 46.57694835}, - {7.04, 46.51031367}, - {7.05, 46.44335335}, - {7.06, 46.37676531}, - {7.07, 46.31017787}, - {7.08, 46.24414673}, - {7.09, 46.17751571}, - {7.1, 46.11185753}, - {7.11, 46.04619998}, - {7.12, 45.98054307}, - {7.13, 45.91395938}, - {7.14, 45.84960253}, - {7.15, 45.78394749}, - {7.16, 45.7182931}, - {7.17, 45.65356684}, - {7.18, 45.58921251}, - {7.19, 45.52356007}, - {7.2, 45.45883582}, - {7.21, 45.39448341}, - {7.22, 45.33013163}, - {7.23, 45.26633701}, - {7.24, 45.20198655}, - {7.25, 45.13856437}, - {7.26, 45.07477189}, - {7.27, 45.01042343}, - {7.28, 44.9470033}, - {7.29, 44.88358387}, - {7.3, 44.82016513}, - {7.31, 44.75771711}, - {7.32, 44.69429966}, - {7.33, 44.63218143}, - {7.34, 44.56969317}, - {7.35, 44.50627783}, - {7.36, 44.44383274}, - {7.37, 44.38171718}, - {7.38, 44.31923174}, - {7.39, 44.25808684}, - {7.4, 44.19560278}, - {7.41, 44.13441787}, - {7.42, 44.07197633}, - {7.43, 44.01079279}, - {7.44, 43.94960999}, - {7.45, 43.88809827}, - {7.46, 43.82691687}, - {7.47, 43.76577664}, - {7.48, 43.70552469}, - {7.49, 43.64434543}, - {7.5, 43.58413512}, - {7.51, 43.52295728}, - {7.52, 43.46274819}, - {7.53, 43.40286997}, - {7.54, 43.3426621}, - {7.55, 43.28241531}, - {7.56, 43.22350686}, - {7.57, 43.16330081}, - {7.58, 43.10398447}, - {7.59, 43.04414954}, - {7.6, 42.98483474}, - {7.61, 42.92592934}, - {7.62, 42.86702451}, - {7.63, 42.80771199}, - {7.64, 42.74880843}, - {7.65, 42.68990543}, - {7.66, 42.63059523}, - {7.67, 42.57169353}, - {7.68, 42.51145649}, - {7.69, 42.45122031}, - {7.7, 42.39102268}, - {7.71, 42.332086}, - {7.72, 42.27281824}, - {7.73, 42.21295458}, - {7.74, 42.1536882}, - {7.75, 42.09475466}, - {7.76, 42.03678752}, - {7.77, 41.97748641}, - {7.78, 41.91859202}, - {7.79, 41.86059036}, - {7.8, 41.80169732}, - {7.81, 41.74369731}, - {7.82, 41.68573437}, - {7.83, 41.62684333}, - {7.84, 41.56884576}, - {7.85, 41.51181369}, - {7.86, 41.45381785}, - {7.87, 41.39585845}, - {7.88, 41.33919733}, - {7.89, 41.28216814}, - {7.9, 41.2241756}, - {7.91, 41.16751658}, - {7.92, 41.11141854}, - {7.93, 41.05379713}, - {7.94, 40.99770067}, - {7.95, 40.94104445}, - {7.96, 40.88494948}, - {7.97, 40.82829464}, - {7.98, 40.77256952}, - {7.99, 40.7168111}, - {8, 40.65979014}, - {8.01, 40.6049964}, - {8.02, 40.5492742}, - {8.03, 40.49355288}, - {8.04, 40.4378322}, - {8.05, 40.38304144}, - {8.06, 40.32769028}, - {8.07, 40.27290099}, - {8.08, 40.21811247}, - {8.09, 40.16332474}, - {8.1, 40.10890567}, - {8.11, 40.05411945}, - {8.12, 39.99933402}, - {8.13, 39.94491715}, - {8.14, 39.89106263}, - {8.15, 39.83664725}, - {8.16, 39.78279435}, - {8.17, 39.7293099}, - {8.18, 39.67582622}, - {8.19, 39.62197577}, - {8.2, 39.56852494}, - {8.21, 39.51504348}, - {8.22, 39.46156279}, - {8.23, 39.40901245}, - {8.24, 39.35553335}, - {8.25, 39.30301525}, - {8.26, 39.24953762}, - {8.27, 39.19699041}, - {8.28, 39.14444404}, - {8.29, 39.0919286}, - {8.3, 39.04068058}, - {8.31, 38.98813658}, - {8.32, 38.93562314}, - {8.33, 38.88344769}, - {8.34, 38.83183584}, - {8.35, 38.78062111}, - {8.36, 38.7290109}, - {8.37, 38.67683857}, - {8.38, 38.62525902}, - {8.39, 38.57494795}, - {8.4, 38.5237365}, - {8.41, 38.47213045}, - {8.42, 38.42089193}, - {8.43, 38.37061247}, - {8.44, 38.31937553}, - {8.45, 38.26909742}, - {8.46, 38.21879208}, - {8.47, 38.16721889}, - {8.48, 38.11821167}, - {8.49, 38.06700627}, - {8.5, 38.01766165}, - {8.51, 37.9673604}, - {8.52, 37.91708712}, - {8.53, 37.86771773}, - {8.54, 37.81744589}, - {8.55, 37.76844436}, - {8.56, 37.71910415}, - {8.57, 37.67013066}, - {8.58, 37.61983531}, - {8.59, 37.57179349}, - {8.6, 37.52245614}, - {8.61, 37.47345951}, - {8.62, 37.42412368}, - {8.63, 37.37608468}, - {8.64, 37.32711599}, - {8.65, 37.27868733}, - {8.66, 37.2297201}, - {8.67, 37.18168396}, - {8.68, 37.1336485}, - {8.69, 37.08558897}, - {8.7, 37.03755502}, - {8.71, 36.98915633}, - {8.72, 36.94241975}, - {8.73, 36.89438792}, - {8.74, 36.84635679}, - {8.75, 36.79923301}, - {8.76, 36.75120345}, - {8.77, 36.70410523}, - {8.78, 36.65700777}, - {8.79, 36.61027618}, - {8.8, 36.5631802}, - {8.81, 36.51608498}, - {8.82, 36.46935551}, - {8.83, 36.42226178}, - {8.84, 36.37646449}, - {8.85, 36.32937229}, - {8.86, 36.28357649}, - {8.87, 36.23648583}, - {8.88, 36.19069152}, - {8.89, 36.14489797}, - {8.9, 36.0987405}, - {8.91, 36.05294848}, - {8.92, 36.00715721}, - {8.93, 35.96136669}, - {8.94, 35.91557693}, - {8.95, 35.86978793}, - {8.96, 35.82493069}, - {8.97, 35.77916419}, - {8.98, 35.7343084}, - {8.99, 35.68852237}, - {9, 35.64366819}, - {9.01, 35.5988148}, - {9.02, 35.55396228}, - {9.03, 35.50913068}, - {9.04, 35.46427965}, - {9.05, 35.41979346}, - {9.06, 35.37587524}, - {9.07, 35.33102668}, - {9.08, 35.28749351}, - {9.09, 35.24264645}, - {9.1, 35.19909533}, - {9.11, 35.15424994}, - {9.12, 35.11071932}, - {9.13, 35.0668068}, - {9.14, 35.02325881}, - {9.15, 34.9793665}, - {9.16, 34.93582004}, - {9.17, 34.8922744}, - {9.18, 34.84838415}, - {9.19, 34.8057715}, - {9.2, 34.76222825}, - {9.21, 34.71963493}, - {9.22, 34.67609322}, - {9.23, 34.63350121}, - {9.24, 34.59089259}, - {9.25, 34.54830191}, - {9.26, 34.50476332}, - {9.27, 34.46215719}, - {9.28, 34.4205002}, - {9.29, 34.37789572}, - {9.3, 34.33530849}, - {9.31, 34.29270565}, - {9.32, 34.25141436}, - {9.33, 34.20881316}, - {9.34, 34.1671604}, - {9.35, 34.12549266}, - {9.36, 34.08327228}, - {9.37, 34.0416216}, - {9.38, 34.00031895}, - {9.39, 33.95866972}, - {9.4, 33.91736872}, - {9.41, 33.87572094}, - {9.42, 33.8344216}, - {9.43, 33.79277528}, - {9.44, 33.75242393}, - {9.45, 33.71112702}, - {9.46, 33.67041482}, - {9.47, 33.62913357}, - {9.48, 33.58877113}, - {9.49, 33.54749128}, - {9.5, 33.506782}, - {9.51, 33.4664221}, - {9.52, 33.42607642}, - {9.53, 33.38349213}, - {9.54, 33.34185389}, - {9.55, 33.29963327}, - {9.56, 33.2579839}, - {9.57, 33.21541585}, - {9.58, 33.17376836}, - {9.59, 33.13248356}, - {9.6, 33.09085011}, - {9.61, 33.04920553}, - {9.62, 33.0079235}, - {9.63, 32.96629251}, - {9.64, 32.92501229}, - {9.65, 32.88337165}, - {9.66, 32.84303684}, - {9.67, 32.80139814}, - {9.68, 32.76106491}, - {9.69, 32.71978933}, - {9.7, 32.67909651}, - {9.71, 32.63875528}, - {9.72, 32.59841507}, - {9.73, 32.55808586}, - {9.74, 32.51738663}, - {9.75, 32.47705906}, - {9.76, 32.43673222}, - {9.77, 32.39639674}, - {9.78, 32.35700414}, - {9.79, 32.31667062}, - {9.8, 32.27634704}, - {9.81, 32.23694815}, - {9.82, 32.19755893}, - {9.83, 32.15722936}, - {9.84, 32.11820228}, - {9.85, 32.07881552}, - {9.86, 32.03942161}, - {9.87, 32.00003659}, - {9.88, 31.96101262}, - {9.89, 31.9216217}, - {9.9, 31.88317208}, - {9.91, 31.84415051}, - {9.92, 31.80476263}, - {9.93, 31.76667564}, - {9.94, 31.72729651}, - {9.95, 31.68920457}, - {9.96, 31.65076021}, - {9.97, 31.61267652}, - {9.98, 31.57459362}, - {9.99, 31.53521874}, - {10, 31.49713169}, - {10.01, 31.4599844}, - {10.02, 31.42190484}, - {10.03, 31.38346657}, - {10.04, 31.34538864}, - {10.05, 31.30823975}, - {10.06, 31.27016359}, - {10.07, 31.23302145}, - {10.08, 31.19494693}, - {10.09, 31.15780647}, - {10.1, 31.12066686}, - {10.11, 31.08352811}, - {10.12, 31.04639022}, - {10.13, 31.00924954}, - {10.14, 30.97211354}, - {10.15, 30.9353373}, - {10.16, 30.89820299}, - {10.17, 30.86106955}, - {10.18, 30.82487043}, - {10.19, 30.78809747}, - {10.2, 30.75096665}, - {10.21, 30.71477021}, - {10.22, 30.67893324}, - {10.23, 30.64180509}, - {10.24, 30.60596983}, - {10.25, 30.569777}, - {10.26, 30.53394347}, - {10.27, 30.49775249}, - {10.28, 30.4619207}, - {10.29, 30.42608978}, - {10.3, 30.38990248}, - {10.31, 30.35407315}, - {10.32, 30.31917839}, - {10.33, 30.2829928}, - {10.34, 30.24809987}, - {10.35, 30.2122741}, - {10.36, 30.17738297}, - {10.37, 30.14155897}, - {10.38, 30.10631156}, - {10.39, 30.07142306}, - {10.4, 30.03653549}, - {10.41, 30.00164884}, - {10.42, 29.96676312}, - {10.43, 29.93187719}, - {10.44, 29.89699318}, - {10.45, 29.86211011}, - {10.46, 29.82758532}, - {10.47, 29.79363808}, - {10.48, 29.75875585}, - {10.49, 29.72387638}, - {10.5, 29.68993191}, - {10.51, 29.65634308}, - {10.52, 29.6214663}, - {10.53, 29.5875246}, - {10.54, 29.55358391}, - {10.55, 29.519998}, - {10.56, 29.48512494}, - {10.57, 29.45154383}, - {10.58, 29.41760332}, - {10.59, 29.38402392}, - {10.6, 29.35102307}, - {10.61, 29.31744152}, - {10.62, 29.28350828}, - {10.63, 29.25086245}, - {10.64, 29.21693108}, - {10.65, 29.18335712}, - {10.66, 29.15071358}, - {10.67, 29.11678509}, - {10.68, 29.08414305}, - {10.69, 29.05021647}, - {10.7, 29.01758148}, - {10.71, 28.98400733}, - {10.72, 28.95137415}, - {10.73, 28.91837999}, - {10.74, 28.88481418}, - {10.75, 28.85218387}, - {10.76, 28.81954804}, - {10.77, 28.78691958}, - {10.78, 28.75428528}, - {10.79, 28.72130305}, - {10.8, 28.68960496}, - {10.81, 28.65698032}, - {10.82, 28.62434918}, - {10.83, 28.5926611}, - {10.84, 28.56003155}, - {10.85, 28.52741071}, - {10.86, 28.49607273}, - {10.87, 28.46438852}, - {10.88, 28.43176212}, - {10.89, 28.40007989}, - {10.9, 28.36838991}, - {10.91, 28.33670971}, - {10.92, 28.30537634}, - {10.93, 28.27368876}, - {10.94, 28.24201145}, - {10.95, 28.21068035}, - {10.96, 28.17900504}, - {10.97, 28.14732081}, - {10.98, 28.115992}, - {10.99, 28.0852546}, - {11, 28.05392737}, - {11.01, 28.02224636}, - {11.02, 27.99186643}, - {11.03, 27.96018712}, - {11.04, 27.92979799}, - {11.05, 27.89906669}, - {11.06, 27.86774413}, - {11.07, 27.8373574}, - {11.08, 27.80662916}, - {11.09, 27.77624411}, - {11.1, 27.74585982}, - {11.11, 27.71512233}, - {11.12, 27.6847521}, - {11.13, 27.65437027}, - {11.14, 27.62398919}, - {11.15, 27.59325515}, - {11.16, 27.56382412}, - {11.17, 27.53344555}, - {11.18, 27.50306776}, - {11.19, 27.47362606}, - {11.2, 27.44324983}, - {11.21, 27.41382376}, - {11.22, 27.38438464}, - {11.23, 27.35401093}, - {11.24, 27.32457343}, - {11.25, 27.29513675}, - {11.26, 27.26570089}, - {11.27, 27.23626586}, - {11.28, 27.20719987}, - {11.29, 27.17776661}, - {11.3, 27.14833418}, - {11.31, 27.1189026}, - {11.32, 27.08947185}, - {11.33, 27.06039469}, - {11.34, 27.03096558}, - {11.35, 27.00247292}, - {11.36, 26.97304553}, - {11.37, 26.94397151}, - {11.38, 26.91548144}, - {11.39, 26.88642605}, - {11.4, 26.85793786}, - {11.41, 26.82945057}, - {11.42, 26.80038071}, - {11.43, 26.77189515}, - {11.44, 26.74376266}, - {11.45, 26.71527887}, - {11.46, 26.68714803}, - {11.47, 26.65901801}, - {11.48, 26.63053685}, - {11.49, 26.6024085}, - {11.5, 26.57391023}, - {11.51, 26.54578339}, - {11.52, 26.51765738}, - {11.53, 26.4895322}, - {11.54, 26.46199216}, - {11.55, 26.43386869}, - {11.56, 26.40574606}, - {11.57, 26.37856025}, - {11.58, 26.35008791}, - {11.59, 26.32290389}, - {11.6, 26.29572074}, - {11.61, 26.2676024}, - {11.62, 26.24040005}, - {11.63, 26.21321936}, - {11.64, 26.18510344}, - {11.65, 26.1579245}, - {11.66, 26.13074645}, - {11.67, 26.1035693}, - {11.68, 26.07639304}, - {11.69, 26.04919555}, - {11.7, 26.02202092}, - {11.71, 25.99578343}, - {11.72, 25.96861063}, - {11.73, 25.94143874}, - {11.74, 25.91459531}, - {11.75, 25.8883613}, - {11.76, 25.86119195}, - {11.77, 25.83402352}, - {11.78, 25.80779235}, - {11.79, 25.78095219}, - {11.8, 25.75472269}, - {11.81, 25.72849417}, - {11.82, 25.70168028}, - {11.83, 25.67542909}, - {11.84, 25.64920322}, - {11.85, 25.62332824}, - {11.86, 25.59614278}, - {11.87, 25.5702694}, - {11.88, 25.54404713}, - {11.89, 25.51780034}, - {11.9, 25.49192951}, - {11.91, 25.46570998}, - {11.92, 25.4407515}, - {11.93, 25.41488322}, - {11.94, 25.38866641}, - {11.95, 25.36277341}, - {11.96, 25.33655837}, - {11.97, 25.30721209}, - {11.98, 25.27786728}, - {11.99, 25.24849653}, - {12, 25.21915441}, - {12.01, 25.1898137}, - {12.02, 25.16047458}, - {12.03, 25.13204538}, - {12.04, 25.10270896}, - {12.05, 25.07337403}, - {12.06, 25.04494849}, - {12.07, 25.01561634}, - {12.08, 24.9862857}, - {12.09, 24.95821224}, - {12.1, 24.92982124}, - {12.11, 24.90049487}, - {12.12, 24.87207673}, - {12.13, 24.84403826}, - {12.14, 24.81562252}, - {12.15, 24.78723881}, - {12.16, 24.75917343}, - {12.17, 24.73079259}, - {12.18, 24.70241334}, - {12.19, 24.67435154}, - {12.2, 24.64597519}, - {12.21, 24.61885284}, - {12.22, 24.59047942}, - {12.23, 24.56335946}, - {12.24, 24.53533627}, - {12.25, 24.50787145}, - {12.26, 24.47981777}, - {12.27, 24.4527359}, - {12.28, 24.42527482}, - {12.29, 24.39725858}, - {12.3, 24.37014694}, - {12.31, 24.34307082}, - {12.32, 24.3156149}, - {12.33, 24.28850687}, - {12.34, 24.26143502}, - {12.35, 24.23432943}, - {12.36, 24.20726048}, - {12.37, 24.18109478}, - {12.38, 24.15399282}, - {12.39, 24.12692815}, - {12.4, 24.10076615}, - {12.41, 24.07366783}, - {12.42, 24.04754501}, - {12.43, 24.02044923}, - {12.44, 23.99429215}, - {12.45, 23.96817377}, - {12.46, 23.94108172}, - {12.47, 23.91492845}, - {12.48, 23.88912194}, - {12.49, 23.86300939}, - {12.5, 23.8368599}, - {12.51, 23.81071164}, - {12.52, 23.78490986}, - {12.53, 23.75876398}, - {12.54, 23.73265881}, - {12.55, 23.7077984}, - {12.56, 23.6816564}, - {12.57, 23.65551566}, - {12.58, 23.62972097}, - {12.59, 23.60452048}, - {12.6, 23.57876888}, - {12.61, 23.55357111}, - {12.62, 23.52871915}, - {12.63, 23.50258603}, - {12.64, 23.47773645}, - {12.65, 23.45288805}, - {12.66, 23.42675865}, - {12.67, 23.40191266}, - {12.68, 23.37706784}, - {12.69, 23.3518802}, - {12.7, 23.32708081}, - {12.71, 23.29967609}, - {12.72, 23.27266003}, - {12.73, 23.24564574}, - {12.74, 23.21863323}, - {12.75, 23.19251634}, - {12.76, 23.16516372}, - {12.77, 23.13909454}, - {12.78, 23.112044}, - {12.79, 23.08597821}, - {12.8, 23.05897606}, - {12.81, 23.03286817}, - {12.82, 23.00680764}, - {12.83, 22.97976445}, - {12.84, 22.95370737}, - {12.85, 22.92799483}, - {12.86, 22.90189438}, - {12.87, 22.87584259}, - {12.88, 22.84974514}, - {12.89, 22.82369687}, - {12.9, 22.79854085}, - {12.91, 22.77283829}, - {12.92, 22.74674687}, - {12.93, 22.72164409}, - {12.94, 22.69589765}, - {12.95, 22.67079842}, - {12.96, 22.64471311}, - {12.97, 22.6199592}, - {12.98, 22.59387694}, - {12.99, 22.56907614}, - {13, 22.54398583}, - {13.01, 22.519188}, - {13.02, 22.49410134}, - {13.03, 22.46836788}, - {13.04, 22.44417194}, - {13.05, 22.41943166}, - {13.06, 22.39464117}, - {13.07, 22.36951124}, - {13.08, 22.3447761}, - {13.09, 22.31999001}, - {13.1, 22.29580348}, - {13.11, 22.27107355}, - {13.12, 22.24629192}, - {13.13, 22.22245045}, - {13.14, 22.19772569}, - {13.15, 22.1738873}, - {13.16, 22.14971031}, - {13.17, 22.12493602}, - {13.18, 22.1011571}, - {13.19, 22.07732477}, - {13.2, 22.0534939}, - {13.21, 22.02966448}, - {13.22, 22.00589262}, - {13.23, 21.98206637}, - {13.24, 21.9582416}, - {13.25, 21.9344183}, - {13.26, 21.91093565}, - {13.27, 21.88711523}, - {13.28, 21.86429292}, - {13.29, 21.84047575}, - {13.3, 21.81666008}, - {13.31, 21.79412373}, - {13.32, 21.770311}, - {13.33, 21.74743888}, - {13.34, 21.72362921}, - {13.35, 21.70109859}, - {13.36, 21.67823104}, - {13.37, 21.6544259}, - {13.38, 21.63195974}, - {13.39, 21.60909704}, - {13.4, 21.58657391}, - {13.41, 21.56371429}, - {13.42, 21.54119407}, - {13.43, 21.51833755}, - {13.44, 21.49582027}, - {13.45, 21.47296687}, - {13.46, 21.45045254}, - {13.47, 21.42760227}, - {13.48, 21.40509091}, - {13.49, 21.38352034}, - {13.5, 21.36067478}, - {13.51, 21.33810451}, - {13.52, 21.31532552}, - {13.53, 21.29060603}, - {13.54, 21.26689173}, - {13.55, 21.24311512}, - {13.56, 21.22034434}, - {13.57, 21.19657128}, - {13.58, 21.17286521}, - {13.59, 21.1490957}, - {13.6, 21.12539377}, - {13.61, 21.10256736}, - {13.62, 21.07886961}, - {13.63, 21.05510731}, - {13.64, 21.03228625}, - {13.65, 21.00859467}, - {13.66, 20.98611288}, - {13.67, 20.96329714}, - {13.68, 20.94055131}, - {13.69, 20.91679968}, - {13.7, 20.89432461}, - {13.71, 20.87158501}, - {13.72, 20.84877843}, - {13.73, 20.82630851}, - {13.74, 20.80357522}, - {13.75, 20.78110883}, - {13.76, 20.7583094}, - {13.77, 20.73551176}, - {13.78, 20.71312132}, - {13.79, 20.69160153}, - {13.8, 20.66880938}, - {13.81, 20.64635316}, - {13.82, 20.62457646}, - {13.83, 20.60212387}, - {13.84, 20.58027891}, - {13.85, 20.5578297}, - {13.86, 20.53632199}, - {13.87, 20.51455595}, - {13.88, 20.49305197}, - {13.89, 20.47060979}, - {13.9, 20.44910917}, - {13.91, 20.42727711}, - {13.92, 20.40577999}, - {13.93, 20.38428458}, - {13.94, 20.36279089}, - {13.95, 20.34137471}, - {13.96, 20.31988474}, - {13.97, 20.29839651}, - {13.98, 20.27785001}, - {13.99, 20.25636527}, - {14, 20.23488228}, - {14.01, 20.2134010}, - {14.02, 20.19286161}, - {14.03, 20.17138391}, - {14.04, 20.15084805}, - {14.05, 20.12937391}, - {14.06, 20.10884164}, - {14.07, 20.08831118}, - {14.08, 20.06684243}, - {14.09, 20.0463156}, - {14.1, 20.02612189}, - {14.11, 20.00559862}, - {14.12, 19.98413703}, - {14.13, 19.96361743}, - {14.14, 19.94334949}, - {14.15, 19.92283321}, - {14.16, 19.90231878}, - {14.17, 19.88213686}, - {14.18, 19.86256627}, - {14.19, 19.84205738}, - {14.2, 19.82188069}, - {14.21, 19.80137546}, - {14.22, 19.78205917}, - {14.23, 19.76155735}, - {14.24, 19.74138739}, - {14.25, 19.72182956}, - {14.26, 19.70166313}, - {14.27, 19.68210906}, - {14.28, 19.66280148}, - {14.29, 19.64231061}, - {14.3, 19.62309135}, - {14.31, 19.60387385}, - {14.32, 19.5833025}, - {14.33, 19.56408835}, - {14.34, 19.54487598}, - {14.35, 19.52533651}, - {14.36, 19.50604058}, - {14.37, 19.48683338}, - {14.38, 19.4672994}, - {14.39, 19.4480959}, - {14.4, 19.42880586}, - {14.41, 19.40960569}, - {14.42, 19.39040734}, - {14.43, 19.36994231}, - {14.44, 19.35041974}, - {14.45, 19.33028662}, - {14.46, 19.31076803}, - {14.47, 19.29157915}, - {14.48, 19.2711241}, - {14.49, 19.25184804}, - {14.5, 19.23266453}, - {14.51, 19.21221515}, - {14.52, 19.19303548}, - {14.53, 19.1738577}, - {14.54, 19.15435486}, - {14.55, 19.13518095}, - {14.56, 19.11600895}, - {14.57, 19.09651222}, - {14.58, 19.07725048}, - {14.59, 19.05808401}, - {14.6, 19.03986}, - {14.61, 19.020371}, - {14.62, 19.00121059}, - {14.63, 18.98195683}, - {14.64, 18.9637404}, - {14.65, 18.94458535}, - {14.66, 18.92543225}, - {14.67, 18.90722172}, - {14.68, 18.88797596}, - {14.69, 18.86976908}, - {14.7, 18.85156418}, - {14.71, 18.83242066}, - {14.72, 18.81412198}, - {14.73, 18.79592278}, - {14.74, 18.77678495}, - {14.75, 18.75849112}, - {14.76, 18.74029765}, - {14.77, 18.72210621}, - {14.78, 18.70391681}, - {14.79, 18.68562961}, - {14.8, 18.66744399}, - {14.81, 18.64958453}, - {14.82, 18.63130218}, - {14.83, 18.61312231}, - {14.84, 18.59484321}, - {14.85, 18.57760787}, - {14.86, 18.55975749}, - {14.87, 18.54148333}, - {14.88, 18.52331312}, - {14.89, 18.506409}, - {14.9, 18.48813982}, - {14.91, 18.47091622}, - {14.92, 18.45297334}, - {14.93, 18.43575355}, - {14.94, 18.41749095}, - {14.95, 18.40059776}, - {14.96, 18.38233843}, - {14.97, 18.36544889}, - {14.98, 18.34823896}, - {14.99, 18.33030698}, - {15, 18.31310094}, - {15.01, 18.2961128}, - {15.02, 18.27891069}, - {15.03, 18.26192569}, - {15.04, 18.24504922}, - {15.05, 18.22774566}, - {15.06, 18.21076522}, - {15.07, 18.19357277}, - {15.08, 18.1765955}, - {15.09, 18.15972822}, - {15.1, 18.14243297}, - {15.11, 18.12556948}, - {15.12, 18.10953931}, - {15.13, 18.0925698}, - {15.14, 18.07539106}, - {15.15, 18.05936558}, - {15.16, 18.04251136}, - {15.17, 18.02554826}, - {15.18, 18.0095274}, - {15.19, 17.99235852}, - {15.2, 17.97634095}, - {15.21, 17.95938407}, - {15.22, 17.94348177}, - {15.23, 17.9274689}, - {15.24, 17.91062964}, - {15.25, 17.89462}, - {15.26, 17.8786118}, - {15.27, 17.86166442}, - {15.28, 17.84577334}, - {15.29, 17.82976996}, - {15.3, 17.8137681}, - {15.31, 17.79788268}, - {15.32, 17.78188412}, - {15.33, 17.76588709}, - {15.34, 17.75000738}, - {15.35, 17.73401367}, - {15.36, 17.7180215}, - {15.37, 17.70203087}, - {15.38, 17.68615874}, - {15.39, 17.67048955}, - {15.4, 17.65544451}, - {15.41, 17.63946025}, - {15.42, 17.62347755}, - {15.43, 17.6076148}, - {15.44, 17.59289385}, - {15.45, 17.57691603}, - {15.46, 17.56093978}, - {15.47, 17.5459059}, - {15.48, 17.53036979}, - {15.49, 17.51533925}, - {15.5, 17.49936953}, - {15.51, 17.48465902}, - {15.52, 17.46963317}, - {15.53, 17.45366815}, - {15.54, 17.43896204}, - {15.55, 17.42312196}, - {15.56, 17.40810274}, - {15.57, 17.39340133}, - {15.58, 17.37838525}, - {15.59, 17.36368679}, - {15.6, 17.34773311}, - {15.61, 17.33303762}, - {15.62, 17.31802786}, - {15.63, 17.30333534}, - {15.64, 17.28832878}, - {15.65, 17.27363925}, - {15.66, 17.25876085}, - {15.67, 17.24407461}, - {15.68, 17.22907476}, - {15.69, 17.21533227}, - {15.7, 17.20065051}, - {15.71, 17.18565547}, - {15.72, 17.17097676}, - {15.73, 17.15629954}, - {15.74, 17.14225006}, - {15.75, 17.12757589}, - {15.76, 17.11290323}, - {15.77, 17.09885862}, - {15.78, 17.08418904}, - {15.79, 17.06952097}, - {15.8, 17.05548126}, - {15.81, 17.0408163}, - {15.82, 17.02709353}, - {15.83, 17.01337227}, - {15.84, 16.99858182}, - {15.85, 16.98455001}, - {15.86, 16.96989246}, - {15.87, 16.9561771}, - {15.88, 16.94246328}, - {15.89, 16.92781034}, - {15.9, 16.91409959}, - {15.91, 16.90039038}, - {15.92, 16.88637013}, - {15.93, 16.8726641}, - {15.94, 16.85895964}, - {15.95, 16.8443161}, - {15.96, 16.83048134}, - {15.97, 16.81678126}, - {15.98, 16.80308275}, - {15.99, 16.78938581}, - {16, 16.77569045}, - {16.01, 16.7619966}, - {16.02, 16.74924503}, - {16.03, 16.73541903}, - {16.04, 16.7217297}, - {16.05, 16.70804196}, - {16.06, 16.69435581}, - {16.07, 16.68098232}, - {16.08, 16.66810304}, - {16.09, 16.6544213}, - {16.1, 16.64074118}, - {16.11, 16.62800319}, - {16.12, 16.61432629}, - {16.13, 16.60051285}, - {16.14, 16.58808973}, - {16.15, 16.5744173}, - {16.16, 16.56168699}, - {16.17, 16.54787856}, - {16.18, 16.53515122}, - {16.19, 16.52179485}, - {16.2, 16.50907066}, - {16.21, 16.49620775}, - {16.22, 16.48285561}, - {16.23, 16.47013594}, - {16.24, 16.45741793}, - {16.25, 16.44361967}, - {16.26, 16.43121377}, - {16.27, 16.41850033}, - {16.28, 16.40564625}, - {16.29, 16.39230423}, - {16.3, 16.37959539}, - {16.31, 16.3671968}, - {16.32, 16.35434777}, - {16.33, 16.34164355}, - {16.34, 16.32924928}, - {16.35, 16.31640408}, - {16.36, 16.30401257}, - {16.37, 16.29131459}, - {16.38, 16.2787811}, - {16.39, 16.2660861}, - {16.4, 16.25355487}, - {16.41, 16.24086288}, - {16.42, 16.22848006}, - {16.43, 16.21658521}, - {16.44, 16.20420519}, - {16.45, 16.19151954}, - {16.46, 16.1789954}, - {16.47, 16.16725301}, - {16.48, 16.15473116}, - {16.49, 16.14235835}, - {16.5, 16.12968041}, - {16.51, 16.11810231}, - {16.52, 16.10573389}, - {16.53, 16.09385165}, - {16.54, 16.08148609}, - {16.55, 16.0696064}, - {16.56, 16.05724372}, - {16.57, 16.04582273}, - {16.58, 16.03331263}, - {16.59, 16.02158869}, - {16.6, 16.00908095}, - {16.61, 15.99766564}, - {16.62, 15.98610027}, - {16.63, 15.97453592}, - {16.64, 15.96391265}, - {16.65, 15.95250262}, - {16.66, 15.94094157}, - {16.67, 15.92938153}, - {16.68, 15.91797559}, - {16.69, 15.90641786}, - {16.7, 15.89580114}, - {16.71, 15.8843993}, - {16.72, 15.87284489}, - {16.73, 15.86129153}, - {16.74, 15.85067917}, - {16.75, 15.83928272}, - {16.76, 15.82803683}, - {16.77, 15.8174277}, - {16.78, 15.8058797}, - {16.79, 15.79542848}, - {16.8, 15.78388282}, - {16.81, 15.7732781}, - {16.82, 15.7620381}, - {16.83, 15.75159221}, - {16.84, 15.74005092}, - {16.85, 15.72945053}, - {16.86, 15.71791138}, - {16.87, 15.70777403}, - {16.88, 15.69623716}, - {16.89, 15.68564114}, - {16.9, 15.67504619}, - {16.91, 15.66381524}, - {16.92, 15.65338142}, - {16.93, 15.64278981}, - {16.94, 15.63219927}, - {16.95, 15.62097241}, - {16.96, 15.61038392}, - {16.97, 15.59995686}, - {16.98, 15.5896728}, - {16.99, 15.57908768}, - {17, 15.567564}, - {17.01, 15.55728}, - {17.02, 15.5467008}, - {17.03, 15.53628186}, - {17.04, 15.5260038}, - {17.05, 15.51542526}, - {17.06, 15.50484782}, - {17.07, 15.49457266}, - {17.08, 15.48399732}, - {17.09, 15.47372408}, - {17.1, 15.46331442}, - {17.11, 15.45304336}, - {17.12, 15.44247247}, - {17.13, 15.43220334}, - {17.14, 15.42163458}, - {17.15, 15.41200639}, - {17.16, 15.40174019}, - {17.17, 15.39117468}, - {17.18, 15.381076}, - {17.19, 15.37051287}, - {17.2, 15.36119022}, - {17.21, 15.35062924}, - {17.22, 15.34036916}, - {17.23, 15.33011005}, - {17.24, 15.32049157}, - {17.25, 15.31023442}, - {17.26, 15.29967879}, - {17.27, 15.29036291}, - {17.28, 15.27980946}, - {17.29, 15.27049554}, - {17.3, 15.26024331}, - {17.31, 15.24969308}, - {17.32, 15.24055107}, - {17.33, 15.23000328}, - {17.34, 15.2206945}, - {17.35, 15.2104475}, - {17.36, 15.20084211}, - {17.37, 15.19059712}, - {17.38, 15.18129224}, - {17.39, 15.17169008}, - {17.4, 15.16144808}, - {17.41, 15.15214615}, - {17.42, 15.14160817}, - {17.43, 15.13230825}, - {17.44, 15.12300929}, - {17.45, 15.11277228}, - {17.46, 15.10317768}, - {17.47, 15.09388171}, - {17.48, 15.0845867}, - {17.49, 15.07435372}, - {17.5, 15.06476346}, - {17.51, 15.05547146}, - {17.52, 15.04618045}, - {17.53, 15.0368904}, - {17.54, 15.02636563}, - {17.55, 15.01707765}, - {17.56, 15.00779064}, - {17.57, 14.99850463}, - {17.58, 14.98921959}, - {17.59, 14.97976001}, - {17.6, 14.97018043}, - {17.61, 14.9608982}, - {17.62, 14.95161697}, - {17.63, 14.94233673}, - {17.64, 14.93305749}, - {17.65, 14.92377925}, - {17.66, 14.914502}, - {17.67, 14.90522576}, - {17.68, 14.89565497}, - {17.69, 14.88638082}, - {17.7, 14.87710768}, - {17.71, 14.86783555}, - {17.72, 14.85950302}, - {17.73, 14.85005401}, - {17.74, 14.84078465}, - {17.75, 14.83151631}, - {17.76, 14.82224899}, - {17.77, 14.81392119}, - {17.78, 14.80465589}, - {17.79, 14.79539161}, - {17.8, 14.78612836}, - {17.81, 14.77780457}, - {17.82, 14.76836233}, - {17.83, 14.7591019}, - {17.84, 14.75078089}, - {17.85, 14.74152251}, - {17.86, 14.73226517}, - {17.87, 14.72394719}, - {17.88, 14.71469191}, - {17.89, 14.70619329}, - {17.9, 14.69693982}, - {17.91, 14.68768739}, - {17.92, 14.67937427}, - {17.93, 14.67041721}, - {17.94, 14.66192218}, - {17.95, 14.65361177}, - {17.96, 14.64559541}, - {17.97, 14.63710245}, - {17.98, 14.62879476}, - {17.99, 14.62078086}, - {18, 14.61228996}, - {18.01, 14.603985}, - {18.02, 14.59578802}, - {18.03, 14.58748477}, - {18.04, 14.57918256}, - {18.05, 14.57098741}, - {18.06, 14.56268691}, - {18.07, 14.55449291}, - {18.08, 14.54619414}, - {18.09, 14.53789641}, - {18.1, 14.52970426}, - {18.11, 14.52234619}, - {18.12, 14.51415519}, - {18.13, 14.50586092}, - {18.14, 14.49785922}, - {18.15, 14.48937856}, - {18.16, 14.48108708}, - {18.17, 14.47383695}, - {18.18, 14.46554721}, - {18.19, 14.45736041}, - {18.2, 14.44907243}, - {18.21, 14.44107645}, - {18.22, 14.43353834}, - {18.23, 14.42554394}, - {18.24, 14.41706943}, - {18.25, 14.41001433}, - {18.26, 14.40154117}, - {18.27, 14.39354998}, - {18.28, 14.38601581}, - {18.29, 14.37802622}, - {18.3, 14.36974732}, - {18.31, 14.36250539}, - {18.32, 14.35451831}, - {18.33, 14.34698749}, - {18.34, 14.33900202}, - {18.35, 14.33053502}, - {18.36, 14.32348868}, - {18.37, 14.31502305}, - {18.38, 14.3079783}, - {18.39, 14.29980353}, - {18.4, 14.29247089}, - {18.41, 14.28429732}, - {18.42, 14.27696645}, - {18.43, 14.2687941}, - {18.44, 14.26175406}, - {18.45, 14.25329386}, - {18.46, 14.24625544}, - {18.47, 14.23902275}, - {18.48, 14.23075982}, - {18.49, 14.22352832}, - {18.5, 14.21555581}, - {18.51, 14.20803692}, - {18.52, 14.20100324}, - {18.53, 14.19283689}, - {18.54, 14.18551649}, - {18.55, 14.17828851}, - {18.56, 14.17032089}, - {18.57, 14.16280601}, - {18.58, 14.15577712}, - {18.59, 14.14855148}, - {18.6, 14.14029932}, - {18.61, 14.1330749}, - {18.62, 14.12585095}, - {18.63, 14.11882591}, - {18.64, 14.11131564}, - {18.65, 14.10335527}, - {18.66, 14.09613369}, - {18.67, 14.08911186}, - {18.68, 14.08160428}, - {18.69, 14.07458408}, - {18.7, 14.06736486}, - {18.71, 14.05940941}, - {18.72, 14.05190453}, - {18.73, 14.04468704}, - {18.74, 14.03767075}, - {18.75, 14.03045441}, - {18.76, 14.0234397}, - {18.77, 14.01593811}, - {18.78, 14.00892505}, - {18.79, 14.00171111}, - {18.8, 13.99449767}, - {18.81, 13.98748687}, - {18.82, 13.97998859}, - {18.83, 13.97297946}, - {18.84, 13.96576844}, - {18.85, 13.95855791}, - {18.86, 13.95155106}, - {18.87, 13.94434172}, - {18.88, 13.93733647}, - {18.89, 13.92984286}, - {18.9, 13.92263527}, - {18.91, 13.91656883}, - {18.92, 13.90936242}, - {18.93, 13.90236113}, - {18.94, 13.89515592}, - {18.95, 13.88795122}, - {18.96, 13.88095224}, - {18.97, 13.87374874}, - {18.98, 13.8671973}, - {18.99, 13.86020068}, - {19, 13.85299895}, - {19.01, 13.8460039}, - {19.02, 13.83973967}, - {19.03, 13.83253964}, - {19.04, 13.82554697}, - {19.05, 13.81834816}, - {19.06, 13.81208601}, - {19.07, 13.80509565}, - {19.08, 13.79789857}, - {19.09, 13.79163808}, - {19.1, 13.78465005}, - {19.11, 13.7774547}, - {19.12, 13.77140432}, - {19.13, 13.76421018}, - {19.14, 13.75701657}, - {19.15, 13.75096847}, - {19.16, 13.74377608}, - {19.17, 13.73658422}, - {19.18, 13.73053841}, - {19.19, 13.72334777}, - {19.2, 13.7170935}, - {19.21, 13.71011415}, - {19.22, 13.70386106}, - {19.23, 13.6966727}, - {19.24, 13.69042063}, - {19.25, 13.68344434}, - {19.26, 13.67719346}, - {19.27, 13.67000739}, - {19.28, 13.66396915}, - {19.29, 13.65678432}, - {19.3, 13.65053567}, - {19.31, 13.64356412}, - {19.32, 13.63759873}, - {19.33, 13.63041613}, - {19.34, 13.62438244}, - {19.35, 13.61813662}, - {19.36, 13.61095581}, - {19.37, 13.60471102}, - {19.38, 13.59774489}, - {19.39, 13.59150132}, - {19.4, 13.58525826}, - {19.41, 13.57857588}, - {19.42, 13.57233396}, - {19.43, 13.56609255}, - {19.44, 13.55891633}, - {19.45, 13.55289095}, - {19.46, 13.54665128}, - {19.47, 13.54069304}, - {19.48, 13.53445433}, - {19.49, 13.52821614}, - {19.5, 13.52225917}, - {19.51, 13.51623801}, - {19.52, 13.51000148}, - {19.53, 13.50404595}, - {19.54, 13.49781038}, - {19.55, 13.49157533}, - {19.56, 13.48534082}, - {19.57, 13.479387}, - {19.58, 13.47315345}, - {19.59, 13.46713797}, - {19.6, 13.46118561}, - {19.61, 13.45495373}, - {19.62, 13.44900223}, - {19.63, 13.44277133}, - {19.64, 13.43654096}, - {19.65, 13.43059075}, - {19.66, 13.42436136}, - {19.67, 13.41906737}, - {19.68, 13.41311843}, - {19.69, 13.40710987}, - {19.7, 13.40088269}, - {19.71, 13.39493524}, - {19.72, 13.38870905}, - {19.73, 13.38369717}, - {19.74, 13.37747196}, - {19.75, 13.37124729}, - {19.76, 13.36530198}, - {19.77, 13.3590783}, - {19.78, 13.35406848}, - {19.79, 13.3480669}, - {19.8, 13.34184492}, - {19.81, 13.33590196}, - {19.82, 13.32968098}, - {19.83, 13.32467339}, - {19.84, 13.3184534}, - {19.85, 13.31251216}, - {19.86, 13.30722762}, - {19.87, 13.30128723}, - {19.88, 13.29506924}, - {19.89, 13.28978619}, - {19.9, 13.28384711}, - {19.91, 13.27763066}, - {19.92, 13.27262679}, - {19.93, 13.26641135}, - {19.94, 13.26069779}, - {19.95, 13.2554178}, - {19.96, 13.2494815}, - {19.97, 13.24326825}, - {19.98, 13.23826703}, - {19.99, 13.23205479}, - {20, 13.2270544}, - {20.01, 13.220843}, - {20.02, 13.2149095}, - {20.03, 13.20963338}, - {20.04, 13.20370058}, - {20.05, 13.19842544}, - {20.06, 13.19249351}, - {20.07, 13.18721934}, - {20.08, 13.18128829}, - {20.09, 13.1760151}, - {20.1, 13.17008493}, - {20.11, 13.1650891}, - {20.12, 13.15888343}, - {20.13, 13.15388844}, - {20.14, 13.14768379}, - {20.15, 13.14268964}, - {20.16, 13.13648602}, - {20.17, 13.13149272}, - {20.18, 13.12529013}, - {20.19, 13.12029767}, - {20.2, 13.11437189}, - {20.21, 13.10910451}, - {20.22, 13.10411329}, - {20.23, 13.09791323}, - {20.24, 13.09292286}, - {20.25, 13.08699927}, - {20.26, 13.08173433}, - {20.27, 13.0767452}, - {20.28, 13.07054769}, - {20.29, 13.06555942}, - {20.3, 13.06057154}, - {20.31, 13.05437555}, - {20.32, 13.04938853}, - {20.33, 13.04412703}, - {20.34, 13.03820743}, - {20.35, 13.03322167}, - {20.36, 13.02796162}, - {20.37, 13.02204335}, - {20.38, 13.01705885}, - {20.39, 13.01180026}, - {20.4, 13.00588332}, - {20.41, 13.00090008}, - {20.42, 12.99564296}, - {20.43, 12.99066057}, - {20.44, 12.9847454}, - {20.45, 12.97948974}, - {20.46, 12.97450862}, - {20.47, 12.9695279}, - {20.48, 12.96427372}, - {20.49, 12.95836078}, - {20.5, 12.95338133}, - {20.51, 12.94812863}, - {20.52, 12.94315004}, - {20.53, 12.93817186}, - {20.54, 12.93292063}, - {20.55, 12.92701037}, - {20.56, 12.92203347}, - {20.57, 12.91678373}, - {20.58, 12.9118077}, - {20.59, 12.90683207}, - {20.6, 12.90185684}, - {20.61, 12.89660905}, - {20.62, 12.89046644}, - {20.63, 12.88549235}, - {20.64, 12.88051866}, - {20.65, 12.87527266}, - {20.66, 12.87029984}, - {20.67, 12.86532743}, - {20.68, 12.86008293}, - {20.69, 12.8551114}, - {20.7, 12.85014027}, - {20.71, 12.84516956}, - {20.72, 12.83992702}, - {20.73, 12.83495719}, - {20.74, 12.82998776}, - {20.75, 12.82501875}, - {20.76, 12.82005016}, - {20.77, 12.8145721}, - {20.78, 12.80960421}, - {20.79, 12.80463674}, - {20.8, 12.79966968}, - {20.81, 12.79443141}, - {20.82, 12.78946524}, - {20.83, 12.78449948}, - {20.84, 12.77953415}, - {20.85, 12.77456923}, - {20.86, 12.76933342}, - {20.87, 12.76436939}, - {20.88, 12.75940579}, - {20.89, 12.75420264}, - {20.9, 12.75017184}, - {20.91, 12.7452093}, - {20.92, 12.73997626}, - {20.93, 12.73501461}, - {20.94, 12.73005338}, - {20.95, 12.72509257}, - {20.96, 12.72013219}, - {20.97, 12.71517224}, - {20.98, 12.71063271}, - {20.99, 12.70567346}, - {21, 12.70071463}, - {21.01, 12.69575622}, - {21.02, 12.6907982}, - {21.03, 12.68677252}, - {21.04, 12.68181537}, - {21.05, 12.67658858}, - {21.06, 12.67163233}, - {21.07, 12.66643363}, - {21.08, 12.6624098}, - {21.09, 12.65745464}, - {21.1, 12.65249991}, - {21.11, 12.64754561}, - {21.12, 12.64352336}, - {21.13, 12.63830036}, - {21.14, 12.63334738}, - {21.15, 12.62815066}, - {21.16, 12.62412991}, - {21.17, 12.61917802}, - {21.18, 12.61422657}, - {21.19, 12.61020702}, - {21.2, 12.6052564}, - {21.21, 12.60030623}, - {21.22, 12.59604259}, - {21.23, 12.59109308}, - {21.24, 12.58707536}, - {21.25, 12.58188098}, - {21.26, 12.57786389}, - {21.27, 12.57384718}, - {21.28, 12.56865347}, - {21.29, 12.56490597}, - {21.3, 12.56064385}, - {21.31, 12.55569711}, - {21.32, 12.551682}, - {21.33, 12.54742049}, - {21.34, 12.54247484}, - {21.35, 12.53846075}, - {21.36, 12.53446804}, - {21.37, 12.5304545}, - {21.38, 12.52551029}, - {21.39, 12.52124994}, - {21.4, 12.51723743}, - {21.41, 12.51204644}, - {21.42, 12.50830244}, - {21.43, 12.5042909}, - {21.44, 12.50003148}, - {21.45, 12.49508964}, - {21.46, 12.49107914}, - {21.47, 12.48682033}, - {21.48, 12.48307799}, - {21.49, 12.47788864}, - {21.5, 12.47387935}, - {21.51, 12.46987045}, - {21.52, 12.46561258}, - {21.53, 12.46160431}, - {21.54, 12.45668327}, - {21.55, 12.45267558}, - {21.56, 12.44866828}, - {21.57, 12.44441136}, - {21.58, 12.4404047}, - {21.59, 12.43548443}, - {21.6, 12.43147835}, - {21.61, 12.42747267}, - {21.62, 12.42321669}, - {21.63, 12.41947837}, - {21.64, 12.41522271}, - {21.65, 12.41121823}, - {21.66, 12.40628365}, - {21.67, 12.40202862}, - {21.68, 12.39829166}, - {21.69, 12.39403695}, - {21.7, 12.3900341}, - {21.71, 12.38603165}, - {21.72, 12.38204381}, - {21.73, 12.37804193}, - {21.74, 12.37285783}, - {21.75, 12.36885662}, - {21.76, 12.36512184}, - {21.77, 12.36086866}, - {21.78, 12.35686841}, - {21.79, 12.35261563}, - {21.8, 12.34888185}, - {21.81, 12.34488258}, - {21.82, 12.34063037}, - {21.83, 12.33663175}, - {21.84, 12.33264554}, - {21.85, 12.32864749}, - {21.86, 12.32439601}, - {21.87, 12.32039861}, - {21.88, 12.316667}, - {21.89, 12.31241609}, - {21.9, 12.30841968}, - {21.91, 12.30416917}, - {21.92, 12.30043857}, - {21.93, 12.29618839}, - {21.94, 12.29219321}, - {21.95, 12.28846345}, - {21.96, 12.28421385}, - {21.97, 12.28021966}, - {21.98, 12.27623533}, - {21.99, 12.27224173}, - {22, 12.26799287}, - {22.01, 12.2639999}, - {22.02, 12.26001611}, - {22.03, 12.25602374}, - {22.04, 12.25203179}, - {22.05, 12.24804842}, - {22.06, 12.24405705}, - {22.07, 12.23980952}, - {22.08, 12.23608316}, - {22.09, 12.23183596}, - {22.1, 12.22784584}, - {22.11, 12.22386326}, - {22.12, 12.22080329}, - {22.13, 12.21681415}, - {22.14, 12.21283198}, - {22.15, 12.20884343}, - {22.16, 12.20459758}, - {22.17, 12.20087358}, - {22.18, 12.19662806}, - {22.19, 12.19264077}, - {22.2, 12.1886594}, - {22.21, 12.18560208}, - {22.22, 12.18135731}, - {22.23, 12.17763483}, - {22.24, 12.17364912}, - {22.25, 12.16940496}, - {22.26, 12.16568333}, - {22.27, 12.16236877}, - {22.28, 12.15838431}, - {22.29, 12.15440414}, - {22.3, 12.15042028}, - {22.31, 12.14644038}, - {22.32, 12.14245712}, - {22.33, 12.13914354}, - {22.34, 12.13542393}, - {22.35, 12.13118156}, - {22.36, 12.12719955}, - {22.37, 12.12322045}, - {22.38, 12.12016807}, - {22.39, 12.11644981}, - {22.4, 12.11220839}, - {22.41, 12.10822799}, - {22.42, 12.10517852}, - {22.43, 12.1011987}, - {22.44, 12.09722055}, - {22.45, 12.09324134}, - {22.46, 12.0899299}, - {22.47, 12.08621368}, - {22.48, 12.08197372}, - {22.49, 12.07825803}, - {22.5, 12.0749472}, - {22.51, 12.07096985}, - {22.52, 12.06699275}, - {22.53, 12.063016}, - {22.54, 12.05996787}, - {22.55, 12.05599171}, - {22.56, 12.05175323}, - {22.57, 12.04896821}, - {22.58, 12.04473009}, - {22.59, 12.04101694}, - {22.6, 12.03770776}, - {22.61, 12.0339951}, - {22.62, 12.02975769}, - {22.63, 12.02671252}, - {22.64, 12.022737}, - {22.65, 12.01876391}, - {22.66, 12.01571712}, - {22.67, 12.01174463}, - {22.68, 12.00776965}, - {22.69, 12.00472615}, - {22.7, 12.00075143}, - {22.71, 11.99678015}, - {22.72, 11.99373403}, - {22.73, 11.98976334}, - {22.74, 11.98552815}, - {22.75, 11.98274735}, - {22.76, 11.97851251}, - {22.77, 11.97573217}, - {22.78, 11.97149769}, - {22.79, 11.96778965}, - {22.8, 11.96448369}, - {22.81, 11.96077615}, - {22.82, 11.95747051}, - {22.83, 11.95376347}, - {22.84, 11.94953007}, - {22.85, 11.94675161}, - {22.86, 11.94251858}, - {22.87, 11.93947411}, - {22.88, 11.93550791}, - {22.89, 11.93246366}, - {22.9, 11.92849806}, - {22.91, 11.92545404}, - {22.92, 11.92148904}, - {22.93, 11.91751736}, - {22.94, 11.91448085}, - {22.95, 11.91050943}, - {22.96, 11.90747349}, - {22.97, 11.90350234}, - {22.98, 11.90046697}, - {22.99, 11.89649608}, - {23, 11.89346127}, - {23.01, 11.8894906}, - {23.02, 11.88645641}, - {23.03, 11.88248606}, - {23.04, 11.87918389}, - {23.05, 11.87548231}, - {23.06, 11.87218047}, - {23.07, 11.86847939}, - {23.08, 11.86517787}, - {23.09, 11.86147731}, - {23.1, 11.85817612}, - {23.11, 11.85540358}, - {23.12, 11.85117522}, - {23.13, 11.84840314}, - {23.14, 11.84417515}, - {23.15, 11.84113377}, - {23.16, 11.83717593}, - {23.17, 11.83413478}, - {23.18, 11.83043642}, - {23.19, 11.82713664}, - {23.2, 11.82436611}, - {23.21, 11.82013934}, - {23.22, 11.81736929}, - {23.23, 11.81314289}, - {23.24, 11.8101025}, - {23.25, 11.8061473}, - {23.26, 11.80310713}, - {23.27, 11.80033817}, - {23.28, 11.79611262}, - {23.29, 11.79334414}, - {23.3, 11.78911897}, - {23.31, 11.78635096}, - {23.32, 11.78305325}, - {23.33, 11.77908676}, - {23.34, 11.77631937}, - {23.35, 11.77302208}, - {23.36, 11.76932815}, - {23.37, 11.76603119}, - {23.38, 11.76326473}, - {23.39, 11.75904116}, - {23.4, 11.75600251}, - {23.41, 11.75230977}, - {23.42, 11.74901358}, - {23.43, 11.74624821}, - {23.44, 11.74202551}, - {23.45, 11.73898739}, - {23.46, 11.73622265}, - {23.47, 11.73292721}, - {23.48, 11.72923616}, - {23.49, 11.72594107}, - {23.5, 11.72317726}, - {23.51, 11.71921311}, - {23.52, 11.71591844}, - {23.53, 11.71315525}, - {23.54, 11.70893428}, - {23.55, 11.70617158}, - {23.56, 11.70313467}, - {23.57, 11.69984077}, - {23.58, 11.6961521}, - {23.59, 11.6931155}, - {23.6, 11.69009692}, - {23.61, 11.68613405}, - {23.62, 11.68311607}, - {23.63, 11.68007992}, - {23.64, 11.67704385}, - {23.65, 11.67310019}, - {23.66, 11.67006435}, - {23.67, 11.66730427}, - {23.68, 11.66401209}, - {23.69, 11.66005021}, - {23.7, 11.65729077}, - {23.71, 11.65399901}, - {23.72, 11.65124002}, - {23.73, 11.64794862}, - {23.74, 11.64398738}, - {23.75, 11.64122902}, - {23.76, 11.63793805}, - {23.77, 11.63490335}, - {23.78, 11.63214559}, - {23.79, 11.62792892}, - {23.8, 11.62517165}, - {23.81, 11.62213741}, - {23.82, 11.61884732}, - {23.83, 11.61609065}, - {23.84, 11.61213068}, - {23.85, 11.6091187}, - {23.86, 11.606085}, - {23.87, 11.60305137}, - {23.88, 11.60004013}, - {23.89, 11.59700673}, - {23.9, 11.59332571}, - {23.91, 11.59003701}, - {23.92, 11.58700393}, - {23.93, 11.58424944}, - {23.94, 11.58096119}, - {23.95, 11.5779284}, - {23.96, 11.57517451}, - {23.97, 11.57096094}, - {23.98, 11.56820755}, - {23.99, 11.56517523}, - {24, 11.56214297}, - {24.01, 11.55913509}, - {24.02, 11.55610306}, - {24.03, 11.55307111}, - {24.04, 11.55006398}, - {24.05, 11.54610666}, - {24.06, 11.54307502}, - {24.07, 11.54006867}, - {24.08, 11.53703727}, - {24.09, 11.53428619}, - {24.1, 11.53125501}, - {24.11, 11.52796925}, - {24.12, 11.52521878}, - {24.13, 11.5221879}, - {24.14, 11.5189026}, - {24.15, 11.51615274}, - {24.16, 11.51312217}, - {24.17, 11.51009166}, - {24.18, 11.50616274}, - {24.19, 11.50313249}, - {24.2, 11.5001023}, - {24.21, 11.4970995}, - {24.22, 11.49406956}, - {24.23, 11.49132142}, - {24.24, 11.48829169}, - {24.25, 11.48500801}, - {24.26, 11.48226049}, - {24.27, 11.47923107}, - {24.28, 11.47620172}, - {24.29, 11.47320095}, - {24.3, 11.47017184}, - {24.31, 11.4671428}, - {24.32, 11.4643965}, - {24.33, 11.46111399}, - {24.34, 11.45808527}, - {24.35, 11.45533958}, - {24.36, 11.45231108}, - {24.37, 11.44902913}, - {24.38, 11.44628407}, - {24.39, 11.44325587}, - {24.4, 11.44022775}, - {24.41, 11.43722995}, - {24.42, 11.43420207}, - {24.43, 11.43117426}, - {24.44, 11.42843043}, - {24.45, 11.42514967}, - {24.46, 11.42212218}, - {24.47, 11.41937897}, - {24.48, 11.41635169}, - {24.49, 11.41332449}, - {24.5, 11.41125361}, - {24.51, 11.40822664}, - {24.52, 11.40519974}, - {24.53, 11.40245772}, - {24.54, 11.39917826}, - {24.55, 11.39615168}, - {24.56, 11.39341029}, - {24.57, 11.39038393}, - {24.58, 11.38735764}, - {24.59, 11.38436428}, - {24.6, 11.38133823}, - {24.61, 11.37831227}, - {24.62, 11.37528637}, - {24.63, 11.37254637}, - {24.64, 11.37019274}, - {24.65, 11.36716715}, - {24.66, 11.36442775}, - {24.67, 11.36140238}, - {24.68, 11.35837708}, - {24.69, 11.35538612}, - {24.7, 11.35236108}, - {24.71, 11.3493361}, - {24.72, 11.34659795}, - {24.73, 11.34449743}, - {24.74, 11.34122079}, - {24.75, 11.3381962}, - {24.76, 11.33545879}, - {24.77, 11.33243443}, - {24.78, 11.32941013}, - {24.79, 11.32642159}, - {24.8, 11.32432166}, - {24.81, 11.32129767}, - {24.82, 11.31856148}, - {24.83, 11.31553772}, - {24.84, 11.31251404}, - {24.85, 11.30923892}, - {24.86, 11.30742751}, - {24.87, 11.30440414}, - {24.88, 11.30138084}, - {24.89, 11.29864603}, - {24.9, 11.29562296}, - {24.91, 11.29234869}, - {24.92, 11.29053838}, - {24.93, 11.28751563}, - {24.94, 11.28449294}, - {24.95, 11.28147034}, - {24.96, 11.2787369}, - {24.97, 11.27663828}, - }; - - - arma::uvec index_E = find(LkTable <= t_disparo, 1, "last"); - - if (index_E(0) < (LkTable.n_rows - 1)) - { - double tdisparo1 = LkTable(index_E(0), 0); - double tdisparo2 = LkTable(index_E(0) + 1, 0); - double Acc1 = LkTable(index_E(0), 1); - double Acc2 = LkTable(index_E(0) + 1, 1); - - Acc = (t_disparo - tdisparo1) * (Acc2 - Acc1) / (tdisparo2 - tdisparo1) + Acc1; - } - else - { - Acc = 9.81; - } - - return Acc; -} - -double Vtl_Engine::EmpujeLkTable(double t_disparo) -{ - double E; - arma::mat LkTable = { - {0.0, 391.083112445424}, - {0.0100578034682081, 385.626317230813}, - {0.0201156069364162, 379.253652903964}, - {0.0301734104046243, 372.850418310078}, - {0.0402312138728324, 366.435105395212}, - {0.0502890173410405, 359.948724887310}, - {0.0603468208092486, 353.452370826679}, - {0.0704046242774566, 346.915160536406}, - {0.0804624277456647, 340.353374212744}, - {0.0905202312138728, 339.982366920698}, - {0.100578034682081, 339.649644036322}, - {0.110635838150289, 339.313119301332}, - {0.120693641618497, 338.971249841340}, - {0.130751445086705, 338.626092336370}, - {0.140809248554913, 338.277926096280}, - {0.150867052023121, 337.923860794114}, - {0.160924855491329, 337.564686821652}, - {0.170982658959538, 337.204565166310}, - {0.181040462427746, 336.836327593982}, - {0.191098265895954, 337.612574596978}, - {0.201156069364162, 338.389812277202}, - {0.211213872832370, 339.172197383571}, - {0.221271676300578, 339.950864133333}, - {0.231329479768786, 340.734286946588}, - {0.241387283236994, 341.513599696996}, - {0.251445086705202, 342.296487929689}, - {0.261502890173410, 343.077730186615}, - {0.271560693641619, 343.862401553296}, - {0.281618497109827, 344.645045182721}, - {0.291676300578035, 345.430222609429}, - {0.301734104046243, 346.216363751051}, - {0.311791907514451, 347.003086022300}, - {0.321849710982659, 347.790403464500}, - {0.331907514450867, 348.576632890101}, - {0.341965317919075, 349.365041767204}, - {0.352023121387283, 350.154345504950}, - {0.362080924855491, 350.943780688588}, - {0.372138728323699, 351.735714016931}, - {0.382196531791908, 352.523676077225}, - {0.392254335260116, 353.317052045327}, - {0.402312138728324, 354.110411623268}, - {0.412369942196532, 354.903279067692}, - {0.422427745664740, 355.692559158889}, - {0.432485549132948, 356.490274978154}, - {0.442543352601156, 357.283451900588}, - {0.452601156069364, 358.078507291348}, - {0.462658959537572, 358.871323078342}, - {0.472716763005780, 359.668187019647}, - {0.482774566473988, 360.465893772213}, - {0.492832369942197, 361.264486481857}, - {0.502890173410405, 362.057713788838}, - {0.512947976878613, 362.855398652135}, - {0.523005780346821, 363.651958513907}, - {0.533063583815029, 364.453139119421}, - {0.543121387283237, 365.248494477390}, - {0.553179190751445, 366.047440571574}, - {0.563236994219653, 366.844541693072}, - {0.573294797687861, 367.645722991578}, - {0.583352601156069, 368.431281642859}, - {0.593410404624277, 369.218032671753}, - {0.603468208092486, 370.005763838889}, - {0.613526011560694, 370.793449087224}, - {0.623583815028902, 371.577331994297}, - {0.633641618497110, 372.361872668242}, - {0.643699421965318, 373.139234321453}, - {0.653757225433526, 373.922395308171}, - {0.663815028901734, 374.698638233448}, - {0.673872832369942, 375.477674739791}, - {0.683930635838150, 376.256375688174}, - {0.693988439306358, 377.561519397833}, - {0.704046242774567, 378.859304679191}, - {0.714104046242775, 380.167718611141}, - {0.724161849710983, 381.477582211590}, - {0.734219653179191, 382.785006750526}, - {0.744277456647399, 384.092555921096}, - {0.754335260115607, 385.403347233778}, - {0.764393063583815, 386.680222025901}, - {0.774450867052023, 387.960348600215}, - {0.784508670520231, 389.241465657245}, - {0.794566473988439, 390.517609695133}, - {0.804624277456647, 391.778341124162}, - {0.814682080924856, 393.038366474572}, - {0.824739884393064, 394.291210028831}, - {0.834797687861272, 395.545183108074}, - {0.844855491329480, 396.779476009029}, - {0.854913294797688, 398.005131564908}, - {0.864971098265896, 399.217607990930}, - {0.875028901734104, 400.433585037519}, - {0.885086705202312, 401.639603766860}, - {0.895144508670520, 402.824960888722}, - {0.905202312138728, 403.999818917039}, - {0.915260115606936, 405.164413592803}, - {0.925317919075145, 406.332094769783}, - {0.935375722543353, 407.489826389568}, - {0.945433526011561, 408.638695158694}, - {0.955491329479769, 409.788208430892}, - {0.965549132947977, 410.920696471959}, - {0.975606936416185, 412.071117188526}, - {0.985664739884393, 413.227362102269}, - {0.995722543352601, 414.376295532213}, - {1.00578034682081, 415.517208260982}, - {1.01583815028902, 416.685111645473}, - {1.02589595375723, 417.874034388355}, - {1.03595375722543, 419.050152356493}, - {1.04601156069364, 420.226540416000}, - {1.05606936416185, 421.431674949807}, - {1.06612716763006, 422.651832857732}, - {1.07618497109827, 423.879432015756}, - {1.08624277456647, 425.096859052146}, - {1.09630057803468, 426.370514159926}, - {1.10635838150289, 427.681420607676}, - {1.11641618497110, 428.990653815442}, - {1.12647398843931, 430.308007852515}, - {1.13653179190751, 431.623136377095}, - {1.14658959537572, 432.942354714805}, - {1.15664739884393, 434.260396644686}, - {1.16670520231214, 435.580636665589}, - {1.17676300578035, 436.903819649131}, - {1.18682080924855, 438.235977095674}, - {1.19687861271676, 439.760282970165}, - {1.20693641618497, 441.285566201751}, - {1.21699421965318, 442.823791210454}, - {1.22705202312139, 444.354330708051}, - {1.23710982658960, 445.893589623130}, - {1.24716763005780, 447.437414139676}, - {1.25722543352601, 449.016306037988}, - {1.26728323699422, 450.605733417807}, - {1.27734104046243, 452.197570123945}, - {1.28739884393064, 453.799041921642}, - {1.29745664739884, 455.395152193126}, - {1.30751445086705, 457.004209080179}, - {1.31757225433526, 458.623015526914}, - {1.32763005780347, 460.238728741943}, - {1.33768786127168, 461.860881948233}, - {1.34774566473988, 463.495649237401}, - {1.35780346820809, 465.129826626932}, - {1.36786127167630, 466.767033528504}, - {1.37791907514451, 468.408766204263}, - {1.38797687861272, 470.059173838784}, - {1.39803468208092, 471.721126584341}, - {1.40809248554913, 473.414912587257}, - {1.41815028901734, 475.097553575053}, - {1.42820809248555, 476.805256760032}, - {1.43826589595376, 478.512506689219}, - {1.44832369942197, 480.230235676597}, - {1.45838150289017, 481.950516788809}, - {1.46843930635838, 483.675596525192}, - {1.47849710982659, 485.408520318711}, - {1.48855491329480, 487.160694244723}, - {1.49861271676301, 488.912737035966}, - {1.50867052023121, 490.676335211920}, - {1.51872832369942, 492.446193358219}, - {1.52878612716763, 494.230761014528}, - {1.53884393063584, 496.014213821540}, - {1.54890173410405, 497.800012215749}, - {1.55895953757225, 499.586844428474}, - {1.56901734104046, 501.389791490613}, - {1.57907514450867, 503.197873321221}, - {1.58913294797688, 505.017976823598}, - {1.59919075144509, 506.842077493572}, - {1.60924855491329, 508.681008225194}, - {1.61930635838150, 510.531745060971}, - {1.62936416184971, 512.383464849326}, - {1.63942196531792, 514.249250804510}, - {1.64947976878613, 516.119098877381}, - {1.65953757225434, 518.005308788433}, - {1.66959537572254, 519.910761252352}, - {1.67965317919075, 521.812275649970}, - {1.68971098265896, 523.727747256148}, - {1.69976878612717, 526.524959987653}, - {1.70982658959538, 529.326074922632}, - {1.71988439306358, 532.152158438731}, - {1.72994219653179, 534.995939192065}, - {1.74000000000000, 537.866310625605}, - }; - - // encuentra el mas cercano justo anterior. - // int index_E = LkTable.elem(find(LkTable<=t_disparo)).max(); - arma::uvec index_E = find(LkTable <= t_disparo, 1, "last"); - // index_E.print("indice E: "); - // uint kk = index_E(0); - if (index_E(0) < (LkTable.n_rows - 1)) - { - double tdisparo1 = LkTable(index_E(0), 0); - double tdisparo2 = LkTable(index_E(0) + 1, 0); - double E1 = LkTable(index_E(0), 1); - double E2 = LkTable(index_E(0) + 1, 1); - - E = (t_disparo - tdisparo1) * (E2 - E1) / (tdisparo2 - tdisparo1) + E1; - } - else - { - E = 0; - } - - return E; -} - std::vector Vtl_Engine::get_position_ecef_m() { std::vector temp = {42, 42, 42}; diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index d1977df72..d7f3d5cb3 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -103,14 +103,11 @@ private: uint64_t refSampleCounter; double delta_t_cmd = 0; - void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor - void kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor + void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); /* */ // Observation Matrix constructor + void kf_F_fill_rocket(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor + void kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); - void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); - void accelerometer(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); - double EmpujeLkTable(double t_disparo); - double AccLkTable(double t_disparo); }; /** \} */ From d78dccdd7ca2128a2cd16f1dba266948994af303 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Thu, 18 Jan 2024 16:05:49 +0100 Subject: [PATCH 158/165] make clang-format happy --- src/algorithms/PVT/libs/rtklib_solver.cc | 46 ++++++++++++------------ src/algorithms/PVT/libs/vtl_engine.cc | 1 - src/algorithms/PVT/libs/vtl_engine.h | 14 ++++---- 3 files changed, 30 insertions(+), 31 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index d63548582..eb084a899 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -62,7 +62,7 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, { // TODO: temporal VTL config parameters are hardcoded here. Move it to PVT adapter config (javi) Vtl_Conf new_vtl_conf; - //TODO: new_vtl_conf.parameter1=blablabla + // TODO: new_vtl_conf.parameter1=blablabla vtl_engine.configure(new_vtl_conf); vtl_engine.reset(); @@ -1846,9 +1846,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ dops(index_aux, azel.data(), 0.0, d_dop.data()); } this->set_valid_position(true); - //this->set_averaging_flag(true); - //this->set_averaging_depth(100); - //this->perform_pos_averaging(); + // this->set_averaging_flag(true); + // this->set_averaging_depth(100); + // this->perform_pos_averaging(); std::array rx_position_and_time{}; if (d_conf.enable_pvt_kf == true) @@ -1902,7 +1902,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ if (enable_vtl == true) { - //VTL input data extraction from rtklib structures + // VTL input data extraction from rtklib structures /* satellite positions, velocities and clocks */ prcopt_t *opt = &d_rtk.opt; /* count rover/base station observations */ @@ -1927,7 +1927,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ /* satellite positions, velocities and clocks */ satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); - //Vtl_Data vtl_data; + // Vtl_Data vtl_data; vtl_data.init_storage(n_sats); vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands @@ -1947,14 +1947,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.sat_health_flag(n) = svh.at(n); vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0] * 0.25; //(0.25 dBHz) // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); - //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) + // To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); + // corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n] + SPEED_OF_LIGHT_M_S * dts[n * 2]; vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0] - SPEED_OF_LIGHT_M_S * dts[1 + 2 * n] / Lambda_GPS_L1; vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; vtl_data.pr_res(n) = pr_residual_vec[n]; } - //VTL input data extraction from rtklib structures + // VTL input data extraction from rtklib structures /* Receiver position, velocity and clock */ /* position/velocity (m|m/s):{x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ vtl_data.rx_p(0) = pvt_sol.rr[0]; @@ -1967,13 +1967,13 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.rx_pvt_var[0] = pvt_sol.qr[0]; vtl_data.rx_pvt_var[1] = pvt_sol.qr[1]; vtl_data.rx_pvt_var[2] = pvt_sol.qr[2]; - //TODO: get direct estimations for V T variances, instead: - vtl_data.rx_pvt_var[3] = pvt_sol.qr[0]; //in general minor than position. + // TODO: get direct estimations for V T variances, instead: + vtl_data.rx_pvt_var[3] = pvt_sol.qr[0]; // in general minor than position. vtl_data.rx_pvt_var[4] = pvt_sol.qr[1]; vtl_data.rx_pvt_var[5] = pvt_sol.qr[2]; - vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time - vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler - //receiver clock offset and receiver clock drift + vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; // time + vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; // doppler + // receiver clock offset and receiver clock drift vtl_data.rx_dts(0) = rx_position_and_time[3]; vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] @@ -1981,15 +1981,15 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } else { - //MAGL: the code should not enter here once the vtl has started - // .. but it does! - //and not only that but pvt_sol.rr seems to have NOT reasonable values - // pvt_sol.rr[0]=rx_position_and_time[0]; // [m] - // pvt_sol.rr[1]=rx_position_and_time[1]; // [m] - // pvt_sol.rr[2]=rx_position_and_time[2]; // [m] - // pvt_sol.rr[3]=4.2e6; - // pvt_sol.rr[4]=4.2e6; - // pvt_sol.rr[5]=4.2e6; + // MAGL: the code should not enter here once the vtl has started + // .. but it does! + // and not only that but pvt_sol.rr seems to have NOT reasonable values + // pvt_sol.rr[0]=rx_position_and_time[0]; // [m] + // pvt_sol.rr[1]=rx_position_and_time[1]; // [m] + // pvt_sol.rr[2]=rx_position_and_time[2]; // [m] + // pvt_sol.rr[3]=4.2e6; + // pvt_sol.rr[4]=4.2e6; + // pvt_sol.rr[5]=4.2e6; } // compute Ground speed and COG double ground_speed_ms = 0.0; diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 56e3fb876..cbfecb786 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -298,7 +298,6 @@ void Vtl_Engine::kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arm } void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x) { - // modulo de la velocidad double vx = kf_x(3); double vy = kf_x(4); diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index d7f3d5cb3..122d51041 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -39,9 +39,9 @@ public: ~Vtl_Engine(); - void configure(Vtl_Conf config_); //set config parameters + void configure(Vtl_Conf config_); // set config parameters - //TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...) + // TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...) bool vtl_loop(Vtl_Data new_data); void reset(); // reset all internal states void debug_print(); // print debug information @@ -57,11 +57,11 @@ public: double get_longitude(); // get_longitude double get_height(); // get_height double get_user_clock_offset_s(); // get_user_clock_offset_s; - double get_user_clock_offset_drift_s_s(); // get_user_clock_offset_drift_s/s; + double get_user_clock_offset_drift_s_s(); // get_user_clock_offset_drift_s/s; private: Vtl_Conf config; - //TODO: Internal VTL persistent variables here + // TODO: Internal VTL persistent variables here // Transformation variables arma::colvec d; @@ -103,9 +103,9 @@ private: uint64_t refSampleCounter; double delta_t_cmd = 0; - void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); /* */ // Observation Matrix constructor - void kf_F_fill_rocket(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor - void kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor + void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); /* */ // Observation Matrix constructor + void kf_F_fill_rocket(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor + void kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); }; From fc30788c12434866ee2aff300634ce3b6c31a291 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Thu, 18 Jan 2024 16:17:48 +0100 Subject: [PATCH 159/165] add license to some files --- src/utils/matlab/vtl/CN0_sat_plotting.m | 14 ++++++++++-- src/utils/matlab/vtl/GnssSDR2struct.m | 11 ++++++++++ src/utils/matlab/vtl/SpirentMotion2struct.m | 22 ++++++++++--------- src/utils/matlab/vtl/SpirentSatData2struct.m | 22 ++++++++++--------- src/utils/matlab/vtl/Vtl2struct.m | 15 ++++++++++--- src/utils/matlab/vtl/dopp_filtered_plotting.m | 12 +++++++++- src/utils/matlab/vtl/doppler_vtl_plot.m | 11 ++++++++++ src/utils/matlab/vtl/general_raw_plot.m | 13 ++++++++++- .../matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m | 5 +++-- src/utils/matlab/vtl/kf_prototype.m | 13 ++++++++++- src/utils/matlab/vtl/kf_prototype4acc.m | 13 ++++++++++- src/utils/matlab/vtl/kf_prototype_clk_d.m | 13 ++++++++++- src/utils/matlab/vtl/kf_prototype_model.m | 13 ++++++++++- src/utils/matlab/vtl/pr_m_vtl_plot.m | 13 ++++++++++- .../vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 6 ++--- src/utils/matlab/vtl/trk2struct.m | 13 +++++++++-- src/utils/matlab/vtl/vtl_general_plot.m | 13 ++++++++++- .../matlab/vtl/vtl_prototype_clk_d_drift.m | 7 +++--- src/utils/python/analysis.py | 14 +++++++++--- src/utils/python/vtl_analysis.py | 16 ++++++++++---- 20 files changed, 209 insertions(+), 50 deletions(-) diff --git a/src/utils/matlab/vtl/CN0_sat_plotting.m b/src/utils/matlab/vtl/CN0_sat_plotting.m index 8101a3c80..6117b5a11 100644 --- a/src/utils/matlab/vtl/CN0_sat_plotting.m +++ b/src/utils/matlab/vtl/CN0_sat_plotting.m @@ -1,4 +1,14 @@ - +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% time_reference_spirent_obs=129780;%s @@ -62,4 +72,4 @@ grid on hold on title('PRN 10 GNSS-SDR') plot(time_vtl_dump_file,vtlSolution.CN0_sat(5,:),'o','DisplayName','filtered VTL') -hold off;grid minor \ No newline at end of file +hold off;grid minor diff --git a/src/utils/matlab/vtl/GnssSDR2struct.m b/src/utils/matlab/vtl/GnssSDR2struct.m index 170b37f24..2ed4f6a89 100644 --- a/src/utils/matlab/vtl/GnssSDR2struct.m +++ b/src/utils/matlab/vtl/GnssSDR2struct.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% % GnssSDR2struct Convert GNSS-SDR output .mat file PVT.mat to a struct. % refSolution = SpirentMotion2struct(path_to_motion_V1_csv) Convert PVT.mat to a struct navSolution % refSolution has the following fields: diff --git a/src/utils/matlab/vtl/SpirentMotion2struct.m b/src/utils/matlab/vtl/SpirentMotion2struct.m index b5789330b..15a3ac686 100644 --- a/src/utils/matlab/vtl/SpirentMotion2struct.m +++ b/src/utils/matlab/vtl/SpirentMotion2struct.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% % SpirentMotion2struct Convert CSV file motionV1.csv to a struct. % refSolution = SpirentMotion2struct(path_to_motion_V1_csv) parse the CSV motionV1.CSV to a struct refSolution % refSolution has the following fields: @@ -26,16 +37,7 @@ % ------------------------------------------------------------------------- % USE EXAMPLE: refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv') % ------------------------------------------------------------------------- -% -% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. -% This file is part of GNSS-SDR. -% -% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) -% SPDX-License-Identifier: GPL-3.0-or-later -% -% ------------------------------------------------------------------------- -% SPDX-FileCopyrightText: 2022 gomezlma(at)inta.es -% SPDX-License-Identifier: GPL-3.0-or-later + diff --git a/src/utils/matlab/vtl/SpirentSatData2struct.m b/src/utils/matlab/vtl/SpirentSatData2struct.m index 640e5ad38..f720c8683 100644 --- a/src/utils/matlab/vtl/SpirentSatData2struct.m +++ b/src/utils/matlab/vtl/SpirentSatData2struct.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% % SpirentSatData2struct Convert CSV file sat_data_V1A1.csv to a struct. % refSolution = SpirentSatData2struct(path_to_sat_V1_csv) parse the CSV sat_data_V1A1.CSV to a struct refSatData % refSatData has the following fields: @@ -33,16 +44,7 @@ % ------------------------------------------------------------------------- % USE EXAMPLE: refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv') % ------------------------------------------------------------------------- -% -% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. -% This file is part of GNSS-SDR. -% -% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) -% SPDX-License-Identifier: GPL-3.0-or-later -% -% ------------------------------------------------------------------------- -% SPDX-FileCopyrightText: 2022 gomezlma(at)inta.es -% SPDX-License-Identifier: GPL-3.0-or-later + diff --git a/src/utils/matlab/vtl/Vtl2struct.m b/src/utils/matlab/vtl/Vtl2struct.m index 6ffc62e85..7bd350bc1 100644 --- a/src/utils/matlab/vtl/Vtl2struct.m +++ b/src/utils/matlab/vtl/Vtl2struct.m @@ -7,8 +7,17 @@ % ------------------------------------------------------------------------- % USE EXAMPLE: vtlSolution = Vtl2struct('dump_vtl_file.csv') % ------------------------------------------------------------------------- - -% Auto-generated by MATLAB on 24-Nov-2022 17:34:27 +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% function [vtlSolution] = Vtl2struct(path_to_vtl_csv) %% Set up the Import Options and import the data opts = delimitedTextImportOptions("NumVariables", 9); @@ -103,4 +112,4 @@ vtlSolution.CN0_sat(3,:)=CN0_sat(:,3); vtlSolution.CN0_sat(4,:)=CN0_sat(:,4); vtlSolution.CN0_sat(5,:)=CN0_sat(:,5); -end \ No newline at end of file +end diff --git a/src/utils/matlab/vtl/dopp_filtered_plotting.m b/src/utils/matlab/vtl/dopp_filtered_plotting.m index 0119eec75..2ce6a130e 100644 --- a/src/utils/matlab/vtl/dopp_filtered_plotting.m +++ b/src/utils/matlab/vtl/dopp_filtered_plotting.m @@ -1,4 +1,14 @@ - +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% time_reference_spirent_obs=129780;%s diff --git a/src/utils/matlab/vtl/doppler_vtl_plot.m b/src/utils/matlab/vtl/doppler_vtl_plot.m index bf175bd6c..e8fbdbb1f 100644 --- a/src/utils/matlab/vtl/doppler_vtl_plot.m +++ b/src/utils/matlab/vtl/doppler_vtl_plot.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% %% LOAD Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),channels); diff --git a/src/utils/matlab/vtl/general_raw_plot.m b/src/utils/matlab/vtl/general_raw_plot.m index e295aa543..f08f0996c 100644 --- a/src/utils/matlab/vtl/general_raw_plot.m +++ b/src/utils/matlab/vtl/general_raw_plot.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% %general_raw_plot.m % navSolution.X(navSolution.X==0) = []; @@ -181,4 +192,4 @@ if(plot_reference) subplot(2,2,3); plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Z,'.','DisplayName','reference'); hold on;grid on -end \ No newline at end of file +end diff --git a/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m b/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m index 6198d51f9..98d708d60 100644 --- a/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m +++ b/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m @@ -1,15 +1,16 @@ + % Read PVG raw dump +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es % ------------------------------------------------------------------------- % % GNSS-SDR is a Global Navigation Satellite System software-defined receiver. % This file is part of GNSS-SDR. % -% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) % SPDX-License-Identifier: GPL-3.0-or-later % % ------------------------------------------------------------------------- % -%% clc close all; clear all; diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index f00e1ca05..dd8d10d97 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% %% vtl KF %% sat_number=5; @@ -233,4 +244,4 @@ for t=2:length(navSolution.RX_time) % carrier_freq_hz = 0; % carrier_freq_rate_hz_s = 0; % code_phase_chips = 0; -end \ No newline at end of file +end diff --git a/src/utils/matlab/vtl/kf_prototype4acc.m b/src/utils/matlab/vtl/kf_prototype4acc.m index 11e4300ab..76b5f6032 100644 --- a/src/utils/matlab/vtl/kf_prototype4acc.m +++ b/src/utils/matlab/vtl/kf_prototype4acc.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% %% vtl KF %% sat_number=5; @@ -232,4 +243,4 @@ for t=2:length(navSolution.RX_time) err_code_phase_chips(chan,t) = (kf_yerr_g(chan,t))/SPEED_OF_LIGHT_M_S*1023e3; end -end \ No newline at end of file +end diff --git a/src/utils/matlab/vtl/kf_prototype_clk_d.m b/src/utils/matlab/vtl/kf_prototype_clk_d.m index 2ee938376..6518a0f78 100644 --- a/src/utils/matlab/vtl/kf_prototype_clk_d.m +++ b/src/utils/matlab/vtl/kf_prototype_clk_d.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% %% vtl KF %% sat_number=5; @@ -215,4 +226,4 @@ for t=2:length(navSolution.RX_time) % carrier_freq_hz = 0; % carrier_freq_rate_hz_s = 0; % code_phase_chips = 0; -end \ No newline at end of file +end diff --git a/src/utils/matlab/vtl/kf_prototype_model.m b/src/utils/matlab/vtl/kf_prototype_model.m index 602665755..2221f07f3 100644 --- a/src/utils/matlab/vtl/kf_prototype_model.m +++ b/src/utils/matlab/vtl/kf_prototype_model.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% %% vtl KF load rocket_model_dynamics.mat %% @@ -277,4 +288,4 @@ end err_code_phase_chips(chan,t) = (kf_yerr_g(chan,t))/SPEED_OF_LIGHT_M_S*1023e3; end -end \ No newline at end of file +end diff --git a/src/utils/matlab/vtl/pr_m_vtl_plot.m b/src/utils/matlab/vtl/pr_m_vtl_plot.m index da96bb1a6..388abc2b7 100644 --- a/src/utils/matlab/vtl/pr_m_vtl_plot.m +++ b/src/utils/matlab/vtl/pr_m_vtl_plot.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% %% LOAD Pseudorange_m_sim=zeros(length(refSatData.GPS.SIM_time),channels); @@ -46,4 +57,4 @@ for channel_cnt=1:channels plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,channel_cnt)','.','DisplayName','SPIRENT reference') % plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL') hold off;grid minor -end \ No newline at end of file +end diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m index 77a0fb415..c6d42de13 100644 --- a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m +++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m @@ -1,15 +1,15 @@ % Read PVG raw dump +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es % ------------------------------------------------------------------------- % % GNSS-SDR is a Global Navigation Satellite System software-defined receiver. % This file is part of GNSS-SDR. % -% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) % SPDX-License-Identifier: GPL-3.0-or-later % % ------------------------------------------------------------------------- % -%% clc close all clearvars @@ -403,4 +403,4 @@ if(plot_skyplot) drawnow limitrate end legend('GPS') -end \ No newline at end of file +end diff --git a/src/utils/matlab/vtl/trk2struct.m b/src/utils/matlab/vtl/trk2struct.m index efee9865b..eebbe3f20 100644 --- a/src/utils/matlab/vtl/trk2struct.m +++ b/src/utils/matlab/vtl/trk2struct.m @@ -7,8 +7,17 @@ % ------------------------------------------------------------------------- % USE EXAMPLE: vtlSolution = Vtl2struct('dump_vtl_file.csv') % ------------------------------------------------------------------------- +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- -% Auto-generated by MATLAB on 24-Nov-2022 17:34:27 function [trkSolution] = trk2struct(path_to_trk_csv) %% Set up the Import Options and import the data opts = delimitedTextImportOptions("NumVariables", 5); @@ -56,4 +65,4 @@ for i=1:length(trk_dopp) trkSolution.doppShift.real=[trk_dopp(:,1) trk_dopp(:,4)]; trkSolution.tao.real=[trk_dopp(:,1) trk_dopp(:,5)]; end -end \ No newline at end of file +end diff --git a/src/utils/matlab/vtl/vtl_general_plot.m b/src/utils/matlab/vtl/vtl_general_plot.m index 9004ecb75..357cf08f8 100644 --- a/src/utils/matlab/vtl/vtl_general_plot.m +++ b/src/utils/matlab/vtl/vtl_general_plot.m @@ -1,3 +1,14 @@ +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% %% % vtl_general_plot.m %% @@ -190,4 +201,4 @@ ylabel('clk drift (m/s)') xlabel('time [s]') title('Subplot 1: clk drift [m/s]') -sgtitle('VTL STATE') \ No newline at end of file +sgtitle('VTL STATE') diff --git a/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m b/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m index e42484acb..c05b6f2ac 100644 --- a/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m +++ b/src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m @@ -1,15 +1,16 @@ + % VTL prototype +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es % ------------------------------------------------------------------------- % % GNSS-SDR is a Global Navigation Satellite System software-defined receiver. % This file is part of GNSS-SDR. % -% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) % SPDX-License-Identifier: GPL-3.0-or-later % % ------------------------------------------------------------------------- % -%% clc close all clearvars @@ -162,4 +163,4 @@ end % for chan=0:4 % eval(['subplot(2,3,' num2str(chan+1) ');plot(Phase_real_CH' num2str(chan) '-transpose(carrier_phase_rads_filt(' num2str(chan+1) ',1:3425))+Phase_real_CH' num2str(chan) '(1)' ')']) % grid minor; -% end \ No newline at end of file +% end diff --git a/src/utils/python/analysis.py b/src/utils/python/analysis.py index c1252451b..2b4da7dae 100644 --- a/src/utils/python/analysis.py +++ b/src/utils/python/analysis.py @@ -1,9 +1,17 @@ #!/usr/bin/env python3 # -*- coding: utf-8 -*- """ -Created on Wed Nov 23 12:45:37 2022 - -@author: miguel +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% """ import numpy as np diff --git a/src/utils/python/vtl_analysis.py b/src/utils/python/vtl_analysis.py index b3ed141f4..b0f8bfa0a 100644 --- a/src/utils/python/vtl_analysis.py +++ b/src/utils/python/vtl_analysis.py @@ -1,9 +1,17 @@ #!/usr/bin/env python3 # -*- coding: utf-8 -*- """ -Created on Sat Nov 26 22:18:25 2022 - -@author: miguel +% Miguel Angel Gomez, 2024. gomezlma(at)inta.es +% ------------------------------------------------------------------------- +% +% GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +% This file is part of GNSS-SDR. +% +% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors) +% SPDX-License-Identifier: GPL-3.0-or-later +% +% ------------------------------------------------------------------------- +% """ #%% import numpy as np @@ -93,4 +101,4 @@ plt.plot(kf_estate[:,5]-kf_estate[0,5],'o',label='kf_estate') plt.plot(rtklib[:,5]-rtklib[0,5],'o',label='rtklib') plt.xlabel('time U.A.') plt.legend() -plt.show() \ No newline at end of file +plt.show() From ef9b737e921a2760583c28457488508925039ad1 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Thu, 18 Jan 2024 16:58:50 +0100 Subject: [PATCH 160/165] make clang-format happy --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 92 +++++++++---------- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 8 +- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 6 +- src/algorithms/libs/rtklib/rtklib_rtksvr.cc | 2 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 12 +-- 5 files changed, 60 insertions(+), 60 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index defa8377f..927c0aebb 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2126,8 +2126,8 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item try { d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx).channel_id = gnss_observables_iter->second.Channel_ID; - //todo: VTL loop CAN NOT run every PVT epoch because it is required to wait for the corrections to be applied to the tracking KF. - //currently the VTL runs every PVT epoch which will create inestabilities. + // todo: VTL loop CAN NOT run every PVT epoch because it is required to wait for the corrections to be applied to the tracking KF. + // currently the VTL runs every PVT epoch which will create inestabilities. const std::shared_ptr trk_cmd_test = std::make_shared(d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx)); this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); idx++; @@ -2138,50 +2138,50 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } } d_internal_pvt_solver->vtl_engine.trk_cmd_outs.clear(); - //Dummy messages for evaluation of msg latency - // std::map::const_iterator gnss_observables_iter; - // for (gnss_observables_iter = d_gnss_observables_map.cbegin(); - // gnss_observables_iter != d_gnss_observables_map.cend(); - // ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system - // { - // // test complete loop - // if (gnss_observables_iter->second.last_vtl_cmd_sample_counter == 0) - // { - // // send new tracking command - // const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); - // trk_cmd_test->carrier_freq_hz = 12345.4; - // trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; - // trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; - // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); - // d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; - // std::cout << "msg pvt_to_trk sent.\n"; - // } - // else - // { - // // std::cout << "CH " << gnss_observables_iter->second.Channel_ID - // // << " T_RX: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs) - // // << " T_last_vtl_trk: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast(gnss_observables_iter->second.fs) - // // << " T_map: " << static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast(gnss_observables_iter->second.fs) - // // << " T2: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) - // // << " T3: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) << "\n"; + // Dummy messages for evaluation of msg latency + // std::map::const_iterator gnss_observables_iter; + // for (gnss_observables_iter = d_gnss_observables_map.cbegin(); + // gnss_observables_iter != d_gnss_observables_map.cend(); + // ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system + // { + // // test complete loop + // if (gnss_observables_iter->second.last_vtl_cmd_sample_counter == 0) + // { + // // send new tracking command + // const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); + // trk_cmd_test->carrier_freq_hz = 12345.4; + // trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; + // trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; + // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); + // d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; + // std::cout << "msg pvt_to_trk sent.\n"; + // } + // else + // { + // // std::cout << "CH " << gnss_observables_iter->second.Channel_ID + // // << " T_RX: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs) + // // << " T_last_vtl_trk: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast(gnss_observables_iter->second.fs) + // // << " T_map: " << static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast(gnss_observables_iter->second.fs) + // // << " T2: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) + // // << " T3: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) << "\n"; // - // // To.Do: check if satellite change, check if there is a possibility to not find the last cmd timestamp in the map... - // if (gnss_observables_iter->second.last_vtl_cmd_sample_counter >= d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) - // { - // std::cout << "CH " << gnss_observables_iter->second.Channel_ID << " processed VTL cmd, total loop time is " - // << ((static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast(gnss_observables_iter->second.fs)) * 1000.0 - // << " [ms]!\n"; - // // send new tracking command - // const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); - // trk_cmd_test->carrier_freq_hz = 12345.4; - // trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; - // trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; - // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); - // d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; - // std::cout << "msg pvt_to_trk sent.\n"; - // } - // } - // } + // // To.Do: check if satellite change, check if there is a possibility to not find the last cmd timestamp in the map... + // if (gnss_observables_iter->second.last_vtl_cmd_sample_counter >= d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) + // { + // std::cout << "CH " << gnss_observables_iter->second.Channel_ID << " processed VTL cmd, total loop time is " + // << ((static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast(gnss_observables_iter->second.fs)) * 1000.0 + // << " [ms]!\n"; + // // send new tracking command + // const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); + // trk_cmd_test->carrier_freq_hz = 12345.4; + // trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter; + // trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID; + // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); + // d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter; + // std::cout << "msg pvt_to_trk sent.\n"; + // } + // } + // } } else { @@ -2303,7 +2303,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { // VTP To.Do: Check why get_PVT is triggered twice. Leave only one get_PVT. - //flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); + // flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, false, d_enable_vtl, d_close_vtl_loop); } diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 69c1ad204..79628c50d 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -690,8 +690,8 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, double *resp, char *msg, - std::vector &tropo_vec, - std::vector &iono_vec, + std::vector &tropo_vec, + std::vector &iono_vec, std::vector &pr_corrected_code_bias_vec, std::vector &pr_residual_vec, std::vector &doppler_residual_vec) @@ -906,8 +906,8 @@ int raim_fde(const obsd_t *obs, int n, const double *rs, std::vector pr_residual_vec; std::vector doppler_residual_vec; /* estimate receiver position without a satellite */ - if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, - vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec,pr_residual_vec,doppler_residual_vec)) + if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, + vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec)) { trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg); continue; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index c6a61372c..0ca0854b9 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -2802,7 +2802,7 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, /* rover position by single point positioning */ if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg, tropo_vec, - iono_vec, pr_corrected_code_bias_vec,pr_residual_vec,doppler_residual_vec)) + iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec)) { errmsg(rtk, "point pos error (%s)\n", msg); if (!rtk->opt.dynamics) @@ -2842,9 +2842,9 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, return 1; } if (opt->mode == PMODE_MOVEB) - { /* moving baseline */ + { /* moving baseline */ /* estimate position/velocity of base station */ - if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, + if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec)) { errmsg(rtk, "base station position error (%s)\n", msg); diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index fcf2b0cdc..8c5bc80bb 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -602,7 +602,7 @@ void *rtksvrthread(void *arg) std::vector pr_residual; std::vector doppler_residual; rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, tropo_vec, - iono_vec, pr_corrected_code_bias_vec,pr_residual,doppler_residual); + iono_vec, pr_corrected_code_bias_vec, pr_residual, doppler_residual); rtksvrunlock(svr); if (svr->rtk.sol.stat != SOLQ_NONE) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 43c09b258..6bfee663b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -657,13 +657,13 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) double old_code_phase_chips = d_x_old_old(0); if (cmd->enable_carrier_nco_cmd) - { - d_x_old_old(2) = tmp_x(2); // replace DOPPLER - } + { + d_x_old_old(2) = tmp_x(2); // replace DOPPLER + } else - { - // std::cout << "correction not applied" << std::endl; - } + { + // std::cout << "correction not applied" << std::endl; + } // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; From ee791a357c2bc75859cdeb0e9be96209625b85dc Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Thu, 18 Jan 2024 18:24:24 +0100 Subject: [PATCH 161/165] make clang-format happy --- src/algorithms/PVT/libs/vtl_conf.h | 2 +- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_conf.h b/src/algorithms/PVT/libs/vtl_conf.h index eb5bfadb3..90258786c 100644 --- a/src/algorithms/PVT/libs/vtl_conf.h +++ b/src/algorithms/PVT/libs/vtl_conf.h @@ -31,7 +31,7 @@ class Vtl_Conf { public: Vtl_Conf(); - //TODO: VTL control parameters and config options here + // TODO: VTL control parameters and config options here }; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 0ca0854b9..1d03f4882 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -2842,7 +2842,7 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, return 1; } if (opt->mode == PMODE_MOVEB) - { /* moving baseline */ + { /* moving baseline */ /* estimate position/velocity of base station */ if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec)) From 7a51a602d01e00c4f399b8aceeeff13942247a98 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Fri, 19 Jan 2024 13:46:56 +0100 Subject: [PATCH 162/165] make clang-tidy happy --- src/algorithms/PVT/libs/rtklib_solver.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index eb084a899..1c6a15158 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -2197,7 +2197,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } // VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file - if (enable_vtl == true) + //if (enable_vtl == true) try { From ed4478734020446d38e5601787a1b06633486ed3 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Fri, 19 Jan 2024 14:05:37 +0100 Subject: [PATCH 163/165] make clang-tidy happy --- src/algorithms/PVT/libs/vtl_conf.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/algorithms/PVT/libs/vtl_conf.cc b/src/algorithms/PVT/libs/vtl_conf.cc index 6353ad7f3..b685d7db1 100644 --- a/src/algorithms/PVT/libs/vtl_conf.cc +++ b/src/algorithms/PVT/libs/vtl_conf.cc @@ -19,4 +19,5 @@ Vtl_Conf::Vtl_Conf() { + } From 0995d0bf3012949e56bda3a162d1e706f1f4ab40 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Fri, 19 Jan 2024 17:22:50 +0100 Subject: [PATCH 164/165] mod: vtl_engine_get_geodetic --- src/algorithms/PVT/libs/rtklib_solver.cc | 178 +++++++++++------------ src/algorithms/PVT/libs/vtl_conf.cc | 1 - src/algorithms/PVT/libs/vtl_engine.cc | 19 ++- src/algorithms/PVT/libs/vtl_engine.h | 4 +- 4 files changed, 99 insertions(+), 103 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 1c6a15158..d9c90050b 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -2197,107 +2197,107 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } // VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file - //if (enable_vtl == true) + // if (enable_vtl == true) + try + { + double tmp_double; + uint32_t tmp_uint32; + // TOW + tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; + d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // WEEK + tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); + d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // PVT GPS time + tmp_double = gnss_observables_map.cbegin()->second.RX_time; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // VTL filtered User clock offset [s] + tmp_double = vtl_engine.get_user_clock_offset_s(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // VTL filtered User clock offset drift[s/s] + tmp_double = vtl_engine.get_user_clock_offset_drift_s_s(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - try - { - double tmp_double; - uint32_t tmp_uint32; - // TOW - tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; - d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); - // WEEK - tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); - d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); - // PVT GPS time - tmp_double = gnss_observables_map.cbegin()->second.RX_time; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // VTL filtered User clock offset [s] - tmp_double = vtl_engine.get_user_clock_offset_s(); - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // VTL filtered User clock offset drift[s/s] - tmp_double = vtl_engine.get_user_clock_offset_drift_s_s(); - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // rtklib User clock offset drift[s/s] + tmp_double = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // rtklib User clock offset drift[s/s] - tmp_double = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double) + std::vector p_vec_m = vtl_engine.get_position_ecef_m(); + tmp_double = p_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double) - std::vector p_vec_m = vtl_engine.get_position_ecef_m(); - tmp_double = p_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = p_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = p_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + std::vector v_vec_m = vtl_engine.get_velocity_ecef_m_s(); + tmp_double = v_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector v_vec_m = vtl_engine.get_velocity_ecef_m_s(); - tmp_double = v_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = v_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = v_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + std::vector a_vec_m = vtl_engine.get_accel_ecef_m_s2(); + tmp_double = a_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector a_vec_m = vtl_engine.get_accel_ecef_m_s2(); - tmp_double = a_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = a_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = a_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // position/velocity/acceleration variance/ (units^2) (9 x double) - // position/velocity/acceleration variance/ (units^2) (9 x double) - - std::vector p_var_vec_m = vtl_engine.get_position_var_ecef_m(); - tmp_double = p_var_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = p_var_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = p_var_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + std::vector p_var_vec_m = vtl_engine.get_position_var_ecef_m(); + tmp_double = p_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); - tmp_double = v_var_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = v_var_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = v_var_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + std::vector v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); + tmp_double = v_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - vector a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); - tmp_double = a_var_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = a_var_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = a_var_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + vector a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); + tmp_double = a_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Latitude [deg] - tmp_double = this->get_latitude(); - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Longitude [deg] - tmp_double = this->get_longitude(); - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Height [m] - tmp_double = this->get_height(); - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Latitude [rad] + vector geo_vec_m = vtl_engine.get_geodetic_rad_m(); + tmp_double = geo_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Longitude [rad] + tmp_double = geo_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Height [m] + tmp_double = geo_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // NUMBER OF VALID SATS - d_vtl_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); + // NUMBER OF VALID SATS + d_vtl_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); - // GDOP / PDOP / HDOP / VDOP - d_vtl_dump_file.write(reinterpret_cast(&d_dop[0]), sizeof(double)); - d_vtl_dump_file.write(reinterpret_cast(&d_dop[1]), sizeof(double)); - d_vtl_dump_file.write(reinterpret_cast(&d_dop[2]), sizeof(double)); - d_vtl_dump_file.write(reinterpret_cast(&d_dop[3]), sizeof(double)); - } - catch (const std::ofstream::failure &e) - { - LOG(WARNING) << "Exception writing VTL dump file " << e.what(); - } + // GDOP / PDOP / HDOP / VDOP + d_vtl_dump_file.write(reinterpret_cast(&d_dop[0]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[1]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[2]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[3]), sizeof(double)); + } + catch (const std::ofstream::failure &e) + { + LOG(WARNING) << "Exception writing VTL dump file " << e.what(); + } } } } diff --git a/src/algorithms/PVT/libs/vtl_conf.cc b/src/algorithms/PVT/libs/vtl_conf.cc index b685d7db1..6353ad7f3 100644 --- a/src/algorithms/PVT/libs/vtl_conf.cc +++ b/src/algorithms/PVT/libs/vtl_conf.cc @@ -19,5 +19,4 @@ Vtl_Conf::Vtl_Conf() { - } diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index cbfecb786..562862f86 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -15,6 +15,7 @@ */ #include "vtl_engine.h" +#include "rtklib_rtkcmn.h" #include "iostream" #include @@ -442,19 +443,17 @@ std::vector Vtl_Engine::get_accel_var_ecef_m_s2() return temp; } -double Vtl_Engine::get_latitude() +std::vector Vtl_Engine::get_geodetic_rad_m() { - return -1.0; -} + std::array temp_ecef = {kf_x[0], kf_x[1], kf_x[2]}; + std::array temp_geo = {42, 42, 42}; -double Vtl_Engine::get_longitude() -{ - return -1.0; -} + ecef2pos(temp_ecef.data(), temp_geo.data()); -double Vtl_Engine::get_height() -{ - return -1.0; + std::vector dest; + dest.insert(dest.begin(), std::begin(temp_geo), std::end(temp_geo)); + + return dest; } double Vtl_Engine::get_user_clock_offset_s() diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 122d51041..d07f31c3f 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -53,9 +53,7 @@ public: std::vector get_position_var_ecef_m(); // get_position_var_ecef_m std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s std::vector get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 - double get_latitude(); // get_latitude - double get_longitude(); // get_longitude - double get_height(); // get_height + std::vector get_geodetic_rad_m(); // get_geodetic_rad_m double get_user_clock_offset_s(); // get_user_clock_offset_s; double get_user_clock_offset_drift_s_s(); // get_user_clock_offset_drift_s/s; From 601f2eb6a2d46e38e56d2b19766e072af989f869 Mon Sep 17 00:00:00 2001 From: miguekf Date: Thu, 1 Feb 2024 22:24:21 +0100 Subject: [PATCH 165/165] Merge: changes about versions and pvt_kf --- AUTHORS | 10 +++++---- CITATION.cff | 15 ++++++++++--- CMakeLists.txt | 4 ++-- cmake/Modules/RemoveDuplicates.cmake | 22 ------------------- docs/CHANGELOG.md | 16 ++++++++++++-- docs/manpage/gnss-sdr-manpage | 2 +- src/algorithms/PVT/adapters/rtklib_pvt.cc | 4 ++-- src/algorithms/PVT/libs/geohash.cc | 2 +- .../volk_gnsssdr/CMakeLists.txt | 2 +- .../volk_gnsssdr-config-info-manpage | 2 +- .../Packaging/volk_gnsssdr_profile-manpage | 2 +- .../signal_source/libs/ad936x_iio_custom.cc | 4 ++-- src/main/CMakeLists.txt | 2 -- src/main/main.cc | 2 +- src/tests/CMakeLists.txt | 18 +++++---------- src/tests/system-tests/position_test.cc | 1 + 16 files changed, 51 insertions(+), 57 deletions(-) delete mode 100644 cmake/Modules/RemoveDuplicates.cmake diff --git a/AUTHORS b/AUTHORS index 653f8a16c..ad0660662 100644 --- a/AUTHORS +++ b/AUTHORS @@ -4,9 +4,9 @@ GNSS-SDR Authorship The GNSS-SDR project is hosted and sponsored by the Centre Tecnològic de Telecomunicacions de Catalunya (CTTC), a non-profit research foundation located in Castelldefels (41.27504 N, 1.987709 E), 20 km south of Barcelona, Spain. -GNSS-SDR is the by-product of GNSS research conducted at the Communications -Systems Division of CTTC, and it is the combined effort of students, -software engineers and researchers from different institutions around the World. +GNSS-SDR is the by-product of GNSS research conducted from the Navigation & +Positioning Research Unit at CTTC, and it is the combined effort of students, +aficionados, and researchers from different institutions around the World. Contact Information -------------------------------------------------------------------------------- @@ -48,10 +48,12 @@ Cillian O'Driscoll cillian.odriscoll@gmail.com Contributor Damian Miralles dmiralles2009@gmail.com Contributor Daniel Fehr daniel.co@bluewin.ch Contributor David Pubill david.pubill@cttc.cat Contributor +En Shin seanstone5923@gmail.com Contributor Fran Fabra fabra@ice.csic.es Contributor Gabriel Araujo gabriel.araujo.5000@gmail.com Contributor Gerald LaMountain gerald@gece.neu.edu Contributor Into Pääkkönen into.paakkonen@aalto.fi Contributor +Irene Pérez Riega iperrie@inta.es Contributor Jim Melton jim.melton@sncorp.com Contributor Josh Schindehette jschindehette@geontech.com Contributor Leonardo Tonetto tonetto.dev@gmail.com Contributor @@ -66,4 +68,4 @@ Will Silberman wsilberm@google.com Contributor Carlos Paniego carpanie@hotmail.com Artwork # SPDX-License-Identifier: GPL-3.0-or-later -# SPDX-FileCopyrightText: 2011-2022 Carles Fernandez-Prades +# SPDX-FileCopyrightText: 2011-2024 Carles Fernandez-Prades diff --git a/CITATION.cff b/CITATION.cff index 27266990b..0f11b55f1 100644 --- a/CITATION.cff +++ b/CITATION.cff @@ -1,5 +1,5 @@ # SPDX-License-Identifier: GPL-3.0-or-later -# SPDX-FileCopyrightText: 2023 C. Fernandez-Prades carles.fernandez(at)cttc.es +# SPDX-FileCopyrightText: 2024 C. Fernandez-Prades carles.fernandez(at)cttc.es --- authors: - affiliation: "Centre Tecnològic de Telecomunicacions de Catalunya (CTTC)" @@ -87,6 +87,11 @@ authors: email: into.paakkonen@aalto.fi family-names: "Pääkkönen" given-names: Into + - alias: Perrielornitorrinco + affiliation: "Instituto Nacional de Técnica Aeroespacial" + email: iperrie@inta.es + given-names: Irene + family-names: "Pérez Riega" - affiliation: "Centre Tecnològic de Telecomunicacions de Catalunya (CTTC)" email: david.pubill@cttc.cat family-names: Pubill @@ -94,6 +99,10 @@ authors: - alias: antonioramosdet family-names: Ramos given-names: Antonio + - alias: seanstone + email: seanstone5923@gmail.com + family-names: Shin + given-names: En - alias: jschindehette email: jschindehette@geontech.com family-names: Schindehette @@ -110,7 +119,7 @@ authors: family-names: "van der Linden" given-names: Stefan cff-version: "1.2.0" -date-released: "2023-04-06" +date-released: "2024-01-23" identifiers: - description: "The concept DOI of the work. This is a DOI always pointing to the latest stable release." type: doi @@ -318,4 +327,4 @@ repository-code: "https://github.com/gnss-sdr/gnss-sdr" title: GNSS-SDR type: software url: "https://gnss-sdr.org" -version: "0.0.18" +version: "0.0.19" diff --git a/CMakeLists.txt b/CMakeLists.txt index afc81e834..99e567462 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -178,9 +178,9 @@ endif() set(VERSION_INFO_MAJOR_VERSION 0) set(VERSION_INFO_API_COMPAT 0) if(${THIS_IS_A_RELEASE}) - set(VERSION_INFO_MINOR_VERSION 18) + set(VERSION_INFO_MINOR_VERSION 19) else() - set(VERSION_INFO_MINOR_VERSION 18.git-${GIT_BRANCH}-${GIT_COMMIT_HASH}) + set(VERSION_INFO_MINOR_VERSION 19.git-${GIT_BRANCH}-${GIT_COMMIT_HASH}) endif() set(VERSION ${VERSION_INFO_MAJOR_VERSION}.${VERSION_INFO_API_COMPAT}.${VERSION_INFO_MINOR_VERSION}) diff --git a/cmake/Modules/RemoveDuplicates.cmake b/cmake/Modules/RemoveDuplicates.cmake deleted file mode 100644 index 446dedbae..000000000 --- a/cmake/Modules/RemoveDuplicates.cmake +++ /dev/null @@ -1,22 +0,0 @@ -# GNSS-SDR is a Global Navigation Satellite System software-defined receiver. -# This file is part of GNSS-SDR. -# -# SPDX-FileCopyrightText: 2024 C. Fernandez-Prades cfernandez(at)cttc.es -# SPDX-License-Identifier: BSD-3-Clause - -# Usage: -# include(RemoveDuplicates) -# remove_duplicate_linked_libraries(my_target) - -if(DEFINED __INCLUDED_REMOVE_DUPLICATE_LINKED_LIBRARIES_MODULE) - return() -endif() -set(__INCLUDED_REMOVE_DUPLICATE_LINKED_LIBRARIES_MODULE TRUE) - -function(remove_duplicate_linked_libraries target_name) - if(CMAKE_VERSION VERSION_GREATER 3.5) - get_target_property(LINK_LIBRARIES ${target_name} LINK_LIBRARIES) - list(REMOVE_DUPLICATES LINK_LIBRARIES) - set_target_properties(${target_name} PROPERTIES LINK_LIBRARIES "${LINK_LIBRARIES}") - endif() -endfunction() \ No newline at end of file diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index 6f5ca432b..0a1b7fee4 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -14,6 +14,18 @@ All notable changes to GNSS-SDR will be documented in this file. ## [Unreleased](https://github.com/gnss-sdr/gnss-sdr/tree/next) +### Improvements in Usability + +- Tidy up the `conf/` folder. + +## [GNSS-SDR v0.0.19.1](https://github.com/gnss-sdr/gnss-sdr/releases/tag/v0.0.19.1) - 2024-01-26 + +[![DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.10579595.svg)](https://doi.org/10.5281/zenodo.10579595) + +- Fix formatting of the `CITATION.cff` file. + +## [GNSS-SDR v0.0.19](https://github.com/gnss-sdr/gnss-sdr/releases/tag/v0.0.19) - 2024-01-23 + ### Improvements in Efficiency: - Fixed some performance inefficiencies detected by Coverity Scan. @@ -61,8 +73,8 @@ All notable changes to GNSS-SDR will be documented in this file. covariances with the following optional parameters (here with their default values): `PVT.kf_measures_ecef_pos_sd_m=1.0`, in [m]; `PVT.kf_measures_ecef_vel_sd_ms=0.1`, in [m/s]; - `PVT.kf_system_ecef_pos_sd_m=0.01`, in [m]; and - `PVT.kf_system_ecef_vel_sd_ms=0.001`, in [m/s]. + `PVT.kf_system_ecef_pos_sd_m=2.0`, in [m]; and + `PVT.kf_system_ecef_vel_sd_ms=0.5`, in [m/s]. ### Improvements in Scalability: diff --git a/docs/manpage/gnss-sdr-manpage b/docs/manpage/gnss-sdr-manpage index c9dce337f..60e2564e4 100644 --- a/docs/manpage/gnss-sdr-manpage +++ b/docs/manpage/gnss-sdr-manpage @@ -2,7 +2,7 @@ .\" SPDX-License-Identifier: GPL-3.0-or-later .\" SPDX-FileCopyrightText: Carles Fernandez-Prades .\" Contact carles.fernandez@cttc.es to correct errors or typos. -.TH gnss\-sdr 1 "6 Apr 2023" "0.0.18" "gnss\-sdr man page" +.TH gnss\-sdr 1 "23 Jan 2024" "0.0.19" "gnss\-sdr man page" .SH NAME \fBgnss\-sdr\fR \- GNSS Software Defined Receiver. .SH SYNOPSIS diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index c02ed3ae5..a81ccbdef 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -78,8 +78,8 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, pvt_output_parameters.enable_pvt_kf = configuration->property(role + ".enable_pvt_kf", false); pvt_output_parameters.measures_ecef_pos_sd_m = configuration->property(role + ".kf_measures_ecef_pos_sd_m", 1.0); pvt_output_parameters.measures_ecef_vel_sd_ms = configuration->property(role + ".kf_measures_ecef_vel_sd_ms", 0.1); - pvt_output_parameters.system_ecef_pos_sd_m = configuration->property(role + ".kf_system_ecef_pos_sd_m", 0.01); - pvt_output_parameters.system_ecef_vel_sd_ms = configuration->property(role + ".kf_system_ecef_vel_sd_ms", 0.001); + pvt_output_parameters.system_ecef_pos_sd_m = configuration->property(role + ".kf_system_ecef_pos_sd_m", 2.0); + pvt_output_parameters.system_ecef_vel_sd_ms = configuration->property(role + ".kf_system_ecef_vel_sd_ms", 0.5); // NMEA Printer settings pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); diff --git a/src/algorithms/PVT/libs/geohash.cc b/src/algorithms/PVT/libs/geohash.cc index 33f14c705..ecc1f6f3a 100644 --- a/src/algorithms/PVT/libs/geohash.cc +++ b/src/algorithms/PVT/libs/geohash.cc @@ -151,7 +151,7 @@ std::array Geohash::bounds(std::string geohash) const for (char chr : geohash) { - int idx = base32.find(chr); + auto idx = static_cast(base32.find(chr)); if (idx == -1) { throw std::runtime_error("Invalid geohash"); diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt index a65a33172..bca6d3d23 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt @@ -206,7 +206,7 @@ message(STATUS "Build type set to ${CMAKE_BUILD_TYPE}.") set(VERSION_INFO_MAJOR_VERSION 0) set(VERSION_INFO_MINOR_VERSION 0) -set(VERSION_INFO_MAINT_VERSION 18) +set(VERSION_INFO_MAINT_VERSION 19) include(VolkGnsssdrVersion) # setup version info diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr-config-info-manpage b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr-config-info-manpage index bd5f400de..b1fbc390d 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr-config-info-manpage +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr-config-info-manpage @@ -2,7 +2,7 @@ .\" SPDX-License-Identifier: GPL-3.0-or-later .\" SPDX-FileCopyrightText: Carles Fernandez-Prades .\" Contact carles.fernandez@cttc.es to correct errors or typos. -.TH volk_gnsssdr\-config\-info 1 "6 Apr 2023" "0.0.18" "volk_gnsssdr\-config\-info man page" +.TH volk_gnsssdr\-config\-info 1 "23 Jan 2024" "0.0.19" "volk_gnsssdr\-config\-info man page" .SH NAME \fBvolk_gnsssdr\-config\-info\fR \- Prints configuration information of libvolk_gnsssdr functions. .SH SYNOPSIS diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr_profile-manpage b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr_profile-manpage index 19636133c..36febf4e3 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr_profile-manpage +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr_profile-manpage @@ -2,7 +2,7 @@ .\" SPDX-License-Identifier: GPL-3.0-or-later .\" SPDX-FileCopyrightText: Carles Fernandez-Prades .\" Contact carles.fernandez@cttc.es to correct errors or typos. -.TH volk_gnsssdr_profile 1 "6 Apr 2023" "0.0.18" "volk_gnsssdr_profile man page" +.TH volk_gnsssdr_profile 1 "23 Jan 2024" "0.0.19" "volk_gnsssdr_profile man page" .SH NAME \fBvolk_gnsssdr_profile\fR \- Profiler application for libvolk_gnsssdr functions. .SH SYNOPSIS diff --git a/src/algorithms/signal_source/libs/ad936x_iio_custom.cc b/src/algorithms/signal_source/libs/ad936x_iio_custom.cc index 23474891c..ce25e7897 100644 --- a/src/algorithms/signal_source/libs/ad936x_iio_custom.cc +++ b/src/algorithms/signal_source/libs/ad936x_iio_custom.cc @@ -171,7 +171,7 @@ void ad936x_iio_custom::configure_params(struct iio_device *phy, pos = it->find('='); if (pos == std::string::npos) { - std::cerr << "Misformed line: " << *it << std::endl; + std::cerr << "Malformed line: " << *it << std::endl; continue; } @@ -686,7 +686,7 @@ bool ad936x_iio_custom::init_config_ad9361_rx(long long bandwidth_, } std::cout << "AD936x Front-end configuration summary: \n"; - std::cout << "RF frequency tunned in AD936x: " << freq_ << " [Hz]\n"; + std::cout << "RF frequency tuned in AD936x: " << freq_ << " [Hz]\n"; std::cout << "Baseband sampling frequency: " << sample_rate_sps << " [SPS]\n"; std::cout << "RX chain gain: " << rf_gain_rx0_ << " [dB][only valid in manual mode]\n"; std::cout << "RX chain gain mode: " << gain_mode_rx0_ << "\n"; diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 05b96fd5f..2895d5973 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -60,8 +60,6 @@ if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") target_link_libraries(gnss-sdr PRIVATE "-lc++") endif() endif() -include(RemoveDuplicates) -remove_duplicate_linked_libraries(gnss-sdr) if(ENABLE_STRIP) set_target_properties(gnss-sdr PROPERTIES LINK_FLAGS "-s") diff --git a/src/main/main.cc b/src/main/main.cc index f01f050ee..e4715a5c3 100644 --- a/src/main/main.cc +++ b/src/main/main.cc @@ -18,7 +18,7 @@ */ #ifndef GNSS_SDR_VERSION -#define GNSS_SDR_VERSION "0.0.18" +#define GNSS_SDR_VERSION "0.0.19" #endif #ifndef GOOGLE_STRIP_LOG diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 94ae13246..5863284ba 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -7,7 +7,6 @@ add_subdirectory(unit-tests/signal-processing-blocks/libs) add_subdirectory(system-tests/libs) -include(RemoveDuplicates) ################################################################################ # Google Test - https://github.com/google/googletest @@ -646,7 +645,6 @@ if(ENABLE_UNIT_TESTING) target_compile_definitions(run_tests PRIVATE -DGNSSTK_OLDER_THAN_9=1) endif() endif() - remove_duplicate_linked_libraries(run_tests) if(ENABLE_STRIP) set_target_properties(run_tests PROPERTIES LINK_FLAGS "-s") endif() @@ -769,7 +767,6 @@ if(ENABLE_FPGA) target_include_directories(gps_l1_ca_dll_pll_tracking_test_fpga INTERFACE ${GNSSSDR_SOURCE_DIR}/src/tests/common-files ) - remove_duplicate_linked_libraries(gps_l1_ca_dll_pll_tracking_test_fpga) install(TARGETS gps_l1_ca_dll_pll_tracking_test_fpga RUNTIME DESTINATION bin COMPONENT "fpga-test" @@ -859,7 +856,6 @@ if(ENABLE_SYSTEM_TESTING) ) if(NOT ENABLE_PACKAGING) add_system_test(ttff) - remove_duplicate_linked_libraries(ttff) endif() if(ENABLE_SYSTEM_TESTING_EXTRA) @@ -881,7 +877,6 @@ if(ENABLE_SYSTEM_TESTING) ) endif() endif() - remove_duplicate_linked_libraries(position_test) if(NOT GNSSSIMULATOR_FOUND OR ENABLE_GNSS_SIM_INSTALL) if(NOT CMAKE_CROSSCOMPILING) add_dependencies(position_test gnss-sim) @@ -942,7 +937,6 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) PRIVATE ${GNSSSDR_SOURCE_DIR}/src/algorithms/libs ) - remove_duplicate_linked_libraries(flowgraph_test) add_test(flowgraph_test flowgraph_test) set_property(TEST flowgraph_test PROPERTY TIMEOUT 30) @@ -997,7 +991,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) if(ENABLE_FPGA) target_compile_definitions(gnss_block_test PRIVATE -DENABLE_FPGA=1) endif() - remove_duplicate_linked_libraries(gnss_block_test) + add_test(gnss_block_test gnss_block_test) set_property(TEST gnss_block_test PROPERTY TIMEOUT 60) @@ -1034,7 +1028,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) core_receiver algorithms_libs ) - remove_duplicate_linked_libraries(gnuradio_block_test) + add_test(gnuradio_block_test gnuradio_block_test) set_property(TEST gnuradio_block_test PROPERTY TIMEOUT 30) @@ -1068,7 +1062,7 @@ target_include_directories(matio_test INTERFACE ${GNSSSDR_SOURCE_DIR}/src/tests/common-files ) -remove_duplicate_linked_libraries(matio_test) + add_test(matio_test matio_test) set_property(TEST matio_test PROPERTY TIMEOUT 30) @@ -1117,7 +1111,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) -DPMT_USES_BOOST_ANY=1 ) endif() - remove_duplicate_linked_libraries(acq_test) + add_test(acq_test acq_test) if(USE_GENERIC_LAMBDAS) @@ -1213,7 +1207,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) -DPMT_USES_BOOST_ANY=1 ) endif() - remove_duplicate_linked_libraries(trk_test) + add_test(trk_test trk_test) set_property(TEST trk_test PROPERTY TIMEOUT 30) @@ -1244,7 +1238,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) algorithms_libs core_receiver ) - remove_duplicate_linked_libraries(control_thread_test) + add_test(control_thread_test control_thread_test) set_property(TEST control_thread_test PROPERTY TIMEOUT 30) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index a2bb5181d..5802aa9ca 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -312,6 +312,7 @@ int PositionSystemTest::configure_receiver() // Set PVT config->set_property("PVT.implementation", "RTKLIB_PVT"); + config->set_property("PVT.enable_pvt_kf", "true"); config->set_property("PVT.positioning_mode", "PPP_Static"); config->set_property("PVT.output_rate_ms", std::to_string(output_rate_ms)); config->set_property("PVT.display_rate_ms", std::to_string(display_rate_ms));