diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index ab145aff4..83c6bb231 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -535,21 +535,18 @@ 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, 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, 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, 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; } @@ -2109,7 +2106,7 @@ 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)) + if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0)) { d_pvt_errors_counter = 0; // Reset consecutive PVT error counter const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); @@ -2223,7 +2220,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, d_output_rate_ms / 1000.0); } if (flag_pvt_valid == true) @@ -2335,28 +2332,28 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { if (current_RX_time_ms % d_kml_rate_ms == 0) { - d_kml_dump->print_position(d_user_pvt_solver.get(), false); + d_kml_dump->print_position(d_user_pvt_solver.get()); } } if (d_gpx_output_enabled) { if (current_RX_time_ms % d_gpx_rate_ms == 0) { - d_gpx_dump->print_position(d_user_pvt_solver.get(), false); + d_gpx_dump->print_position(d_user_pvt_solver.get()); } } if (d_geojson_output_enabled) { if (current_RX_time_ms % d_geojson_rate_ms == 0) { - d_geojson_printer->print_position(d_user_pvt_solver.get(), false); + d_geojson_printer->print_position(d_user_pvt_solver.get()); } } if (d_nmea_output_file_enabled) { if (current_RX_time_ms % d_nmea_rate_ms == 0) { - d_nmea_printer->Print_Nmea_Line(d_user_pvt_solver.get(), false); + d_nmea_printer->Print_Nmea_Line(d_user_pvt_solver.get()); } } if (d_rinex_output_enabled) diff --git a/src/algorithms/PVT/libs/geojson_printer.cc b/src/algorithms/PVT/libs/geojson_printer.cc index 85e9e62d7..f57e9d582 100644 --- a/src/algorithms/PVT/libs/geojson_printer.cc +++ b/src/algorithms/PVT/libs/geojson_printer.cc @@ -156,24 +156,11 @@ bool GeoJSON_Printer::set_headers(const std::string& filename, bool time_tag_nam } -bool GeoJSON_Printer::print_position(const Pvt_Solution* const position, bool print_average_values) +bool GeoJSON_Printer::print_position(const Pvt_Solution* const position) { - double latitude; - double longitude; - double height; - - if (print_average_values == false) - { - latitude = position->get_latitude(); - longitude = position->get_longitude(); - height = position->get_height(); - } - else - { - latitude = position->get_avg_latitude(); - longitude = position->get_avg_longitude(); - height = position->get_avg_height(); - } + const double latitude = position->get_latitude(); + const double longitude = position->get_longitude(); + const double height = position->get_height(); if (geojson_file.is_open()) { diff --git a/src/algorithms/PVT/libs/geojson_printer.h b/src/algorithms/PVT/libs/geojson_printer.h index 98651df22..cb7c00cdb 100644 --- a/src/algorithms/PVT/libs/geojson_printer.h +++ b/src/algorithms/PVT/libs/geojson_printer.h @@ -42,7 +42,7 @@ public: explicit GeoJSON_Printer(const std::string& base_path = "."); ~GeoJSON_Printer(); bool set_headers(const std::string& filename, bool time_tag_name = true); - bool print_position(const Pvt_Solution* const position, bool print_average_values); + bool print_position(const Pvt_Solution* const position); bool close_file(); private: diff --git a/src/algorithms/PVT/libs/gpx_printer.cc b/src/algorithms/PVT/libs/gpx_printer.cc index ea33cdb1c..699f92db5 100644 --- a/src/algorithms/PVT/libs/gpx_printer.cc +++ b/src/algorithms/PVT/libs/gpx_printer.cc @@ -140,12 +140,8 @@ bool Gpx_Printer::set_headers(const std::string& filename, bool time_tag_name) } -bool Gpx_Printer::print_position(const Pvt_Solution* const position, bool print_average_values) +bool Gpx_Printer::print_position(const Pvt_Solution* const position) { - double latitude; - double longitude; - double height; - positions_printed = true; const double speed_over_ground = position->get_speed_over_ground(); // expressed in m/s @@ -162,18 +158,9 @@ bool Gpx_Printer::print_position(const Pvt_Solution* const position, bool print_ utc_time.resize(23, '0'); // time up to ms utc_time.append("Z"); // UTC time zone - if (print_average_values == false) - { - latitude = position->get_latitude(); - longitude = position->get_longitude(); - height = position->get_height(); - } - else - { - latitude = position->get_avg_latitude(); - longitude = position->get_avg_longitude(); - height = position->get_avg_height(); - } + const double latitude = position->get_latitude(); + const double longitude = position->get_longitude(); + const double height = position->get_height(); if (gpx_file.is_open()) { diff --git a/src/algorithms/PVT/libs/gpx_printer.h b/src/algorithms/PVT/libs/gpx_printer.h index 0013beeba..697ac6523 100644 --- a/src/algorithms/PVT/libs/gpx_printer.h +++ b/src/algorithms/PVT/libs/gpx_printer.h @@ -42,7 +42,7 @@ public: explicit Gpx_Printer(const std::string& base_path = "."); ~Gpx_Printer(); bool set_headers(const std::string& filename, bool time_tag_name = true); - bool print_position(const Pvt_Solution* const position, bool print_average_values); + bool print_position(const Pvt_Solution* const position); bool close_file(); private: diff --git a/src/algorithms/PVT/libs/kml_printer.cc b/src/algorithms/PVT/libs/kml_printer.cc index ad7ccb0e3..72ea272d9 100644 --- a/src/algorithms/PVT/libs/kml_printer.cc +++ b/src/algorithms/PVT/libs/kml_printer.cc @@ -210,12 +210,8 @@ bool Kml_Printer::set_headers(const std::string& filename, bool time_tag_name) } -bool Kml_Printer::print_position(const Pvt_Solution* const position, bool print_average_values) +bool Kml_Printer::print_position(const Pvt_Solution* const position) { - double latitude; - double longitude; - double height; - positions_printed = true; const double speed_over_ground = position->get_speed_over_ground(); // expressed in m/s @@ -232,18 +228,9 @@ bool Kml_Printer::print_position(const Pvt_Solution* const position, bool print_ utc_time.resize(23, '0'); // time up to ms utc_time.append("Z"); // UTC time zone - if (print_average_values == false) - { - latitude = position->get_latitude(); - longitude = position->get_longitude(); - height = position->get_height(); - } - else - { - latitude = position->get_avg_latitude(); - longitude = position->get_avg_longitude(); - height = position->get_avg_height(); - } + const double latitude = position->get_latitude(); + const double longitude = position->get_longitude(); + const double height = position->get_height(); if (kml_file.is_open() && tmp_file.is_open()) { diff --git a/src/algorithms/PVT/libs/kml_printer.h b/src/algorithms/PVT/libs/kml_printer.h index 9813cca8b..af3c535ad 100644 --- a/src/algorithms/PVT/libs/kml_printer.h +++ b/src/algorithms/PVT/libs/kml_printer.h @@ -41,7 +41,7 @@ public: explicit Kml_Printer(const std::string& base_path = std::string(".")); ~Kml_Printer(); bool set_headers(const std::string& filename, bool time_tag_name = true); - bool print_position(const Pvt_Solution* const position, bool print_average_values); + bool print_position(const Pvt_Solution* const position); bool close_file(); private: diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index 1939313ee..20c876059 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -100,7 +100,7 @@ Nmea_Printer::Nmea_Printer(const std::string& filename, { nmea_dev_descriptor = -1; } - print_avg_pos = false; + d_PVT_data = nullptr; } @@ -190,11 +190,10 @@ void Nmea_Printer::close_serial() const } -bool Nmea_Printer::Print_Nmea_Line(const Rtklib_Solver* const pvt_data, bool print_average_values) +bool Nmea_Printer::Print_Nmea_Line(const Rtklib_Solver* const pvt_data) { // set the new PVT data d_PVT_data = pvt_data; - print_avg_pos = print_average_values; // generate the NMEA sentences diff --git a/src/algorithms/PVT/libs/nmea_printer.h b/src/algorithms/PVT/libs/nmea_printer.h index 42ec117d3..58b5f0912 100644 --- a/src/algorithms/PVT/libs/nmea_printer.h +++ b/src/algorithms/PVT/libs/nmea_printer.h @@ -57,7 +57,7 @@ public: /*! * \brief Print NMEA PVT and satellite info to the initialized device */ - bool Print_Nmea_Line(const Rtklib_Solver* const pvt_data, bool print_average_values); + bool Print_Nmea_Line(const Rtklib_Solver* const pvt_data); private: int init_serial(const std::string& serial_device); // serial port control @@ -80,8 +80,6 @@ private: std::string nmea_devname; int nmea_dev_descriptor; // NMEA serial device descriptor (i.e. COM port) - - bool print_avg_pos; bool d_flag_nmea_output_file; }; diff --git a/src/algorithms/PVT/libs/pvt_kf.cc b/src/algorithms/PVT/libs/pvt_kf.cc index 10083b221..805a53a57 100644 --- a/src/algorithms/PVT/libs/pvt_kf.cc +++ b/src/algorithms/PVT/libs/pvt_kf.cc @@ -19,16 +19,16 @@ #include -void Pvt_Kf::init_kf(const arma::vec& p, +void Pvt_Kf::init_Kf(const arma::vec& p, const arma::vec& v, - double solver_interval_s, + double update_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; + const double Ti = update_interval_s; d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0}, {0.0, 1.0, 0.0, 0.0, Ti, 0.0}, @@ -68,7 +68,7 @@ void Pvt_Kf::init_kf(const arma::vec& p, d_x_old_old.subvec(0, 2) = p; d_x_old_old.subvec(3, 5) = v; - initialized = true; + d_initialized = true; DLOG(INFO) << "Ti: " << Ti; DLOG(INFO) << "F: " << d_F; @@ -80,28 +80,54 @@ void Pvt_Kf::init_kf(const arma::vec& p, } +bool Pvt_Kf::is_initialized() const +{ + return d_initialized; +} + + +void Pvt_Kf::reset_Kf() +{ + d_initialized = false; +} + + void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v) { - // Kalman loop - // Prediction - 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; + if (d_initialized) + { + // Kalman loop + // Prediction + 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; - // 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 + // Measurement update + try + { + 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); - d_P_new_new = (arma::eye(6, 6) - K * d_H) * d_P_new_old; + d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old); + 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; + // prepare data for next KF epoch + d_x_old_old = d_x_new_new; + d_P_old_old = d_P_new_new; + } + catch (...) + { + d_x_new_new = d_x_new_old; + this->reset_Kf(); + } + } } -void Pvt_Kf::get_pvt(arma::vec& p, arma::vec& v) +void Pvt_Kf::get_pv_Kf(arma::vec& p, arma::vec& v) const { - p = d_x_new_new.subvec(0, 2); - v = d_x_new_new.subvec(3, 5); + if (d_initialized) + { + 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 index 26dc25a43..204035f21 100644 --- a/src/algorithms/PVT/libs/pvt_kf.h +++ b/src/algorithms/PVT/libs/pvt_kf.h @@ -35,16 +35,17 @@ class Pvt_Kf public: Pvt_Kf() = default; virtual ~Pvt_Kf() = default; - void init_kf(const arma::vec& p, + void init_Kf(const arma::vec& p, const arma::vec& v, - double solver_interval_s, + double update_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); + bool is_initialized() const; void run_Kf(const arma::vec& p, const arma::vec& v); - void get_pvt(arma::vec& p, arma::vec& v); - bool initialized{false}; + void get_pv_Kf(arma::vec& p, arma::vec& v) const; + void reset_Kf(); private: // Kalman Filter class variables @@ -58,6 +59,7 @@ private: arma::vec d_x_old_old; arma::vec d_x_new_old; arma::vec d_x_new_new; + bool d_initialized{false}; }; diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index 7ebeb0324..c97a12d70 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -69,70 +69,6 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection) } -void Pvt_Solution::set_averaging_depth(int depth) -{ - d_averaging_depth = depth; -} - - -void Pvt_Solution::set_averaging_flag(bool flag) -{ - d_flag_averaging = flag; -} - - -void Pvt_Solution::perform_pos_averaging() -{ - // MOVING AVERAGE PVT - bool avg = d_flag_averaging; - if (avg == true) - { - if (d_hist_longitude_d.size() == static_cast(d_averaging_depth)) - { - // Pop oldest value - d_hist_longitude_d.pop_back(); - d_hist_latitude_d.pop_back(); - d_hist_height_m.pop_back(); - // Push new values - d_hist_longitude_d.push_front(d_longitude_d); - d_hist_latitude_d.push_front(d_latitude_d); - d_hist_height_m.push_front(d_height_m); - - d_avg_latitude_d = 0.0; - d_avg_longitude_d = 0.0; - d_avg_height_m = 0.0; - for (size_t i = 0; i < d_hist_longitude_d.size(); i++) - { - d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i); - d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); - d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); - } - d_avg_latitude_d = d_avg_latitude_d / static_cast(d_averaging_depth); - d_avg_longitude_d = d_avg_longitude_d / static_cast(d_averaging_depth); - d_avg_height_m = d_avg_height_m / static_cast(d_averaging_depth); - d_valid_position = true; - } - else - { - // int current_depth=d_hist_longitude_d.size(); - // Push new values - d_hist_longitude_d.push_front(d_longitude_d); - d_hist_latitude_d.push_front(d_latitude_d); - d_hist_height_m.push_front(d_height_m); - - d_avg_latitude_d = d_latitude_d; - d_avg_longitude_d = d_longitude_d; - d_avg_height_m = d_height_m; - d_valid_position = false; - } - } - else - { - d_valid_position = true; - } -} - - double Pvt_Solution::get_time_offset_s() const { return d_rx_dt_s; @@ -199,30 +135,6 @@ double Pvt_Solution::get_course_over_ground() const } -double Pvt_Solution::get_avg_latitude() const -{ - return d_avg_latitude_d; -} - - -double Pvt_Solution::get_avg_longitude() const -{ - return d_avg_longitude_d; -} - - -double Pvt_Solution::get_avg_height() const -{ - return d_avg_height_m; -} - - -bool Pvt_Solution::is_averaging() const -{ - return d_flag_averaging; -} - - bool Pvt_Solution::is_valid_position() const { return d_valid_position; diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index 3d9764537..bd00208bf 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -20,7 +20,6 @@ #include #include -#include /** \addtogroup PVT * \{ */ @@ -38,20 +37,10 @@ public: Pvt_Solution() = default; virtual ~Pvt_Solution() = default; - void set_rx_pos(const std::array &pos); //!< Set position: X, Y, Z in Cartesian ECEF coordinates [m] - void set_rx_vel(const std::array &vel); //!< Set velocity: East [m/s], North [m/s], Up [m/s] - void set_position_UTC_time(const boost::posix_time::ptime &pt); - void set_time_offset_s(double offset); //!< Set RX time offset [s] - void set_clock_drift_ppm(double clock_drift_ppm); //!< Set the Rx clock drift [ppm] - void set_speed_over_ground(double speed_m_s); //!< Set RX speed over ground [m/s] - void set_course_over_ground(double cog_deg); //!< Set RX course over ground [deg] - void set_valid_position(bool is_valid); - void set_num_valid_observations(int num); //!< Set the number of valid pseudorange observations (valid satellites) - void set_pre_2009_file(bool pre_2009_file); //!< Flag for the week rollover computation in post processing mode for signals older than 2009 - // averaging - void set_averaging_depth(int depth); //!< Set length of averaging window - void set_averaging_flag(bool flag); - void perform_pos_averaging(); + virtual double get_hdop() const = 0; + virtual double get_vdop() const = 0; + virtual double get_pdop() const = 0; + virtual double get_gdop() const = 0; std::array get_rx_pos() const; std::array get_rx_vel() const; @@ -63,18 +52,20 @@ public: double get_clock_drift_ppm() const; //!< Get the Rx clock drift [ppm] double get_speed_over_ground() const; //!< Get RX speed over ground [m/s] double get_course_over_ground() const; //!< Get RX course over ground [deg] - double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg] - double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg] - double get_avg_height() const; //!< Get RX position averaged height WGS84 [m] int get_num_valid_observations() const; //!< Get the number of valid pseudorange observations (valid satellites) bool is_pre_2009() const; bool is_valid_position() const; - bool is_averaging() const; - virtual double get_hdop() const = 0; - virtual double get_vdop() const = 0; - virtual double get_pdop() const = 0; - virtual double get_gdop() const = 0; + void set_rx_pos(const std::array &pos); //!< Set position: X, Y, Z in Cartesian ECEF coordinates [m] + void set_rx_vel(const std::array &vel); //!< Set velocity: East [m/s], North [m/s], Up [m/s] + void set_position_UTC_time(const boost::posix_time::ptime &pt); + void set_time_offset_s(double offset); //!< Set RX time offset [s] + void set_clock_drift_ppm(double clock_drift_ppm); //!< Set the Rx clock drift [ppm] + void set_speed_over_ground(double speed_m_s); //!< Set RX speed over ground [m/s] + void set_course_over_ground(double cog_deg); //!< Set RX course over ground [deg] + void set_valid_position(bool is_valid); + void set_num_valid_observations(int num); //!< Set the number of valid pseudorange observations (valid satellites) + void set_pre_2009_file(bool pre_2009_file); //!< Flag for the week rollover computation in post processing mode for signals older than 2009 private: /* @@ -98,10 +89,6 @@ private: std::array d_rx_vel{}; boost::posix_time::ptime d_position_UTC_time; - std::deque d_hist_latitude_d; - std::deque d_hist_longitude_d; - std::deque d_hist_height_m; - double d_latitude_d{0.0}; // RX position Latitude WGS84 [deg] double d_longitude_d{0.0}; // RX position Longitude WGS84 [deg] double d_height_m{0.0}; // RX position height WGS84 [m] @@ -110,16 +97,10 @@ private: double d_speed_over_ground_m_s{0.0}; // RX speed over ground [m/s] double d_course_over_ground_d{0.0}; // RX course over ground [deg] - double d_avg_latitude_d{0.0}; // Averaged latitude in degrees - double d_avg_longitude_d{0.0}; // Averaged longitude in degrees - double d_avg_height_m{0.0}; // Averaged height [m] - - int d_averaging_depth{0}; // Length of averaging window int d_valid_observations{0}; // Number of valid observations in this epoch bool d_pre_2009_file{false}; // Flag to correct week rollover in post processing mode for signals older than 2009 bool d_valid_position{false}; - bool d_flag_averaging{false}; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index e17edfa14..8b9d5b8ee 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -56,8 +56,6 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, d_flag_dump_enabled(flag_dump_to_file), d_flag_dump_mat_enabled(flag_dump_to_mat) { - this->set_averaging_flag(false); - // see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc // function: satwavelen d_rtklib_freq_index[0] = 0; @@ -900,7 +898,7 @@ void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, ui } -bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging) +bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, double kf_update_interval_s) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -911,8 +909,6 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ const Glonass_Gnav_Utc_Model &gnav_utc = this->glonass_gnav_utc_model; - this->set_averaging_flag(flag_averaging); - // ******************************************************************************** // ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************ // ******************************************************************************** @@ -1509,7 +1505,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ this->set_num_valid_observations(0); if (d_conf.enable_pvt_kf == true) { - d_pvt_kf.initialized = false; + d_pvt_kf.reset_Kf(); } } else @@ -1548,14 +1544,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ if (d_conf.enable_pvt_kf == true) { - if (d_pvt_kf.initialized == false) + if (d_pvt_kf.is_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, + d_pvt_kf.init_Kf(p, v, - d_conf.observable_interval_ms / 1000.0, + kf_update_interval_s, d_conf.measures_ecef_pos_sd_m, d_conf.measures_ecef_vel_sd_ms, d_conf.system_ecef_pos_sd_m, @@ -1566,7 +1562,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ 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); + d_pvt_kf.get_pv_Kf(p, v); pvt_sol.rr[0] = p[0]; // [m] pvt_sol.rr[1] = p[1]; // [m] pvt_sol.rr[2] = p[2]; // [m] diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 7bffa042d..f24a37b2a 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -89,7 +89,7 @@ public: Pvt_Conf conf); ~Rtklib_Solver(); - bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); + bool get_PVT(const std::map& gnss_observables_map, double kf_update_interval_s); double get_hdop() const override; double get_vdop() const override; 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 fcc0920be..9032f878a 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 @@ -15,8 +15,8 @@ */ -#include "gnss_sdr_filesystem.h" #include "nmea_printer.h" +#include "gnss_sdr_filesystem.h" #include "pvt_conf.h" #include "rtklib_rtkpos.h" #include "rtklib_solver.h" @@ -168,7 +168,7 @@ TEST_F(NmeaPrinterTest, PrintLine) bool flag_nmea_output_file = true; ASSERT_NO_THROW({ std::shared_ptr nmea_printer = std::make_shared(filename, flag_nmea_output_file, false, ""); - nmea_printer->Print_Nmea_Line(pvt_solution.get(), false); + nmea_printer->Print_Nmea_Line(pvt_solution.get()); }) << "Failure printing NMEA messages."; std::ifstream test_file(filename);