mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-10-31 15:16:22 +00:00
Adding a simple PVT Holonomic Kalman filter for position and velocity
This commit is contained in:
parent
d30b87c2c1
commit
36e709dda6
@ -74,6 +74,13 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
|
|||||||
// display rate
|
// display rate
|
||||||
pvt_output_parameters.display_rate_ms = bc::lcm(pvt_output_parameters.output_rate_ms, configuration->property(role + ".display_rate_ms", 500));
|
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
|
// NMEA Printer settings
|
||||||
pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false);
|
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);
|
pvt_output_parameters.nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename);
|
||||||
|
@ -178,7 +178,6 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
|
|||||||
d_enable_rx_clock_correction(conf_.enable_rx_clock_correction),
|
d_enable_rx_clock_correction(conf_.enable_rx_clock_correction),
|
||||||
d_an_printer_enabled(conf_.an_output_enabled),
|
d_an_printer_enabled(conf_.an_output_enabled),
|
||||||
d_log_timetag(conf_.log_source_timetag),
|
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_has_corrections(conf_.use_has_corrections),
|
||||||
d_use_unhealthy_sats(conf_.use_unhealthy_sats)
|
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
|
// setup two PVT solvers: internal solver for rx clock and user solver
|
||||||
// user PVT 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, d_use_e6_for_pvt);
|
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_averaging_depth(1);
|
||||||
d_user_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
|
d_user_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
|
||||||
|
|
||||||
// internal PVT solver, mainly used to estimate the receiver clock
|
// internal PVT solver, mainly used to estimate the receiver clock
|
||||||
rtk_t internal_rtk = rtk;
|
rtk_t internal_rtk = rtk;
|
||||||
internal_rtk.opt.mode = PMODE_SINGLE; // use single positioning mode in internal PVT solver
|
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, d_use_e6_for_pvt);
|
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_averaging_depth(1);
|
||||||
d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
|
d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
// only one solver, customized by the user options
|
// 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, d_use_e6_for_pvt);
|
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_averaging_depth(1);
|
||||||
d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
|
d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
|
||||||
d_user_pvt_solver = d_internal_pvt_solver;
|
d_user_pvt_solver = d_internal_pvt_solver;
|
||||||
|
@ -276,7 +276,6 @@ private:
|
|||||||
bool d_enable_has_messages;
|
bool d_enable_has_messages;
|
||||||
bool d_an_printer_enabled;
|
bool d_an_printer_enabled;
|
||||||
bool d_log_timetag;
|
bool d_log_timetag;
|
||||||
bool d_use_e6_for_pvt;
|
|
||||||
bool d_use_has_corrections;
|
bool d_use_has_corrections;
|
||||||
bool d_use_unhealthy_sats;
|
bool d_use_unhealthy_sats;
|
||||||
};
|
};
|
||||||
|
@ -23,6 +23,7 @@ set(PVT_LIB_SOURCES
|
|||||||
monitor_ephemeris_udp_sink.cc
|
monitor_ephemeris_udp_sink.cc
|
||||||
has_simple_printer.cc
|
has_simple_printer.cc
|
||||||
geohash.cc
|
geohash.cc
|
||||||
|
pvt_kf.cc
|
||||||
)
|
)
|
||||||
|
|
||||||
set(PVT_LIB_HEADERS
|
set(PVT_LIB_HEADERS
|
||||||
@ -45,6 +46,7 @@ set(PVT_LIB_HEADERS
|
|||||||
monitor_ephemeris_udp_sink.h
|
monitor_ephemeris_udp_sink.h
|
||||||
has_simple_printer.h
|
has_simple_printer.h
|
||||||
geohash.h
|
geohash.h
|
||||||
|
pvt_kf.h
|
||||||
)
|
)
|
||||||
|
|
||||||
list(SORT PVT_LIB_HEADERS)
|
list(SORT PVT_LIB_HEADERS)
|
||||||
|
@ -94,6 +94,13 @@ public:
|
|||||||
bool use_e6_for_pvt = true;
|
bool use_e6_for_pvt = true;
|
||||||
bool use_has_corrections = true;
|
bool use_has_corrections = true;
|
||||||
bool use_unhealthy_sats = false;
|
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;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
132
src/algorithms/PVT/libs/pvt_kf.cc
Normal file
132
src/algorithms/PVT/libs/pvt_kf.cc
Normal file
@ -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 <glog/logging.h>
|
||||||
|
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
66
src/algorithms/PVT/libs/pvt_kf.h
Normal file
66
src/algorithms/PVT/libs/pvt_kf.h
Normal file
@ -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 <armadillo>
|
||||||
|
|
||||||
|
/** \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
|
@ -49,12 +49,13 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
|
|||||||
uint32_t type_of_rx,
|
uint32_t type_of_rx,
|
||||||
bool flag_dump_to_file,
|
bool flag_dump_to_file,
|
||||||
bool flag_dump_to_mat,
|
bool flag_dump_to_mat,
|
||||||
bool use_e6_for_pvt) : d_dump_filename(dump_filename),
|
Pvt_Conf conf) : d_dump_filename(dump_filename),
|
||||||
d_rtk(rtk),
|
d_rtk(rtk),
|
||||||
d_type_of_rx(type_of_rx),
|
d_type_of_rx(type_of_rx),
|
||||||
d_flag_dump_enabled(flag_dump_to_file),
|
d_flag_dump_enabled(flag_dump_to_file),
|
||||||
d_flag_dump_mat_enabled(flag_dump_to_mat),
|
d_flag_dump_mat_enabled(flag_dump_to_mat),
|
||||||
d_use_e6_for_pvt(use_e6_for_pvt)
|
d_conf(conf)
|
||||||
|
|
||||||
{
|
{
|
||||||
this->set_averaging_flag(false);
|
this->set_averaging_flag(false);
|
||||||
|
|
||||||
@ -1039,7 +1040,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
|
|||||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
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);
|
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
|
if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
|
||||||
@ -1466,6 +1467,10 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
|
|||||||
d_rtk.neb = 0; // clear error buffer to avoid repeating the error message
|
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_time_offset_s(0.0); // reset rx time estimation
|
||||||
this->set_num_valid_observations(0);
|
this->set_num_valid_observations(0);
|
||||||
|
if (d_conf.enable_pvt_kf == true)
|
||||||
|
{
|
||||||
|
d_pvt_kf.initialized = false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
@ -1500,9 +1505,41 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
|
|||||||
}
|
}
|
||||||
this->set_valid_position(true);
|
this->set_valid_position(true);
|
||||||
std::array<double, 4> rx_position_and_time{};
|
std::array<double, 4> 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[0] = pvt_sol.rr[0]; // [m]
|
||||||
rx_position_and_time[1] = pvt_sol.rr[1]; // [m]
|
rx_position_and_time[1] = pvt_sol.rr[1]; // [m]
|
||||||
rx_position_and_time[2] = pvt_sol.rr[2]; // [m]
|
rx_position_and_time[2] = pvt_sol.rr[2]; // [m]
|
||||||
|
|
||||||
// todo: fix this ambiguity in the RTKLIB units in receiver clock offset!
|
// todo: fix this ambiguity in the RTKLIB units in receiver clock offset!
|
||||||
if (d_rtk.opt.mode == PMODE_SINGLE)
|
if (d_rtk.opt.mode == PMODE_SINGLE)
|
||||||
{
|
{
|
||||||
|
@ -56,6 +56,8 @@
|
|||||||
#include "gps_iono.h"
|
#include "gps_iono.h"
|
||||||
#include "gps_utc_model.h"
|
#include "gps_utc_model.h"
|
||||||
#include "monitor_pvt.h"
|
#include "monitor_pvt.h"
|
||||||
|
#include "pvt_conf.h"
|
||||||
|
#include "pvt_kf.h"
|
||||||
#include "pvt_solution.h"
|
#include "pvt_solution.h"
|
||||||
#include "rtklib.h"
|
#include "rtklib.h"
|
||||||
#include "rtklib_conversions.h"
|
#include "rtklib_conversions.h"
|
||||||
@ -84,7 +86,7 @@ public:
|
|||||||
uint32_t type_of_rx,
|
uint32_t type_of_rx,
|
||||||
bool flag_dump_to_file,
|
bool flag_dump_to_file,
|
||||||
bool flag_dump_to_mat,
|
bool flag_dump_to_mat,
|
||||||
bool use_e6_for_pvt = true);
|
Pvt_Conf conf);
|
||||||
~Rtklib_Solver();
|
~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, bool flag_averaging);
|
||||||
@ -152,7 +154,8 @@ private:
|
|||||||
uint32_t d_type_of_rx;
|
uint32_t d_type_of_rx;
|
||||||
bool d_flag_dump_enabled;
|
bool d_flag_dump_enabled;
|
||||||
bool d_flag_dump_mat_enabled;
|
bool d_flag_dump_mat_enabled;
|
||||||
bool d_use_e6_for_pvt;
|
Pvt_Conf d_conf;
|
||||||
|
Pvt_Kf d_pvt_kf;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
@ -17,6 +17,7 @@
|
|||||||
|
|
||||||
#include "gnss_sdr_filesystem.h"
|
#include "gnss_sdr_filesystem.h"
|
||||||
#include "nmea_printer.h"
|
#include "nmea_printer.h"
|
||||||
|
#include "pvt_conf.h"
|
||||||
#include "rtklib_rtkpos.h"
|
#include "rtklib_rtkpos.h"
|
||||||
#include "rtklib_solver.h"
|
#include "rtklib_solver.h"
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
@ -144,7 +145,9 @@ void NmeaPrinterTest::conf()
|
|||||||
TEST_F(NmeaPrinterTest, PrintLine)
|
TEST_F(NmeaPrinterTest, PrintLine)
|
||||||
{
|
{
|
||||||
std::string filename("nmea_test.nmea");
|
std::string filename("nmea_test.nmea");
|
||||||
std::shared_ptr<Rtklib_Solver> pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 1, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
std::shared_ptr<Rtklib_Solver> pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 1, false, false, conf);
|
||||||
|
|
||||||
boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
|
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));
|
boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46));
|
||||||
|
@ -15,6 +15,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include "gnss_sdr_filesystem.h"
|
#include "gnss_sdr_filesystem.h"
|
||||||
|
#include "pvt_conf.h"
|
||||||
#include "rinex_printer.h"
|
#include "rinex_printer.h"
|
||||||
#include "rtklib_rtkpos.h"
|
#include "rtklib_rtkpos.h"
|
||||||
#include "rtklib_solver.h"
|
#include "rtklib_solver.h"
|
||||||
@ -142,7 +143,9 @@ void RinexPrinterTest::conf()
|
|||||||
|
|
||||||
TEST_F(RinexPrinterTest, GalileoObsHeader)
|
TEST_F(RinexPrinterTest, GalileoObsHeader)
|
||||||
{
|
{
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 4, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 4, false, false, conf);
|
||||||
auto eph = Galileo_Ephemeris();
|
auto eph = Galileo_Ephemeris();
|
||||||
eph.PRN = 1;
|
eph.PRN = 1;
|
||||||
pvt_solution->galileo_ephemeris_map[1] = eph;
|
pvt_solution->galileo_ephemeris_map[1] = eph;
|
||||||
@ -228,7 +231,9 @@ TEST_F(RinexPrinterTest, GalileoObsHeader)
|
|||||||
|
|
||||||
TEST_F(RinexPrinterTest, GlonassObsHeader)
|
TEST_F(RinexPrinterTest, GlonassObsHeader)
|
||||||
{
|
{
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 28, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 28, false, false, conf);
|
||||||
auto eph = Glonass_Gnav_Ephemeris();
|
auto eph = Glonass_Gnav_Ephemeris();
|
||||||
eph.PRN = 1;
|
eph.PRN = 1;
|
||||||
pvt_solution->glonass_gnav_ephemeris_map[1] = eph;
|
pvt_solution->glonass_gnav_ephemeris_map[1] = eph;
|
||||||
@ -288,7 +293,9 @@ TEST_F(RinexPrinterTest, MixedObsHeader)
|
|||||||
auto eph_gps = Gps_Ephemeris();
|
auto eph_gps = Gps_Ephemeris();
|
||||||
eph_gal.PRN = 1;
|
eph_gal.PRN = 1;
|
||||||
eph_gps.PRN = 1;
|
eph_gps.PRN = 1;
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 106, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 106, false, false, conf);
|
||||||
pvt_solution->galileo_ephemeris_map[1] = eph_gal;
|
pvt_solution->galileo_ephemeris_map[1] = eph_gal;
|
||||||
|
|
||||||
pvt_solution->gps_ephemeris_map[1] = eph_gps;
|
pvt_solution->gps_ephemeris_map[1] = eph_gps;
|
||||||
@ -358,7 +365,9 @@ TEST_F(RinexPrinterTest, MixedObsHeaderGpsGlo)
|
|||||||
auto eph_gps = Gps_Ephemeris();
|
auto eph_gps = Gps_Ephemeris();
|
||||||
eph_glo.PRN = 1;
|
eph_glo.PRN = 1;
|
||||||
eph_gps.PRN = 1;
|
eph_gps.PRN = 1;
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 26, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 26, false, false, conf);
|
||||||
pvt_solution->glonass_gnav_ephemeris_map[1] = eph_glo;
|
pvt_solution->glonass_gnav_ephemeris_map[1] = eph_glo;
|
||||||
|
|
||||||
pvt_solution->gps_ephemeris_map[1] = eph_gps;
|
pvt_solution->gps_ephemeris_map[1] = eph_gps;
|
||||||
@ -425,7 +434,9 @@ TEST_F(RinexPrinterTest, GalileoObsLog)
|
|||||||
bool no_more_finds = false;
|
bool no_more_finds = false;
|
||||||
auto eph = Galileo_Ephemeris();
|
auto eph = Galileo_Ephemeris();
|
||||||
eph.PRN = 1;
|
eph.PRN = 1;
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 4, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 4, false, false, conf);
|
||||||
pvt_solution->galileo_ephemeris_map[1] = eph;
|
pvt_solution->galileo_ephemeris_map[1] = eph;
|
||||||
std::map<int, Gnss_Synchro> gnss_observables_map;
|
std::map<int, Gnss_Synchro> gnss_observables_map;
|
||||||
|
|
||||||
@ -505,7 +516,9 @@ TEST_F(RinexPrinterTest, GlonassObsLog)
|
|||||||
bool no_more_finds = false;
|
bool no_more_finds = false;
|
||||||
auto eph = Glonass_Gnav_Ephemeris();
|
auto eph = Glonass_Gnav_Ephemeris();
|
||||||
eph.PRN = 22;
|
eph.PRN = 22;
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 23, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 23, false, false, conf);
|
||||||
pvt_solution->glonass_gnav_ephemeris_map[1] = eph;
|
pvt_solution->glonass_gnav_ephemeris_map[1] = eph;
|
||||||
std::map<int, Gnss_Synchro> gnss_observables_map;
|
std::map<int, Gnss_Synchro> gnss_observables_map;
|
||||||
|
|
||||||
@ -587,7 +600,9 @@ TEST_F(RinexPrinterTest, GpsObsLogDualBand)
|
|||||||
auto eph_cnav = Gps_CNAV_Ephemeris();
|
auto eph_cnav = Gps_CNAV_Ephemeris();
|
||||||
eph.PRN = 1;
|
eph.PRN = 1;
|
||||||
eph_cnav.PRN = 1;
|
eph_cnav.PRN = 1;
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 7, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 7, false, false, conf);
|
||||||
pvt_solution->gps_ephemeris_map[1] = eph;
|
pvt_solution->gps_ephemeris_map[1] = eph;
|
||||||
pvt_solution->gps_cnav_ephemeris_map[1] = eph_cnav;
|
pvt_solution->gps_cnav_ephemeris_map[1] = eph_cnav;
|
||||||
std::map<int, Gnss_Synchro> gnss_observables_map;
|
std::map<int, Gnss_Synchro> gnss_observables_map;
|
||||||
@ -675,7 +690,9 @@ TEST_F(RinexPrinterTest, GpsObsLogDualBand)
|
|||||||
|
|
||||||
TEST_F(RinexPrinterTest, GalileoObsLogDualBand)
|
TEST_F(RinexPrinterTest, GalileoObsLogDualBand)
|
||||||
{
|
{
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 14, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 14, false, false, conf);
|
||||||
auto eph = Galileo_Ephemeris();
|
auto eph = Galileo_Ephemeris();
|
||||||
eph.PRN = 1;
|
eph.PRN = 1;
|
||||||
pvt_solution->galileo_ephemeris_map[1] = eph;
|
pvt_solution->galileo_ephemeris_map[1] = eph;
|
||||||
@ -775,7 +792,9 @@ TEST_F(RinexPrinterTest, MixedObsLog)
|
|||||||
auto eph_gal = Galileo_Ephemeris();
|
auto eph_gal = Galileo_Ephemeris();
|
||||||
eph_gps.PRN = 1;
|
eph_gps.PRN = 1;
|
||||||
eph_gal.PRN = 1;
|
eph_gal.PRN = 1;
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 9, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 9, false, false, conf);
|
||||||
pvt_solution->gps_ephemeris_map[1] = eph_gps;
|
pvt_solution->gps_ephemeris_map[1] = eph_gps;
|
||||||
pvt_solution->galileo_ephemeris_map[1] = eph_gal;
|
pvt_solution->galileo_ephemeris_map[1] = eph_gal;
|
||||||
std::map<int, Gnss_Synchro> gnss_observables_map;
|
std::map<int, Gnss_Synchro> gnss_observables_map;
|
||||||
@ -899,7 +918,9 @@ TEST_F(RinexPrinterTest, MixedObsLogGpsGlo)
|
|||||||
auto eph_glo = Glonass_Gnav_Ephemeris();
|
auto eph_glo = Glonass_Gnav_Ephemeris();
|
||||||
eph_gps.PRN = 1;
|
eph_gps.PRN = 1;
|
||||||
eph_glo.PRN = 1;
|
eph_glo.PRN = 1;
|
||||||
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 26, false, false);
|
Pvt_Conf conf;
|
||||||
|
conf.use_e6_for_pvt = false;
|
||||||
|
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, "filename", 26, false, false, conf);
|
||||||
pvt_solution->gps_ephemeris_map[1] = eph_gps;
|
pvt_solution->gps_ephemeris_map[1] = eph_gps;
|
||||||
pvt_solution->glonass_gnav_ephemeris_map[1] = eph_glo;
|
pvt_solution->glonass_gnav_ephemeris_map[1] = eph_glo;
|
||||||
std::map<int, Gnss_Synchro> gnss_observables_map;
|
std::map<int, Gnss_Synchro> gnss_observables_map;
|
||||||
|
@ -18,6 +18,7 @@
|
|||||||
#include "gnss_sdr_make_unique.h"
|
#include "gnss_sdr_make_unique.h"
|
||||||
#include "gnss_sdr_supl_client.h"
|
#include "gnss_sdr_supl_client.h"
|
||||||
#include "in_memory_configuration.h"
|
#include "in_memory_configuration.h"
|
||||||
|
#include "pvt_conf.h"
|
||||||
#include "rtklib_solver.h"
|
#include "rtklib_solver.h"
|
||||||
#include <armadillo>
|
#include <armadillo>
|
||||||
#include <boost/archive/xml_iarchive.hpp>
|
#include <boost/archive/xml_iarchive.hpp>
|
||||||
@ -28,7 +29,6 @@
|
|||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
|
|
||||||
rtk_t configure_rtklib_options()
|
rtk_t configure_rtklib_options()
|
||||||
{
|
{
|
||||||
std::shared_ptr<InMemoryConfiguration> configuration;
|
std::shared_ptr<InMemoryConfiguration> configuration;
|
||||||
@ -384,7 +384,9 @@ TEST(RTKLibSolverTest, test1)
|
|||||||
bool save_to_mat = false;
|
bool save_to_mat = false;
|
||||||
rtk_t rtk = configure_rtklib_options();
|
rtk_t rtk = configure_rtklib_options();
|
||||||
|
|
||||||
auto d_ls_pvt = std::make_unique<Rtklib_Solver>(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<Rtklib_Solver>(rtk, nchannels, dump_filename, 1, flag_dump_to_file, save_to_mat, conf);
|
||||||
d_ls_pvt->set_averaging_depth(1);
|
d_ls_pvt->set_averaging_depth(1);
|
||||||
|
|
||||||
// load ephemeris
|
// load ephemeris
|
||||||
|
Loading…
Reference in New Issue
Block a user