mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-08-01 08:20:34 +00:00
Code Issues Releases Wiki Activity

[ADD] estatic_measures_sd flag

Browse Source
This commit is contained in:
M.A. Gomez 2023-07-12 17:26:45 +02:00
parent 643bf5516a
commit dd7b1f9f6a
No known key found for this signature in database
GPG Key ID: 69D837A2B262D414
5 changed files with 3111 additions and 3091 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1975
src/algorithms/PVT/adapters/rtklib_pvt.cc Normal file → Executable file
View File

File diff suppressed because it is too large Load Diff

219
src/algorithms/PVT/libs/pvt_conf.h Normal file → Executable file
View File

@ -1,109 +1,110 @@
/*! /*!
* \file pvt_conf.h * \file pvt_conf.h
* \brief Class that contains all the configuration parameters for the PVT block * \brief Class that contains all the configuration parameters for the PVT block
* \author Carles Fernandez, 2018. cfernandez(at)cttc.es * \author Carles Fernandez, 2018. cfernandez(at)cttc.es
* *
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* *
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver. * GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR. * This file is part of GNSS-SDR.
* *
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later * SPDX-License-Identifier: GPL-3.0-or-later
* *
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
*/ */
#ifndef GNSS_SDR_PVT_CONF_H #ifndef GNSS_SDR_PVT_CONF_H
#define GNSS_SDR_PVT_CONF_H #define GNSS_SDR_PVT_CONF_H
#include <cstdint> #include <cstdint>
#include <map> #include <map>
#include <string> #include <string>
/** \addtogroup PVT /** \addtogroup PVT
* \{ */ * \{ */
/** \addtogroup PVT_libs /** \addtogroup PVT_libs
* \{ */ * \{ */
class Pvt_Conf class Pvt_Conf
{ {
public: public:
std::map<int, int> rtcm_msg_rate_ms; std::map<int, int> rtcm_msg_rate_ms;
std::string rinex_name = std::string("-"); std::string rinex_name = std::string("-");
std::string dump_filename; std::string dump_filename;
std::string nmea_dump_filename; std::string nmea_dump_filename;
std::string nmea_dump_devname; std::string nmea_dump_devname;
std::string rtcm_dump_devname; std::string rtcm_dump_devname;
std::string an_dump_devname; std::string an_dump_devname;
std::string output_path = std::string("."); std::string output_path = std::string(".");
std::string rinex_output_path = std::string("."); std::string rinex_output_path = std::string(".");
std::string gpx_output_path = std::string("."); std::string gpx_output_path = std::string(".");
std::string geojson_output_path = std::string("."); std::string geojson_output_path = std::string(".");
std::string nmea_output_file_path = std::string("."); std::string nmea_output_file_path = std::string(".");
std::string kml_output_path = std::string("."); std::string kml_output_path = std::string(".");
std::string xml_output_path = std::string("."); std::string xml_output_path = std::string(".");
std::string rtcm_output_file_path = std::string("."); std::string rtcm_output_file_path = std::string(".");
std::string udp_addresses; std::string udp_addresses;
std::string udp_eph_addresses; std::string udp_eph_addresses;
std::string log_source_timetag_file; std::string log_source_timetag_file;
uint32_t type_of_receiver = 0; uint32_t type_of_receiver = 0;
uint32_t observable_interval_ms = 20; uint32_t observable_interval_ms = 20;
int32_t output_rate_ms = 0; int32_t output_rate_ms = 0;
int32_t display_rate_ms = 0; int32_t display_rate_ms = 0;
int32_t kml_rate_ms = 1000; int32_t kml_rate_ms = 1000;
int32_t gpx_rate_ms = 1000; int32_t gpx_rate_ms = 1000;
int32_t geojson_rate_ms = 1000; int32_t geojson_rate_ms = 1000;
int32_t nmea_rate_ms = 1000; int32_t nmea_rate_ms = 1000;
int32_t rinex_version = 0; int32_t rinex_version = 0;
int32_t rinexobs_rate_ms = 0; int32_t rinexobs_rate_ms = 0;
int32_t an_rate_ms = 1000; int32_t an_rate_ms = 1000;
int32_t max_obs_block_rx_clock_offset_ms = 40; int32_t max_obs_block_rx_clock_offset_ms = 40;
int udp_port = 0; int udp_port = 0;
int udp_eph_port = 0; int udp_eph_port = 0;
int rtk_trace_level = 0; int rtk_trace_level = 0;
uint16_t rtcm_tcp_port = 0; uint16_t rtcm_tcp_port = 0;
uint16_t rtcm_station_id = 0; uint16_t rtcm_station_id = 0;
bool flag_nmea_tty_port = false; bool flag_nmea_tty_port = false;
bool flag_rtcm_server = false; bool flag_rtcm_server = false;
bool flag_rtcm_tty_port = false; bool flag_rtcm_tty_port = false;
bool output_enabled = true; bool output_enabled = true;
bool rinex_output_enabled = true; bool rinex_output_enabled = true;
bool gpx_output_enabled = true; bool gpx_output_enabled = true;
bool geojson_output_enabled = true; bool geojson_output_enabled = true;
bool nmea_output_file_enabled = true; bool nmea_output_file_enabled = true;
bool an_output_enabled = false; bool an_output_enabled = false;
bool kml_output_enabled = true; bool kml_output_enabled = true;
bool xml_output_enabled = true; bool xml_output_enabled = true;
bool rtcm_output_file_enabled = true; bool rtcm_output_file_enabled = true;
bool monitor_enabled = false; bool monitor_enabled = false;
bool monitor_ephemeris_enabled = false; bool monitor_ephemeris_enabled = false;
bool protobuf_enabled = true; bool protobuf_enabled = true;
bool enable_rx_clock_correction = true; bool enable_rx_clock_correction = true;
bool show_local_time_zone = false; bool show_local_time_zone = false;
bool pre_2009_file = false; bool pre_2009_file = false;
bool dump = false; bool dump = false;
bool dump_mat = true; bool dump_mat = true;
bool log_source_timetag; bool log_source_timetag;
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 // PVT KF parameters
bool enable_pvt_kf = false; bool enable_pvt_kf = false;
double measures_ecef_pos_sd_m = 1.0; bool estatic_measures_sd = false;
double measures_ecef_vel_sd_ms = 0.1; double measures_ecef_pos_sd_m = 1.0;
double system_ecef_pos_sd_m = 0.01; double measures_ecef_vel_sd_ms = 0.1;
double system_ecef_vel_sd_ms = 0.001; double system_ecef_pos_sd_m = 0.01;
}; double system_ecef_vel_sd_ms = 0.001;
};
/** \} */
/** \} */ /** \} */
#endif // GNSS_SDR_PVT_CONF_H /** \} */
#endif // GNSS_SDR_PVT_CONF_H

309
src/algorithms/PVT/libs/pvt_kf.cc Normal file → Executable file
View File

@ -1,147 +1,162 @@
/*! /*!
* \file pvt_kf.cc * \file pvt_kf.cc
* \brief Kalman Filter for Position and Velocity * \brief Kalman Filter for Position and Velocity
* \author Javier Arribas, 2023. jarribas(at)cttc.es * \author Javier Arribas, 2023. jarribas(at)cttc.es
* *
* *
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* *
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver. * GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR. * This file is part of GNSS-SDR.
* *
* Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later * SPDX-License-Identifier: GPL-3.0-or-later
* *
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
*/ */
#include "pvt_kf.h" #include "pvt_kf.h"
#include <glog/logging.h> #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, const arma::vec& v,
const arma::vec& res_p, const arma::vec& res_p,
double solver_interval_s, double solver_interval_s,
double measures_ecef_pos_sd_m, bool estatic_measures_sd,
double measures_ecef_vel_sd_ms, double measures_ecef_pos_sd_m,
double system_ecef_pos_sd_m, double measures_ecef_vel_sd_ms,
double system_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; // Kalman Filter class variables
const double Ti = solver_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}, d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0},
{0.0, 0.0, 1.0, 0.0, 0.0, Ti}, {0.0, 1.0, 0.0, 0.0, Ti, 0.0},
{0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, {0.0, 0.0, 1.0, 0.0, 0.0, Ti},
{0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, {0.0, 0.0, 0.0, 1.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 1.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);
d_H = arma::eye(6, 6);
// measurement matrix static covariances
d_R = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.0}, // measurement matrix static covariances
{res_p[3], res_p[1], res_p[4], 0.0, 0.0, 0.0}, if(estatic_measures_sd){
{res_p[4], res_p[5], res_p[2], 0.0, 0.0, 0.0}, d_R = {{pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.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, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.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, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.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, 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},
// system covariance matrix (static) {0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}};
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}, d_static = true;
{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}, }else{
{0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0}, d_R = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0)}}; {res_p[3], res_p[1], res_p[4], 0.0, 0.0, 0.0},
{res_p[4], res_p[5], res_p[2], 0.0, 0.0, 0.0},
// initial Kalman covariance matrix {0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0, 0.0},
d_P_old_old = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.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, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.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, 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}, d_static = false;
{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)}}; // system covariance matrix (static)
d_Q = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
// states: position ecef [m], velocity ecef [m/s] {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0},
d_x_old_old = arma::zeros(6); {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0},
d_x_old_old.subvec(0, 2) = p; {0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0, 0.0},
d_x_old_old.subvec(3, 5) = v; {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)}};
d_initialized = true;
// initial Kalman covariance matrix
DLOG(INFO) << "Ti: " << Ti; d_P_old_old = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
DLOG(INFO) << "F: " << d_F; {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0},
DLOG(INFO) << "H: " << d_H; {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0},
DLOG(INFO) << "R: " << d_R; {0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0, 0.0},
DLOG(INFO) << "Q: " << d_Q; {0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0},
DLOG(INFO) << "P: " << d_P_old_old; {0.0, 0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0)}};
DLOG(INFO) << "x: " << d_x_old_old;
} // states: position ecef [m], velocity ecef [m/s]
d_x_old_old = arma::zeros(6);
d_x_old_old.subvec(0, 2) = p;
bool Pvt_Kf::is_initialized() const d_x_old_old.subvec(3, 5) = v;
{
return d_initialized; d_initialized = true;
}
DLOG(INFO) << "Ti: " << Ti;
DLOG(INFO) << "F: " << d_F;
void Pvt_Kf::reset_Kf() DLOG(INFO) << "H: " << d_H;
{ DLOG(INFO) << "R: " << d_R;
d_initialized = false; DLOG(INFO) << "Q: " << d_Q;
} DLOG(INFO) << "P: " << d_P_old_old;
DLOG(INFO) << "x: " << d_x_old_old;
}
void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p)
{
if (d_initialized) bool Pvt_Kf::is_initialized() const
{ {
// Kalman loop return d_initialized;
// 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;
void Pvt_Kf::reset_Kf()
// Measurement update {
try d_initialized = false;
{ }
// Measurement residuals
d_R(0, 0) = res_p[0];
d_R(0, 1) = res_p[3]; void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p)
d_R(0, 2) = res_p[5]; {
d_R(1, 0) = res_p[3]; if (d_initialized)
d_R(1, 1) = res_p[1]; {
d_R(1, 2) = res_p[4]; // Kalman loop
d_R(2, 0) = res_p[5]; // Prediction
d_R(2, 1) = res_p[4]; d_x_new_old = d_F * d_x_old_old;
d_R(2, 2) = res_p[2]; d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;
// Measurement update // Measurement update
arma::vec z = arma::join_cols(p, v); try
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 {
if(!d_static){
d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old); // Measurement residuals update
arma::mat A = (arma::eye(6, 6) - K * d_H); d_R(0, 0) = res_p[0];
d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t(); d_R(0, 1) = res_p[3];
d_R(0, 2) = res_p[5];
// prepare data for next KF epoch d_R(1, 0) = res_p[3];
d_x_old_old = d_x_new_new; d_R(1, 1) = res_p[1];
d_P_old_old = d_P_new_new; d_R(1, 2) = res_p[4];
} d_R(2, 0) = res_p[5];
catch (...) d_R(2, 1) = res_p[4];
{ d_R(2, 2) = res_p[2];
d_x_new_new = d_x_new_old; }
this->reset_Kf(); // 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);
arma::mat A = (arma::eye(6, 6) - K * d_H);
void Pvt_Kf::get_pv_Kf(arma::vec& p, arma::vec& v) const d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t();
{
if (d_initialized) // prepare data for next KF epoch
{ d_x_old_old = d_x_new_new;
p = d_x_new_new.subvec(0, 2); d_P_old_old = d_P_new_new;
v = d_x_new_new.subvec(3, 5); }
} catch (...)
} {
d_x_new_new = d_x_new_old;
this->reset_Kf();
}
}
}
void Pvt_Kf::get_pv_Kf(arma::vec& p, arma::vec& v) const
{
if (d_initialized)
{
p = d_x_new_new.subvec(0, 2);
v = d_x_new_new.subvec(3, 5);
}
}

140
src/algorithms/PVT/libs/pvt_kf.h Normal file → Executable file
View File

@ -1,69 +1,71 @@
/*! /*!
* \file pvt_kf.h * \file pvt_kf.h
* \brief Kalman Filter for Position and Velocity * \brief Kalman Filter for Position and Velocity
* \author Javier Arribas, 2023. jarribas(at)cttc.es * \author Javier Arribas, 2023. jarribas(at)cttc.es
* *
* *
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* *
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver. * GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR. * This file is part of GNSS-SDR.
* *
* Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later * SPDX-License-Identifier: GPL-3.0-or-later
* *
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
*/ */
#ifndef GNSS_SDR_PVT_KF_H #ifndef GNSS_SDR_PVT_KF_H
#define GNSS_SDR_PVT_KF_H #define GNSS_SDR_PVT_KF_H
#include <armadillo> #include <armadillo>
/** \addtogroup PVT /** \addtogroup PVT
* \{ */ * \{ */
/** \addtogroup PVT_libs /** \addtogroup PVT_libs
* \{ */ * \{ */
/*! /*!
* \brief Kalman Filter for Position and Velocity * \brief Kalman Filter for Position and Velocity
* *
*/ */
class Pvt_Kf class Pvt_Kf
{ {
public: public:
Pvt_Kf() = default; Pvt_Kf() = default;
virtual ~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, const arma::vec& v,
const arma::vec& res_p, const arma::vec& res_p,
double solver_interval_s, double solver_interval_s,
double measures_ecef_pos_sd_m, bool estatic_measures_sd,
double measures_ecef_vel_sd_ms, double measures_ecef_pos_sd_m,
double system_ecef_pos_sd_m, double measures_ecef_vel_sd_ms,
double system_ecef_vel_sd_ms); double system_ecef_pos_sd_m,
bool is_initialized() const; double system_ecef_vel_sd_ms);
void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p); bool is_initialized() const;
void get_pv_Kf(arma::vec& p, arma::vec& v) const; void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p);
void reset_Kf(); void get_pv_Kf(arma::vec& p, arma::vec& v) const;
void reset_Kf();
private:
// Kalman Filter class variables private:
arma::mat d_F; // Kalman Filter class variables
arma::mat d_H; arma::mat d_F;
arma::mat d_R; arma::mat d_H;
arma::mat d_Q; arma::mat d_R;
arma::mat d_P_old_old; arma::mat d_Q;
arma::mat d_P_new_old; arma::mat d_P_old_old;
arma::mat d_P_new_new; arma::mat d_P_new_old;
arma::vec d_x_old_old; arma::mat d_P_new_new;
arma::vec d_x_new_old; arma::vec d_x_old_old;
arma::vec d_x_new_new; arma::vec d_x_new_old;
bool d_initialized{false}; arma::vec d_x_new_new;
}; bool d_initialized{false};
bool d_static{false};
};
/** \} */
/** \} */
#endif // GNSS_SDR_Pvt_Kf_H /** \} */
/** \} */
#endif // GNSS_SDR_Pvt_Kf_H

3559
src/algorithms/PVT/libs/rtklib_solver.cc Normal file → Executable file
View File

File diff suppressed because it is too large Load Diff