mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-07-11 14:14:26 +00:00
[ADD] estatic_measures_sd flag
This commit is contained in:
M.A. Gomez
parent
643bf5516a
commit
dd7b1f9f6a
1975
src/algorithms/PVT/adapters/rtklib_pvt.cc
Normal file → Executable file
1975
src/algorithms/PVT/adapters/rtklib_pvt.cc
Normal file → Executable file
File diff suppressed because it is too large
Load Diff
219
src/algorithms/PVT/libs/pvt_conf.h
Normal file → Executable file
219
src/algorithms/PVT/libs/pvt_conf.h
Normal file → Executable file
|
|
@ -1,109 +1,110 @@
|
|||
/*!
|
||||
* \file pvt_conf.h
|
||||
* \brief Class that contains all the configuration parameters for the PVT block
|
||||
* \author Carles Fernandez, 2018. cfernandez(at)cttc.es
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_PVT_CONF_H
|
||||
#define GNSS_SDR_PVT_CONF_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
/** \addtogroup PVT
|
||||
* \{ */
|
||||
/** \addtogroup PVT_libs
|
||||
* \{ */
|
||||
|
||||
|
||||
class Pvt_Conf
|
||||
{
|
||||
public:
|
||||
std::map<int, int> rtcm_msg_rate_ms;
|
||||
|
||||
std::string rinex_name = std::string("-");
|
||||
std::string dump_filename;
|
||||
std::string nmea_dump_filename;
|
||||
std::string nmea_dump_devname;
|
||||
std::string rtcm_dump_devname;
|
||||
std::string an_dump_devname;
|
||||
std::string output_path = std::string(".");
|
||||
std::string rinex_output_path = std::string(".");
|
||||
std::string gpx_output_path = std::string(".");
|
||||
std::string geojson_output_path = std::string(".");
|
||||
std::string nmea_output_file_path = std::string(".");
|
||||
std::string kml_output_path = std::string(".");
|
||||
std::string xml_output_path = std::string(".");
|
||||
std::string rtcm_output_file_path = std::string(".");
|
||||
std::string udp_addresses;
|
||||
std::string udp_eph_addresses;
|
||||
std::string log_source_timetag_file;
|
||||
|
||||
uint32_t type_of_receiver = 0;
|
||||
uint32_t observable_interval_ms = 20;
|
||||
|
||||
int32_t output_rate_ms = 0;
|
||||
int32_t display_rate_ms = 0;
|
||||
int32_t kml_rate_ms = 1000;
|
||||
int32_t gpx_rate_ms = 1000;
|
||||
int32_t geojson_rate_ms = 1000;
|
||||
int32_t nmea_rate_ms = 1000;
|
||||
int32_t rinex_version = 0;
|
||||
int32_t rinexobs_rate_ms = 0;
|
||||
int32_t an_rate_ms = 1000;
|
||||
int32_t max_obs_block_rx_clock_offset_ms = 40;
|
||||
int udp_port = 0;
|
||||
int udp_eph_port = 0;
|
||||
int rtk_trace_level = 0;
|
||||
|
||||
uint16_t rtcm_tcp_port = 0;
|
||||
uint16_t rtcm_station_id = 0;
|
||||
|
||||
bool flag_nmea_tty_port = false;
|
||||
bool flag_rtcm_server = false;
|
||||
bool flag_rtcm_tty_port = false;
|
||||
bool output_enabled = true;
|
||||
bool rinex_output_enabled = true;
|
||||
bool gpx_output_enabled = true;
|
||||
bool geojson_output_enabled = true;
|
||||
bool nmea_output_file_enabled = true;
|
||||
bool an_output_enabled = false;
|
||||
bool kml_output_enabled = true;
|
||||
bool xml_output_enabled = true;
|
||||
bool rtcm_output_file_enabled = true;
|
||||
bool monitor_enabled = false;
|
||||
bool monitor_ephemeris_enabled = false;
|
||||
bool protobuf_enabled = true;
|
||||
bool enable_rx_clock_correction = true;
|
||||
bool show_local_time_zone = false;
|
||||
bool pre_2009_file = false;
|
||||
bool dump = false;
|
||||
bool dump_mat = true;
|
||||
bool log_source_timetag;
|
||||
bool use_e6_for_pvt = true;
|
||||
bool use_has_corrections = true;
|
||||
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;
|
||||
};
|
||||
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_PVT_CONF_H
|
||||
/*!
|
||||
* \file pvt_conf.h
|
||||
* \brief Class that contains all the configuration parameters for the PVT block
|
||||
* \author Carles Fernandez, 2018. cfernandez(at)cttc.es
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_PVT_CONF_H
|
||||
#define GNSS_SDR_PVT_CONF_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
/** \addtogroup PVT
|
||||
* \{ */
|
||||
/** \addtogroup PVT_libs
|
||||
* \{ */
|
||||
|
||||
|
||||
class Pvt_Conf
|
||||
{
|
||||
public:
|
||||
std::map<int, int> rtcm_msg_rate_ms;
|
||||
|
||||
std::string rinex_name = std::string("-");
|
||||
std::string dump_filename;
|
||||
std::string nmea_dump_filename;
|
||||
std::string nmea_dump_devname;
|
||||
std::string rtcm_dump_devname;
|
||||
std::string an_dump_devname;
|
||||
std::string output_path = std::string(".");
|
||||
std::string rinex_output_path = std::string(".");
|
||||
std::string gpx_output_path = std::string(".");
|
||||
std::string geojson_output_path = std::string(".");
|
||||
std::string nmea_output_file_path = std::string(".");
|
||||
std::string kml_output_path = std::string(".");
|
||||
std::string xml_output_path = std::string(".");
|
||||
std::string rtcm_output_file_path = std::string(".");
|
||||
std::string udp_addresses;
|
||||
std::string udp_eph_addresses;
|
||||
std::string log_source_timetag_file;
|
||||
|
||||
uint32_t type_of_receiver = 0;
|
||||
uint32_t observable_interval_ms = 20;
|
||||
|
||||
int32_t output_rate_ms = 0;
|
||||
int32_t display_rate_ms = 0;
|
||||
int32_t kml_rate_ms = 1000;
|
||||
int32_t gpx_rate_ms = 1000;
|
||||
int32_t geojson_rate_ms = 1000;
|
||||
int32_t nmea_rate_ms = 1000;
|
||||
int32_t rinex_version = 0;
|
||||
int32_t rinexobs_rate_ms = 0;
|
||||
int32_t an_rate_ms = 1000;
|
||||
int32_t max_obs_block_rx_clock_offset_ms = 40;
|
||||
int udp_port = 0;
|
||||
int udp_eph_port = 0;
|
||||
int rtk_trace_level = 0;
|
||||
|
||||
uint16_t rtcm_tcp_port = 0;
|
||||
uint16_t rtcm_station_id = 0;
|
||||
|
||||
bool flag_nmea_tty_port = false;
|
||||
bool flag_rtcm_server = false;
|
||||
bool flag_rtcm_tty_port = false;
|
||||
bool output_enabled = true;
|
||||
bool rinex_output_enabled = true;
|
||||
bool gpx_output_enabled = true;
|
||||
bool geojson_output_enabled = true;
|
||||
bool nmea_output_file_enabled = true;
|
||||
bool an_output_enabled = false;
|
||||
bool kml_output_enabled = true;
|
||||
bool xml_output_enabled = true;
|
||||
bool rtcm_output_file_enabled = true;
|
||||
bool monitor_enabled = false;
|
||||
bool monitor_ephemeris_enabled = false;
|
||||
bool protobuf_enabled = true;
|
||||
bool enable_rx_clock_correction = true;
|
||||
bool show_local_time_zone = false;
|
||||
bool pre_2009_file = false;
|
||||
bool dump = false;
|
||||
bool dump_mat = true;
|
||||
bool log_source_timetag;
|
||||
bool use_e6_for_pvt = true;
|
||||
bool use_has_corrections = true;
|
||||
bool use_unhealthy_sats = false;
|
||||
|
||||
// PVT KF parameters
|
||||
bool enable_pvt_kf = false;
|
||||
bool estatic_measures_sd = 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;
|
||||
};
|
||||
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_PVT_CONF_H
|
||||
|
|
|
|||
309
src/algorithms/PVT/libs/pvt_kf.cc
Normal file → Executable file
309
src/algorithms/PVT/libs/pvt_kf.cc
Normal file → Executable file
|
|
@ -1,147 +1,162 @@
|
|||
/*!
|
||||
* \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(const arma::vec& p,
|
||||
const arma::vec& v,
|
||||
const arma::vec& res_p,
|
||||
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;
|
||||
|
||||
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 = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.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},
|
||||
{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;
|
||||
|
||||
d_initialized = true;
|
||||
|
||||
DLOG(INFO) << "Ti: " << Ti;
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
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, const arma::vec& res_p)
|
||||
{
|
||||
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
|
||||
try
|
||||
{
|
||||
// Measurement residuals
|
||||
d_R(0, 0) = res_p[0];
|
||||
d_R(0, 1) = res_p[3];
|
||||
d_R(0, 2) = res_p[5];
|
||||
d_R(1, 0) = res_p[3];
|
||||
d_R(1, 1) = res_p[1];
|
||||
d_R(1, 2) = res_p[4];
|
||||
d_R(2, 0) = res_p[5];
|
||||
d_R(2, 1) = res_p[4];
|
||||
d_R(2, 2) = res_p[2];
|
||||
|
||||
// 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);
|
||||
d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t();
|
||||
|
||||
// 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_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);
|
||||
}
|
||||
}
|
||||
/*!
|
||||
* \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(const arma::vec& p,
|
||||
const arma::vec& v,
|
||||
const arma::vec& res_p,
|
||||
double solver_interval_s,
|
||||
bool estatic_measures_sd,
|
||||
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;
|
||||
|
||||
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
|
||||
if(estatic_measures_sd){
|
||||
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)}};
|
||||
|
||||
d_static = true;
|
||||
|
||||
}else{
|
||||
d_R = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.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},
|
||||
{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)}};
|
||||
|
||||
d_static = false;
|
||||
}
|
||||
// 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;
|
||||
|
||||
d_initialized = true;
|
||||
|
||||
DLOG(INFO) << "Ti: " << Ti;
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
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, const arma::vec& res_p)
|
||||
{
|
||||
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
|
||||
try
|
||||
{
|
||||
if(!d_static){
|
||||
// Measurement residuals update
|
||||
d_R(0, 0) = res_p[0];
|
||||
d_R(0, 1) = res_p[3];
|
||||
d_R(0, 2) = res_p[5];
|
||||
d_R(1, 0) = res_p[3];
|
||||
d_R(1, 1) = res_p[1];
|
||||
d_R(1, 2) = res_p[4];
|
||||
d_R(2, 0) = res_p[5];
|
||||
d_R(2, 1) = res_p[4];
|
||||
d_R(2, 2) = res_p[2];
|
||||
}
|
||||
// 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);
|
||||
d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t();
|
||||
|
||||
// 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_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
140
src/algorithms/PVT/libs/pvt_kf.h
Normal file → Executable file
|
|
@ -1,69 +1,71 @@
|
|||
/*!
|
||||
* \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() = default;
|
||||
virtual ~Pvt_Kf() = default;
|
||||
void init_Kf(const arma::vec& p,
|
||||
const arma::vec& v,
|
||||
const arma::vec& res_p,
|
||||
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);
|
||||
bool is_initialized() const;
|
||||
void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p);
|
||||
void get_pv_Kf(arma::vec& p, arma::vec& v) const;
|
||||
void reset_Kf();
|
||||
|
||||
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;
|
||||
bool d_initialized{false};
|
||||
};
|
||||
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_Pvt_Kf_H
|
||||
/*!
|
||||
* \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() = default;
|
||||
virtual ~Pvt_Kf() = default;
|
||||
void init_Kf(const arma::vec& p,
|
||||
const arma::vec& v,
|
||||
const arma::vec& res_p,
|
||||
double solver_interval_s,
|
||||
bool estatic_measures_sd,
|
||||
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, const arma::vec& res_p);
|
||||
void get_pv_Kf(arma::vec& p, arma::vec& v) const;
|
||||
void reset_Kf();
|
||||
|
||||
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;
|
||||
bool d_initialized{false};
|
||||
bool d_static{false};
|
||||
};
|
||||
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_Pvt_Kf_H
|
||||
|
|
|
|||
3559
src/algorithms/PVT/libs/rtklib_solver.cc
Normal file → Executable file
3559
src/algorithms/PVT/libs/rtklib_solver.cc
Normal file → Executable file
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user