From 36e709dda646484c44eb6591db1d9e429c624922 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Tue, 4 Jul 2023 13:09:45 +0200 Subject: [PATCH] Adding a simple PVT Holonomic Kalman filter for position and velocity --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 7 + .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 7 +- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 1 - src/algorithms/PVT/libs/CMakeLists.txt | 2 + src/algorithms/PVT/libs/pvt_conf.h | 7 + src/algorithms/PVT/libs/pvt_kf.cc | 132 ++++++++++++++++++ src/algorithms/PVT/libs/pvt_kf.h | 66 +++++++++ src/algorithms/PVT/libs/rtklib_solver.cc | 51 ++++++- src/algorithms/PVT/libs/rtklib_solver.h | 7 +- .../pvt/nmea_printer_test.cc | 5 +- .../pvt/rinex_printer_test.cc | 41 ++++-- .../pvt/rtklib_solver_test.cc | 6 +- 12 files changed, 305 insertions(+), 27 deletions(-) create mode 100644 src/algorithms/PVT/libs/pvt_kf.cc create mode 100644 src/algorithms/PVT/libs/pvt_kf.h diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 9ce402bcc..9bf6174ab 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -74,6 +74,13 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, // display rate pvt_output_parameters.display_rate_ms = bc::lcm(pvt_output_parameters.output_rate_ms, configuration->property(role + ".display_rate_ms", 500)); + // PVT KF settings + 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); + // NMEA Printer settings pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); pvt_output_parameters.nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 8f6f1648a..ab145aff4 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -178,7 +178,6 @@ 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_has_corrections(conf_.use_has_corrections), d_use_unhealthy_sats(conf_.use_unhealthy_sats) { @@ -535,21 +534,21 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, { // setup two PVT solvers: internal solver for rx clock and user solver // user PVT solver - d_user_pvt_solver = std::make_shared(rtk, dump_ls_pvt_filename, d_type_of_rx, d_dump, d_dump_mat, d_use_e6_for_pvt); + d_user_pvt_solver = std::make_shared(rtk, dump_ls_pvt_filename, d_type_of_rx, d_dump, d_dump_mat, conf_); d_user_pvt_solver->set_averaging_depth(1); d_user_pvt_solver->set_pre_2009_file(conf_.pre_2009_file); // internal PVT solver, mainly used to estimate the receiver clock rtk_t internal_rtk = rtk; internal_rtk.opt.mode = PMODE_SINGLE; // use single positioning mode in internal PVT solver - d_internal_pvt_solver = std::make_shared(internal_rtk, dump_ls_pvt_filename, d_type_of_rx, false, false, d_use_e6_for_pvt); + d_internal_pvt_solver = std::make_shared(internal_rtk, dump_ls_pvt_filename, d_type_of_rx, false, false, conf_); d_internal_pvt_solver->set_averaging_depth(1); d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file); } else { // only one solver, customized by the user options - d_internal_pvt_solver = std::make_shared(rtk, dump_ls_pvt_filename, d_type_of_rx, d_dump, d_dump_mat, d_use_e6_for_pvt); + d_internal_pvt_solver = std::make_shared(rtk, dump_ls_pvt_filename, d_type_of_rx, d_dump, d_dump_mat, conf_); d_internal_pvt_solver->set_averaging_depth(1); d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file); d_user_pvt_solver = d_internal_pvt_solver; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 747c31c65..83d1f830d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -276,7 +276,6 @@ private: bool d_enable_has_messages; bool d_an_printer_enabled; bool d_log_timetag; - bool d_use_e6_for_pvt; bool d_use_has_corrections; bool d_use_unhealthy_sats; }; diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 2460a3d4b..07e905a2c 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -23,6 +23,7 @@ set(PVT_LIB_SOURCES monitor_ephemeris_udp_sink.cc has_simple_printer.cc geohash.cc + pvt_kf.cc ) set(PVT_LIB_HEADERS @@ -45,6 +46,7 @@ set(PVT_LIB_HEADERS monitor_ephemeris_udp_sink.h has_simple_printer.h geohash.h + pvt_kf.h ) list(SORT PVT_LIB_HEADERS) diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index d77a0b24c..26aac201a 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -94,6 +94,13 @@ public: bool use_e6_for_pvt = true; bool use_has_corrections = true; bool use_unhealthy_sats = false; + + //PVT KF parameters + bool enable_pvt_kf = false; + double measures_ecef_pos_sd_m = 1.0; + double measures_ecef_vel_sd_ms = 0.1; + double system_ecef_pos_sd_m = 0.01; + double system_ecef_vel_sd_ms = 0.001; }; diff --git a/src/algorithms/PVT/libs/pvt_kf.cc b/src/algorithms/PVT/libs/pvt_kf.cc new file mode 100644 index 000000000..be7871974 --- /dev/null +++ b/src/algorithms/PVT/libs/pvt_kf.cc @@ -0,0 +1,132 @@ +/*! + * \file Pvt_Kf.cc + * \brief Kalman Filter for Position and Velocity + * \author Javier Arribas, 2023. 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-2023 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ +#include "pvt_kf.h" +#include + + +void Pvt_Kf::init_kf(arma::vec p, arma::vec v, + double solver_interval_s, + double measures_ecef_pos_sd_m, + double measures_ecef_vel_sd_ms, + double system_ecef_pos_sd_m, + double system_ecef_vel_sd_ms) +{ + // Kalman Filter class variables + const double Ti = solver_interval_s; + + std::cout << "Ti=" << Ti << "\n"; + d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0}, + {0.0, 1.0, 0.0, 0.0, Ti, 0.0}, + {0.0, 0.0, 1.0, 0.0, 0.0, Ti}, + {0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}}; + + d_H = arma::eye(6, 6); + + //measurement matrix static covariances + d_R = {{pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}}; + + + // system covariance matrix (static) + + d_Q = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0)}}; + + // initial Kalman covariance matrix + d_P_old_old = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0)}}; + + // states: position ecef [m], velocity ecef [m/s] + d_x_old_old = arma::zeros(6); + d_x_old_old.subvec(0, 2) = p; + d_x_old_old.subvec(3, 5) = v; + + initialized = true; + + DLOG(INFO) << "F: " << d_F; + DLOG(INFO) << "H: " << d_H; + DLOG(INFO) << "R: " << d_R; + DLOG(INFO) << "Q: " << d_Q; + DLOG(INFO) << "P: " << d_P_old_old; + DLOG(INFO) << "x: " << d_x_old_old; +} + +void Pvt_Kf::run_Kf(arma::vec p, arma::vec v) +{ + // + // Pkp{1,i}=F*Pk{1,i-1}*F'+Q; + // Xkp{1,i}=F*Xk{1,i-1}; %nuevo estado a partir del modelo de transición + // + // %% Ganancia de Kalman + // + // K=Pkp{1,i}*A'*inv(A*Pkp{1,i}*A'+R); %en base a lo que acaba de predecir + // + // %% Corrección de la predicción y la covarianza, usando la info de las + // %% observaciones + // + // Xk{1,i}=Xkp{1,i}+K*(Z{1,i}-A*Xkp{1,i}); %correccion de lo predicho en base a la observación Z + // + // Pk{1,i}=(eye(4)-K*A)*Pkp{1,i}; % Corrección de la covarianza + // %pause(1) + // + + // Kalman loop + // Prediction + // std::cout << "d_x_old_old=" << d_x_old_old << "\n"; + d_x_new_old = d_F * d_x_old_old; + // std::cout << "d_x_new_old=" << d_x_new_old << "\n"; + d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q; + + // Measurement update + arma::vec z = arma::join_cols(p, v); + arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R); // Kalman gain + d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old); + // std::cout << "z=" << z << "\n"; + // std::cout << "K=" << K << "\n"; + + // std::cout << "d_x_new_new=" << d_x_new_new << "\n"; + d_P_new_new = (arma::eye(6, 6) - K * d_H) * d_P_new_old; + + // prepare data for next KF epoch + d_x_old_old = d_x_new_new; + d_P_old_old = d_P_new_new; +} + +Pvt_Kf::Pvt_Kf() +{ + initialized = false; +} + +void Pvt_Kf::get_pvt(arma::vec& p, arma::vec& v) +{ + p = d_x_new_new.subvec(0, 2); + v = d_x_new_new.subvec(3, 5); +} diff --git a/src/algorithms/PVT/libs/pvt_kf.h b/src/algorithms/PVT/libs/pvt_kf.h new file mode 100644 index 000000000..ce1a3f8ba --- /dev/null +++ b/src/algorithms/PVT/libs/pvt_kf.h @@ -0,0 +1,66 @@ +/*! + * \file Pvt_Kf.h + * \brief Kalman Filter for Position and Velocity + * \author Javier Arribas, 2023. 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-2023 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_PVT_KF_H +#define GNSS_SDR_PVT_KF_H + +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +/*! + * \brief Kalman Filter for Position and Velocity + * + */ +class Pvt_Kf +{ +public: + Pvt_Kf(); + virtual ~Pvt_Kf() = default; + void init_kf(arma::vec p, arma::vec v, + double solver_interval_s, + double measures_ecef_pos_sd_m, + double measures_ecef_vel_sd_ms, + double system_ecef_pos_sd_m, + double system_ecef_vel_sd_ms); + + void run_Kf(arma::vec p, arma::vec v); + bool initialized; + void get_pvt(arma::vec &p, arma::vec &v); + +private: + // Kalman Filter class variables + arma::mat d_F; + arma::mat d_H; + arma::mat d_R; + arma::mat d_Q; + arma::mat d_P_old_old; + arma::mat d_P_new_old; + arma::mat d_P_new_new; + arma::vec d_x_old_old; + arma::vec d_x_new_old; + arma::vec d_x_new_new; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_Pvt_Kf_H diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 8763cc12a..538de50df 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -49,12 +49,13 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, uint32_t type_of_rx, bool flag_dump_to_file, bool flag_dump_to_mat, - bool use_e6_for_pvt) : d_dump_filename(dump_filename), - d_rtk(rtk), - d_type_of_rx(type_of_rx), - d_flag_dump_enabled(flag_dump_to_file), - d_flag_dump_mat_enabled(flag_dump_to_mat), - d_use_e6_for_pvt(use_e6_for_pvt) + Pvt_Conf conf) : d_dump_filename(dump_filename), + d_rtk(rtk), + d_type_of_rx(type_of_rx), + d_flag_dump_enabled(flag_dump_to_file), + d_flag_dump_mat_enabled(flag_dump_to_mat), + d_conf(conf) + { this->set_averaging_flag(false); @@ -1039,7 +1040,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; } } - if (sig_ == "E6" && d_use_e6_for_pvt) + if (sig_ == "E6" && d_conf.use_e6_for_pvt) { galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) @@ -1466,6 +1467,10 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_rtk.neb = 0; // clear error buffer to avoid repeating the error message this->set_time_offset_s(0.0); // reset rx time estimation this->set_num_valid_observations(0); + if (d_conf.enable_pvt_kf == true) + { + d_pvt_kf.initialized = false; + } } else { @@ -1500,9 +1505,41 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } this->set_valid_position(true); std::array rx_position_and_time{}; + + if (d_conf.enable_pvt_kf == true) + { + if (d_pvt_kf.initialized == false) + { + arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; + arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; + + d_pvt_kf.init_kf(p, + v, + d_conf.observable_interval_ms / 1000.0, + d_conf.measures_ecef_pos_sd_m, + d_conf.measures_ecef_vel_sd_ms, + d_conf.system_ecef_pos_sd_m, + d_conf.system_ecef_vel_sd_ms); + } + else + { + arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; + arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; + d_pvt_kf.run_Kf(p, v); + d_pvt_kf.get_pvt(p, v); + pvt_sol.rr[0] = p[0]; // [m] + pvt_sol.rr[1] = p[1]; // [m] + pvt_sol.rr[2] = p[2]; // [m] + pvt_sol.rr[3] = v[0]; // [ms] + pvt_sol.rr[4] = v[1]; // [ms] + pvt_sol.rr[5] = v[2]; // [ms] + } + } + rx_position_and_time[0] = pvt_sol.rr[0]; // [m] rx_position_and_time[1] = pvt_sol.rr[1]; // [m] rx_position_and_time[2] = pvt_sol.rr[2]; // [m] + // todo: fix this ambiguity in the RTKLIB units in receiver clock offset! if (d_rtk.opt.mode == PMODE_SINGLE) { diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 0a7fc056b..b967cb02f 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -56,6 +56,8 @@ #include "gps_iono.h" #include "gps_utc_model.h" #include "monitor_pvt.h" +#include "pvt_conf.h" +#include "pvt_kf.h" #include "pvt_solution.h" #include "rtklib.h" #include "rtklib_conversions.h" @@ -84,7 +86,7 @@ public: uint32_t type_of_rx, bool flag_dump_to_file, bool flag_dump_to_mat, - bool use_e6_for_pvt = true); + Pvt_Conf conf); ~Rtklib_Solver(); bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); @@ -152,7 +154,8 @@ private: uint32_t d_type_of_rx; bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; - bool d_use_e6_for_pvt; + Pvt_Conf d_conf; + Pvt_Kf d_pvt_kf; }; diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc index 94c904f95..fcc0920be 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc @@ -17,6 +17,7 @@ #include "gnss_sdr_filesystem.h" #include "nmea_printer.h" +#include "pvt_conf.h" #include "rtklib_rtkpos.h" #include "rtklib_solver.h" #include @@ -144,7 +145,9 @@ void NmeaPrinterTest::conf() TEST_F(NmeaPrinterTest, PrintLine) { std::string filename("nmea_test.nmea"); - std::shared_ptr pvt_solution = std::make_shared(rtk, "filename", 1, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + std::shared_ptr pvt_solution = std::make_shared(rtk, "filename", 1, false, false, conf); boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19), boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc index 05f7d115e..a4a4fe90a 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc @@ -15,6 +15,7 @@ */ #include "gnss_sdr_filesystem.h" +#include "pvt_conf.h" #include "rinex_printer.h" #include "rtklib_rtkpos.h" #include "rtklib_solver.h" @@ -142,7 +143,9 @@ void RinexPrinterTest::conf() TEST_F(RinexPrinterTest, GalileoObsHeader) { - auto pvt_solution = std::make_shared(rtk, "filename", 4, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 4, false, false, conf); auto eph = Galileo_Ephemeris(); eph.PRN = 1; pvt_solution->galileo_ephemeris_map[1] = eph; @@ -228,7 +231,9 @@ TEST_F(RinexPrinterTest, GalileoObsHeader) TEST_F(RinexPrinterTest, GlonassObsHeader) { - auto pvt_solution = std::make_shared(rtk, "filename", 28, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 28, false, false, conf); auto eph = Glonass_Gnav_Ephemeris(); eph.PRN = 1; pvt_solution->glonass_gnav_ephemeris_map[1] = eph; @@ -288,7 +293,9 @@ TEST_F(RinexPrinterTest, MixedObsHeader) auto eph_gps = Gps_Ephemeris(); eph_gal.PRN = 1; eph_gps.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", 106, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 106, false, false, conf); pvt_solution->galileo_ephemeris_map[1] = eph_gal; pvt_solution->gps_ephemeris_map[1] = eph_gps; @@ -358,7 +365,9 @@ TEST_F(RinexPrinterTest, MixedObsHeaderGpsGlo) auto eph_gps = Gps_Ephemeris(); eph_glo.PRN = 1; eph_gps.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", 26, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 26, false, false, conf); pvt_solution->glonass_gnav_ephemeris_map[1] = eph_glo; pvt_solution->gps_ephemeris_map[1] = eph_gps; @@ -425,7 +434,9 @@ TEST_F(RinexPrinterTest, GalileoObsLog) bool no_more_finds = false; auto eph = Galileo_Ephemeris(); eph.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", 4, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 4, false, false, conf); pvt_solution->galileo_ephemeris_map[1] = eph; std::map gnss_observables_map; @@ -505,7 +516,9 @@ TEST_F(RinexPrinterTest, GlonassObsLog) bool no_more_finds = false; auto eph = Glonass_Gnav_Ephemeris(); eph.PRN = 22; - auto pvt_solution = std::make_shared(rtk, "filename", 23, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 23, false, false, conf); pvt_solution->glonass_gnav_ephemeris_map[1] = eph; std::map gnss_observables_map; @@ -587,7 +600,9 @@ TEST_F(RinexPrinterTest, GpsObsLogDualBand) auto eph_cnav = Gps_CNAV_Ephemeris(); eph.PRN = 1; eph_cnav.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", 7, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 7, false, false, conf); pvt_solution->gps_ephemeris_map[1] = eph; pvt_solution->gps_cnav_ephemeris_map[1] = eph_cnav; std::map gnss_observables_map; @@ -675,7 +690,9 @@ TEST_F(RinexPrinterTest, GpsObsLogDualBand) TEST_F(RinexPrinterTest, GalileoObsLogDualBand) { - auto pvt_solution = std::make_shared(rtk, "filename", 14, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 14, false, false, conf); auto eph = Galileo_Ephemeris(); eph.PRN = 1; pvt_solution->galileo_ephemeris_map[1] = eph; @@ -775,7 +792,9 @@ TEST_F(RinexPrinterTest, MixedObsLog) auto eph_gal = Galileo_Ephemeris(); eph_gps.PRN = 1; eph_gal.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", 9, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 9, false, false, conf); pvt_solution->gps_ephemeris_map[1] = eph_gps; pvt_solution->galileo_ephemeris_map[1] = eph_gal; std::map gnss_observables_map; @@ -899,7 +918,9 @@ TEST_F(RinexPrinterTest, MixedObsLogGpsGlo) auto eph_glo = Glonass_Gnav_Ephemeris(); eph_gps.PRN = 1; eph_glo.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", 26, false, false); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto pvt_solution = std::make_shared(rtk, "filename", 26, false, false, conf); pvt_solution->gps_ephemeris_map[1] = eph_gps; pvt_solution->glonass_gnav_ephemeris_map[1] = eph_glo; std::map gnss_observables_map; diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc index 4da572895..805ffdb9e 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc @@ -18,6 +18,7 @@ #include "gnss_sdr_make_unique.h" #include "gnss_sdr_supl_client.h" #include "in_memory_configuration.h" +#include "pvt_conf.h" #include "rtklib_solver.h" #include #include @@ -28,7 +29,6 @@ #include #include - rtk_t configure_rtklib_options() { std::shared_ptr configuration; @@ -384,7 +384,9 @@ TEST(RTKLibSolverTest, test1) bool save_to_mat = false; rtk_t rtk = configure_rtklib_options(); - auto d_ls_pvt = std::make_unique(rtk, nchannels, dump_filename, 1, flag_dump_to_file, save_to_mat); + Pvt_Conf conf; + conf.use_e6_for_pvt = false; + auto d_ls_pvt = std::make_unique(rtk, nchannels, dump_filename, 1, flag_dump_to_file, save_to_mat, conf); d_ls_pvt->set_averaging_depth(1); // load ephemeris