mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Fix PVT Kalman filter for any PVT.output_rate_ms 

Remove averaging in Pvt_Solution, since it was broken, misleading, not used, and added unnecessary complexity in the interfaces.
Uniformize names of public Pvt_Kf class memben functions
This commit is contained in:
Carles Fernandez 2023-07-08 19:18:08 +02:00
parent 795889e0c0
commit 2568b8be81
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
16 changed files with 98 additions and 226 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@ -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<Rtklib_Solver>(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<Rtklib_Solver>(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<Rtklib_Solver>(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)

21
src/algorithms/PVT/libs/geojson_printer.cc
View File

@ -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())
{

2
src/algorithms/PVT/libs/geojson_printer.h
View File

@ -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:

21
src/algorithms/PVT/libs/gpx_printer.cc
View File

@ -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())
{

2
src/algorithms/PVT/libs/gpx_printer.h
View File

@ -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:

21
src/algorithms/PVT/libs/kml_printer.cc
View File

@ -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())
{

2
src/algorithms/PVT/libs/kml_printer.h
View File

@ -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:

5
src/algorithms/PVT/libs/nmea_printer.cc
View File

@ -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

4
src/algorithms/PVT/libs/nmea_printer.h
View File

@ -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;
};

64
src/algorithms/PVT/libs/pvt_kf.cc
View File

@ -19,16 +19,16 @@
#include <glog/logging.h>
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);
}
}

10
src/algorithms/PVT/libs/pvt_kf.h
View File

@ -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};
};

88
src/algorithms/PVT/libs/pvt_solution.cc
View File

@ -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<unsigned int>(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<double>(d_averaging_depth);
d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
d_avg_height_m = d_avg_height_m / static_cast<double>(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;

47
src/algorithms/PVT/libs/pvt_solution.h
View File

@ -20,7 +20,6 @@
#include <boost/date_time/posix_time/posix_time.hpp>
#include <array>
#include <deque>
/** \addtogroup PVT
* \{ */
@ -38,20 +37,10 @@ public:
Pvt_Solution() = default;
virtual ~Pvt_Solution() = default;
void set_rx_pos(const std::array<double, 3> &pos); //!< Set position: X, Y, Z in Cartesian ECEF coordinates [m]
void set_rx_vel(const std::array<double, 3> &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<double, 3> get_rx_pos() const;
std::array<double, 3> 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<double, 3> &pos); //!< Set position: X, Y, Z in Cartesian ECEF coordinates [m]
void set_rx_vel(const std::array<double, 3> &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<double, 3> d_rx_vel{};
boost::posix_time::ptime d_position_UTC_time;
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> 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};
};

16
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -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<int, Gnss_Synchro> &gnss_observables_map, bool flag_averaging)
bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s)
{
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
@ -911,8 +909,6 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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]

2
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -89,7 +89,7 @@ public:
Pvt_Conf conf);
~Rtklib_Solver();
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging);
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double kf_update_interval_s);
double get_hdop() const override;
double get_vdop() const override;

4
src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc
View File

@ -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> nmea_printer = std::make_shared<Nmea_Printer>(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);