mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-06 04:47:59 +00:00
Code Issues Releases Wiki Activity

feat: use sensor measurements to estimate velocity in PVT KF solver

Browse Source
This commit is contained in:
Victor Castillo
2025-06-08 23:22:23 +02:00
committed by Carles Fernandez
parent 195886244a
commit 03aa5efa3d
6 changed files with 57 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@@ -86,6 +86,10 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
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);
// PVT Sensor data settings
SensorDataSourceConfiguration sensor_data_configuration{configuration};
pvt_output_parameters.kf_use_imu_vel = configuration->property(role + ".kf_use_imu_vel", false);
// 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);
@@ -628,7 +632,7 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
}
// make PVT object
pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk);
pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk, sensor_data_configuration);
DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";
if (out_streams_ > 0)
{

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

@@ -114,17 +114,20 @@ namespace wht = std;
rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels,
const Pvt_Conf& conf_,
const rtk_t& rtk)
const rtk_t& rtk,
const SensorDataSourceConfiguration& sensor_data_configuration)
{
return rtklib_pvt_gs_sptr(new rtklib_pvt_gs(nchannels,
conf_,
rtk));
rtk,
sensor_data_configuration));
}
rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
const Pvt_Conf& conf_,
const rtk_t& rtk)
const rtk_t& rtk,
const SensorDataSourceConfiguration& sensor_data_configuration)
: gr::sync_block("rtklib_pvt_gs",
gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
gr::io_signature::make(0, 0, 0)),
@@ -605,6 +608,16 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
}
}
// Sensor Data
std::vector<SensorIdentifier::value_type> required_sensors{};
if (conf_.kf_use_imu_vel)
{
required_sensors.emplace_back(SensorIdentifier::IMU_VEL_X);
required_sensors.emplace_back(SensorIdentifier::IMU_VEL_Y);
required_sensors.emplace_back(SensorIdentifier::IMU_VEL_Z);
}
d_sensor_data_aggregator = std::make_unique<SensorDataAggregator>(sensor_data_configuration, required_sensors);
d_start = std::chrono::system_clock::now();
}
@@ -1989,8 +2002,8 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
gr_vector_void_star& output_items __attribute__((unused)))
{
std::vector<gr::tag_t> sensor_tags;
this->get_tags_in_range(sensor_tags, 0, this->nitems_read(0), this->nitems_read(0) + noutput_items, pmt::mp("sensor_data"));
SensorDataAggregator sensor_data{sensor_tags};
this->get_tags_in_range(sensor_tags, 0, this->nitems_read(0), this->nitems_read(0) + noutput_items, d_sensor_data_aggregator->SENSOR_DATA_TAG);
d_sensor_data_aggregator->update(sensor_tags);
for (int32_t epoch = 0; epoch < noutput_items; epoch++)
{
@@ -2159,7 +2172,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, d_observable_interval_ms / 1000.0))
if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, *d_sensor_data_aggregator))
{
d_pvt_errors_counter = 0; // Reset consecutive PVT error counter
const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s();
@@ -2273,7 +2286,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, d_output_rate_ms / 1000.0);
flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, d_output_rate_ms / 1000.0, *d_sensor_data_aggregator);
}
if (flag_pvt_valid == true)

13
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h
View File

@@ -23,6 +23,8 @@
#include "gnss_time.h"
#include "osnma_data.h"
#include "rtklib.h"
#include "sensor_data/sensor_data_aggregator.h"
#include "sensor_data/sensor_data_source_configuration.h"
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/interprocess/ipc/message_queue.hpp>
@@ -74,7 +76,8 @@ using rtklib_pvt_gs_sptr = gnss_shared_ptr<rtklib_pvt_gs>;
rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels,
const Pvt_Conf& conf_,
const rtk_t& rtk);
const rtk_t& rtk,
const SensorDataSourceConfiguration& sensor_data_configuration);
/*!
* \brief This class implements a block that computes the PVT solution using the RTKLIB integrated library
@@ -135,11 +138,13 @@ public:
private:
friend rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels,
const Pvt_Conf& conf_,
const rtk_t& rtk);
const rtk_t& rtk,
const SensorDataSourceConfiguration& sensor_data_configuration);
rtklib_pvt_gs(uint32_t nchannels,
const Pvt_Conf& conf_,
const rtk_t& rtk);
const rtk_t& rtk,
const SensorDataSourceConfiguration& sensor_data_configuration);
void log_source_timetag_info(double RX_time_ns, double TAG_time_ns);
@@ -173,6 +178,8 @@ private:
std::fstream d_log_timetag_file;
std::unique_ptr<SensorDataAggregator> d_sensor_data_aggregator;
std::shared_ptr<Rtklib_Solver> d_internal_pvt_solver;
std::shared_ptr<Rtklib_Solver> d_user_pvt_solver;

3
src/algorithms/PVT/libs/pvt_conf.h
View File

@@ -103,6 +103,9 @@ public:
double measures_ecef_vel_sd_ms = 0.1;
double system_ecef_pos_sd_m = 0.01;
double system_ecef_vel_sd_ms = 0.001;
// Sensor Data parameters
bool kf_use_imu_vel = false;
};

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

@@ -906,7 +906,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, double kf_update_interval_s)
bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s, const SensorDataAggregator &sensor_data_aggregator)
{
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
@@ -1556,6 +1556,14 @@ 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]};
if (d_conf.kf_use_imu_vel)
{
v = {
sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_X).value,
sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_Y).value,
sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_Z).value};
}
d_pvt_kf.init_Kf(p,
v,
@@ -1569,6 +1577,14 @@ 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]};
if (d_conf.kf_use_imu_vel)
{
v = {
sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_X).value,
sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_Y).value,
sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_Z).value};
}
d_pvt_kf.run_Kf(p, v);
d_pvt_kf.get_pv_Kf(p, v);
pvt_sol.rr[0] = p[0]; // [m]

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

@@ -61,6 +61,7 @@
#include "pvt_solution.h"
#include "rtklib.h"
#include "rtklib_conversions.h"
#include "sensor_data/sensor_data_aggregator.h"
#include <array>
#include <cstdint>
#include <fstream>
@@ -90,7 +91,7 @@ public:
~Rtklib_Solver();
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double kf_update_interval_s);
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double kf_update_interval_s, const SensorDataAggregator& sensor_data_aggregator);
double get_hdop() const override;
double get_vdop() const override;