diff --git a/conf/File_input/MultiCons/gnss-sdr_GPS_Galileo_VTL.conf b/conf/File_input/MultiCons/gnss-sdr_GPS_Galileo_VTL.conf new file mode 100755 index 000000000..1f589e69f --- /dev/null +++ b/conf/File_input/MultiCons/gnss-sdr_GPS_Galileo_VTL.conf @@ -0,0 +1,262 @@ +; This is a GNSS-SDR configuration file +; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/ +; SPDX-License-Identifier: GPL-3.0-or-later +; SPDX-FileCopyrightText: (C) 2010-2020 (see AUTHORS file for a list of contributors) + +; You can define your own receiver and invoke it by doing +; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf +; + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +GNSS-SDR.internal_fs_sps=12500000 +GNSS-SDR.telecommand_enabled=true +GNSS-SDR.telecommand_tcp_port=3333 + +;######### SUPL RRLP GPS assistance configuration ##### +; Check https://www.mcc-mnc.com/ +; On Android: https://play.google.com/store/apps/details?id=net.its_here.cellidinfo&hl=en +GNSS-SDR.SUPL_gps_enabled=false +GNSS-SDR.SUPL_read_gps_assistance_xml=true +GNSS-SDR.SUPL_gps_ephemeris_server=supl.google.com +GNSS-SDR.SUPL_gps_ephemeris_port=7275 +GNSS-SDR.SUPL_gps_acquisition_server=supl.google.com +GNSS-SDR.SUPL_gps_acquisition_port=7275 +GNSS-SDR.SUPL_MCC=244 +GNSS-SDR.SUPL_MNC=5 +GNSS-SDR.SUPL_LAC=0x59e2 +GNSS-SDR.SUPL_CI=0x31b0 + +GNSS-SDR.osnma_enable=false + + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=File_Signal_Source +SignalSource.filename= ; <- PUT YOUR FILE HERE +SignalSource.item_type=ibyte +SignalSource.sampling_frequency=12500000 + + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner + + +;######### DATA_TYPE_ADAPTER CONFIG ############ +DataTypeAdapter.implementation=Ibyte_To_Complex + + +;######### INPUT_FILTER CONFIG ############ +InputFilter.implementation=Pass_Through +InputFilter.input_item_type=gr_complex +InputFilter.output_item_type=gr_complex + + +;######### RESAMPLER CONFIG ############ +Resampler.implementation=Pass_Through +Resampler.item_type=gr_complex +Resampler.sample_freq_in=12500000 +Resampler.sample_freq_out=12500000 + + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1C.count=10 +Channels_L5.count=0 +Channels_1B.count=10 +Channels_5X.count=0 +Channels.in_acquisition=1 + +Channel0.signal=1C +Channel1.signal=1C +Channel2.signal=1C +Channel3.signal=1C +Channel4.signal=1C +Channel5.signal=1C +Channel6.signal=1C +Channel7.signal=1C +Channel8.signal=1C +Channel9.signal=1C + +Channel10.signal=1B +Channel11.signal=1B +Channel12.signal=1B +Channel13.signal=1B +Channel14.signal=1B +Channel15.signal=1B +Channel16.signal=1B +Channel17.signal=1B +Channel18.signal=1B +Channel19.signal=1B + +Channel20.signal=1B +Channel21.signal=1B +Channel22.signal=1B +Channel23.signal=1B +Channel24.signal=1B +Channel25.signal=1B +Channel26.signal=1B +Channel27.signal=1B +Channel28.signal=1B +Channel29.signal=1B + +Channel30.signal=5X +Channel31.signal=5X +Channel32.signal=5X +Channel33.signal=5X +Channel34.signal=5X +Channel35.signal=5X +Channel36.signal=5X +Channel37.signal=5X +Channel38.signal=5X +Channel39.signal=5X + + +;######### ACQUISITION CONFIG ############ +;# GPS L1 +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.item_type=gr_complex +Acquisition_1C.threshold=2 +Acquisition_1C.doppler_max=5000 +Acquisition_1C.doppler_step=125 +Acquisition_1C.dump=false +Acquisition_1C.dump_filename=./acq_1C_dump.dat +;# GPS L5 +Acquisition_L5.implementation=GPS_L5i_PCPS_Acquisition +Acquisition_L5.item_type=gr_complex +Acquisition_L5.threshold=2 +Acquisition_L5.doppler_max=5000 +Acquisition_L5.doppler_step=125 +Acquisition_L5.dump=false +Acquisition_L5.dump_filename=./acq_L5_dump.dat +;# Galileo E1b +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.item_type=gr_complex +Acquisition_1B.threshold=2 +Acquisition_1B.doppler_max=5000 +Acquisition_1B.doppler_step=125 +Acquisition_1B.dump=false +Acquisition_1B.dump_filename=./acq_1B_dump.dat +;# Galileo E5a +Acquisition_5X.implementation=Galileo_E5a_Pcps_Acquisition +Acquisition_5X.item_type=gr_complex +Acquisition_5X.threshold=2 +Acquisition_5X.doppler_max=5000 +Acquisition_5X.doppler_step=125 +Acquisition_5X.dump=false +Acquisition_5X.dump_filename=./acq_5X_dump.dat + + +;######### TRACKING CONFIG ############ +;# GPS L1 +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.dump=false +Tracking_1C.dump_mat=false +Tracking_1C.dump_filename=./trck_1C_dump.dat +;# GPS L5 +Tracking_L5.implementation=GPS_L5_DLL_PLL_Tracking +Tracking_L5.item_type=gr_complex +Tracking_L5.dump=false +Tracking_L5.dump_mat=false +Tracking_L5.dump_filename=./trck_L5_dump.dat +;# Galileo E1b +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +Tracking_1B.item_type=gr_complex +Tracking_1B.dump=false +Tracking_1B.dump_mat=false +Tracking_1B.dump_filename=./trck_1B_dump.dat +;# Galileo E5a +Tracking_5X.implementation=Galileo_E5a_DLL_PLL_Tracking +Tracking_5X.item_type=gr_complex +Tracking_5X.dump=false +Tracking_5X.dump_mat=false +Tracking_5X.dump_filename=./trck_5X_dump.dat + + +;######### TELEMETRY DECODER CONFIG ############ +;# GPS L1 +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false +TelemetryDecoder_1C.dump_mat=false +TelemetryDecoder_1C.dump_filename=./telem_1C_dump.dat +;# GPS L5 +TelemetryDecoder_L5.implementation=GPS_L5_Telemetry_Decoder +TelemetryDecoder_L5.dump=false +TelemetryDecoder_L5.dump_mat=false +TelemetryDecoder_L5.dump_filename=./telem_L5_dump.dat +;# Galileo E1b +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false +TelemetryDecoder_1B.dump_mat=false +TelemetryDecoder_1B.dump_filename=./telem_1B_dump.dat +;# Galileo E5a +TelemetryDecoder_5X.implementation=Galileo_E5a_Telemetry_Decoder +TelemetryDecoder_5X.dump=false +TelemetryDecoder_5X.dump_mat=false +TelemetryDecoder_5X.dump_filename=./telem_5X_dump.dat + + +;######### OBSERVABLES CONFIG ############ +Observables.implementation=Hybrid_Observables +Observables.dump=false +Observables.dump_mat=false +Observables.dump_filename=./observables.dat + + +;######### PVT CONFIG ############ +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=Single;PPP_Kinematic;PPP_Static; +PVT.output_rate_ms=500 +PVT.display_rate_ms=500 +PVT.iono_model=Broadcast;Iono-Free-LC; +PVT.trop_model=Saastamoinen +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 +PVT.rtcm_tcp_port=2101 +PVT.rtcm_MT1019_rate_ms=5000 +PVT.rtcm_MT1077_rate_ms=1000 +PVT.rinex_output_enabled=true +PVT.rinexobs_rate_ms=1000 +PVT.enable_monitor=true +PVT.monitor_client_addresses=127.0.0.1 +PVT.monitor_udp_port=0101 +PVT.flag_nmea_tty_port=true +PVT.nmea_dump_devname=/dev/pts/12 +PVT.nmea_output_file_enabled=true +PVT.enable_rx_clock_correction=false +PVT.elevation_mask=5 +PVT.dump=true +PVT.dump_mat=false +PVT.dump_filename=./PVT.dat +PVT.nmea_dump_filename=NMEA.nmea +PVT.rinex_output_path=./ +;# PVT KF +PVT.enable_pvt_kf=false ; PVT denoising +PVT.kf_measures_ecef_pos_sd_m=1.0 +PVT.kf_measures_ecef_vel_sd_ms=0.1 +PVT.kf_system_ecef_pos_sd_m=0.001 ; static scenario +PVT.kf_system_ecef_pos_sd_m=0.001 +;# VECTOR TRACKING CONFIG +PVT.enable_pvt_vtl=true ; enable/disable VTL +PVT.enable_pvt_output_vtl=false ; enable/disable receiver to use VTL solution +PVT.enable_pvt_closure_vtl=false ; enable/disable VTL feedback to tracking loops +PVT.vtl_kinematic=false ; enable/disable kinematic model +PVT.vtl_init_pos_ecef_sd_m=10.0 +PVT.vtl_init_vel_ecef_sd_ms=5.0 +PVT.vtl_init_clk_b_sd_m=100 +PVT.vtl_init_clk_d_sd_ms=100 +PVT.vtl_sys_acc_noise_sd_ms2=0.316 +PVT.vtl_sys_clk_b_noise_sd_m=0.0 +PVT.vtl_sys_clk_d_noise_sd_ms=0.0 +PVT.vtl_meas_prange_sd_m=7.75 +PVT.vtl_meas_prange_rate_sd_ms=0.45 +PVT.vtl_dump=true ; enable/disable VTL status dump +PVT.vtl_dump_filename=./vtl_dump.dat + + +;######### MONITOR CONFIG ############ +Monitor.enable_monitor=false +Monitor.decimator_factor=50 +Monitor.client_addresses=127.0.0.1 +Monitor.udp_port=0101 \ No newline at end of file diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index aac9ce12f..307f53f68 100755 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -27,6 +27,7 @@ set(PVT_LIB_SOURCES signal_enabled_flags.cc receiver_type.cc vtl_data.cc + vtl_core.cc ) set(PVT_LIB_HEADERS diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index fef2dfcb6..92812a458 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -58,7 +58,8 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, d_signal_enabled_flags(signal_enabled_flags), d_flag_dump_enabled(flag_dump_to_file), d_flag_dump_mat_enabled(flag_dump_to_mat), - vtl_data(nullptr) + vtl_data(nullptr), + vtl_Core(nullptr) { // see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc // function: satwavelen @@ -155,6 +156,8 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, { vtl_data = std::make_unique(); vtl_data->init_storage(d_conf.vtl_gps_channels + d_conf.vtl_gal_channels); + vtl_Core = std::make_unique(d_conf); + vtl_Core->vtl_init(d_conf); } } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h old mode 100755 new mode 100644 index e92390537..57319db5a --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -62,6 +62,7 @@ #include "rtklib.h" #include "rtklib_conversions.h" #include "sensor_data/sensor_data_aggregator.h" +#include "vtl_core.h" #include "vtl_data.h" #include #include @@ -162,6 +163,7 @@ private: // vector tracking std::unique_ptr vtl_data; + std::unique_ptr vtl_Core; }; diff --git a/src/algorithms/PVT/libs/vtl_core.cc b/src/algorithms/PVT/libs/vtl_core.cc new file mode 100755 index 000000000..7db4f1990 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_core.cc @@ -0,0 +1,736 @@ +/*! + * \file vtl_core.cc + * \brief Class that implements a Vector Tracking Loop (VTL) + * \author Pedro Pereira, 2025. pereirapedrocp@gmail.com + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "vtl_core.h" +#include "gnss_sdr_filesystem.h" +#include + +#if USE_GLOG_AND_GFLAGS +#include +#else +#include +#endif + + +Vtl_Core::Vtl_Core(const Pvt_Conf &conf) + : N_gps_ch(conf.vtl_gps_channels), // allocated GPS channels + N_gal_ch(conf.vtl_gal_channels), // allocated GAL channels + dump_enabled(conf.vtl_dump), // vtl dump flag + dump_filename(conf.vtl_dump_filename), // vtl dump file name + N_ch(conf.vtl_gps_channels + conf.vtl_gal_channels), // total allocated channels + N_st((conf.vtl_gal_channels > 0) ? 9 : 8), // x, vx, y, vy, z, vz, b (gps), b (gal), d + N_meas(N_ch * 2), // 3 measurements (pr, prr, pracc) per channel + N_st_idx(arma::linspace(0, N_st - 1, N_st)), + i_clkb_E(i_clkb_G + 1), + i_clkd((conf.vtl_gal_channels > 0) ? (i_clkb_G + 2) : (i_clkb_G + 1)) +{ + if (dump_enabled == true) + { + if (dump_file.is_open() == false) + { + try + { + dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); + dump_file.open(dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "VTL lib dump enabled Log file: " << dump_filename.c_str(); + } + catch (const std::ofstream::failure &e) + { + dump_enabled = false; + LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what(); + } + } + } +} + +Vtl_Core::~Vtl_Core() +{ + if (dump_file.is_open() == true) + { + const auto pos = dump_file.tellp(); + try + { + dump_file.close(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor closing the VTL dump file " << ex.what(); + } + } +} + +void Vtl_Core::vtl_init(const Pvt_Conf &conf) +{ + epoch = 0; + N_meascov_updt_cnt = 0; + + vtl_kinematic = conf.vtl_kinematic; + vtl_init_pos_ecef_var_m2 = std::pow(conf.vtl_init_pos_ecef_sd_m, 2); + vtl_init_vel_ecef_var_m2s2 = std::pow(conf.vtl_init_vel_ecef_sd_ms, 2); + vtl_init_clk_b_var_m2 = std::pow(conf.vtl_init_clk_b_sd_m, 2); + vtl_init_clk_d_var_m2s2 = std::pow(conf.vtl_init_clk_d_sd_ms, 2); + vtl_sys_acc_noise_var_m2s4 = std::pow(conf.vtl_sys_acc_noise_sd_ms2, 2); + vtl_sys_clk_b_noise_var_m2 = std::pow(conf.vtl_sys_clk_b_noise_sd_m, 2); + vtl_sys_clk_d_noise_var_m2s2 = std::pow(conf.vtl_sys_clk_d_noise_sd_ms, 2); + vtl_meas_prange_var_m2 = std::pow(conf.vtl_meas_prange_sd_m, 2); + vtl_meas_prange_rate_var_m2s2 = std::pow(conf.vtl_meas_prange_rate_sd_ms, 2); + + ekf_X = arma::zeros(N_st, 1); + ekf_X_updt = arma::zeros(N_st, 1); + ekf_obs_Z = arma::zeros(N_meas, 1); + ekf_comp_Z = arma::zeros(N_meas, 1); + ekf_updt_comp_Z = arma::zeros(N_meas, 1); + ekf_updt_comp_Z_copy = arma::zeros(N_meas, 1); + ekf_updt_comp_Z_last = arma::zeros(N_meas, 1); + ekf_preFit = arma::zeros(N_meas, 1); + ekf_postFit = arma::zeros(N_meas, 1); + ekf_S = arma::zeros(N_meas, N_meas); + ekf_K = arma::zeros(N_st, N_meas); + rho = arma::zeros(N_ch, 1); + rho_squared = arma::zeros(N_ch, 1); + u_x = arma::zeros(N_ch, 1); + u_y = arma::zeros(N_ch, 1); + u_z = arma::zeros(N_ch, 1); + vtl_feedback = arma::zeros(N_ch, 1); + epochs_to_recover = num_recovery_epochs; + + // initialize transition matrix + ekf_F = arma::eye(N_st, N_st); + ekf_F_t = arma::eye(N_st, N_st); + + // initialize process noise covariance + ekf_Q.zeros(N_st, N_st); + + // initialize measurement noise covariance matrix + ekf_R = arma::zeros(N_meas, N_meas); +} + +bool Vtl_Core::vtl_work(const Vtl_Data &rtklib_data) +{ + // indexants for available satellites + aidx = arma::find(rtklib_data.active_ch == 1); + ext_aidx = arma::join_cols(aidx, aidx + N_ch); + + // timming + dt_s = rtklib_data.dt_s; + // recover from observables jump when PVT fails + if (dt_s > 2) + { + epochs_to_recover = num_recovery_epochs; + } + + if (epochs_to_recover == 0) + { + //************************* + // EKF PREDICTION + //************************* + update_process(dt_s); // irregular vtl epoch intervals + ekf_X = ekf_F * ekf_X; + ekf_P = ekf_F * ekf_P * ekf_F_t + ekf_Q; + + + //************************* + // EKF MEASUREMENT + //************************* + compute_prefit(rtklib_data); + + + //************************* + // EKF UPDATE + //************************* + const arma::uvec newidx = arma::find(rtklib_data.new_ch == 1); + if ((newidx.n_elem > 0) || (++N_meascov_updt_cnt == N_meascov_updt)) + { + update_meas_cov(rtklib_data, aidx, true); + N_meascov_updt_cnt = 0; + } + + update_jacobian(rtklib_data); + + const arma::mat J_sub = ekf_J(ext_aidx, N_st_idx); + const arma::mat R_sub = ekf_R(ext_aidx, ext_aidx); + ekf_P_J_t = ekf_P * J_sub.t(); + ekf_S = J_sub * ekf_P_J_t + R_sub; + ekf_K = arma::solve(ekf_S, ekf_P_J_t.t()).t(); + ekf_P_aux = ekf_eye - ekf_K * J_sub; + ekf_P = ekf_P_aux * ekf_P * ekf_P_aux.t() + ekf_K * R_sub * ekf_K.t(); + ekf_X_updt = ekf_K * ekf_preFit(ext_aidx); + ekf_X += ekf_X_updt; + + + // post fits/residuals + ekf_postFit = ekf_preFit - (ekf_J * ekf_X_updt); + // updated computed measurement + ekf_updt_comp_Z = ekf_obs_Z - ekf_postFit; + + + //************************* + // VTL LOOP CLOSURE + //************************* + send_vtl_feedback(rtklib_data); + } + else + { + vtl_reset(rtklib_data); + epochs_to_recover--; + } + + + // log VTL data + if (dump_enabled) + { + saveVTLdata(rtklib_data); + } + + epoch++; + return true; +} + +void Vtl_Core::vtl_reset(const Vtl_Data &rtk_data) +{ + // initialize with rx position from rtklib + ekf_X(i_px) = rtk_data.rx_p(0); + ekf_X(i_vx) = rtk_data.rx_v(0); + ekf_X(i_py) = rtk_data.rx_p(1); + ekf_X(i_vy) = rtk_data.rx_v(1); + ekf_X(i_pz) = rtk_data.rx_p(2); + ekf_X(i_vz) = rtk_data.rx_v(2); + ekf_X(i_clkb_G) = rtk_data.rx_clk(0); + if (N_gal_ch > 0) + { + ekf_X(i_clkb_E) = rtk_data.rx_clk(1); + } + ekf_X(i_clkd) = rtk_data.rx_clk(2); + + dt_s = default_vtl_dt; + + // initialize state covariance matrix + ekf_P = arma::zeros(N_st, N_st); + ekf_P_aux = arma::zeros(N_st, N_st); + ekf_eye = arma::eye(size(ekf_P)); + ekf_P(i_px, i_px) = vtl_init_pos_ecef_var_m2; + ekf_P(i_py, i_py) = vtl_init_pos_ecef_var_m2; + ekf_P(i_pz, i_pz) = vtl_init_pos_ecef_var_m2; + ekf_P(i_vx, i_vx) = vtl_init_vel_ecef_var_m2s2; + ekf_P(i_vy, i_vy) = vtl_init_vel_ecef_var_m2s2; + ekf_P(i_vz, i_vz) = vtl_init_vel_ecef_var_m2s2; + ekf_P(i_clkb_G, i_clkb_G) = vtl_init_clk_b_var_m2; + if (N_gal_ch > 0) + { + ekf_P(i_clkb_E, i_clkb_E) = vtl_init_clk_b_var_m2; + } + ekf_P(i_clkd, i_clkd) = vtl_init_clk_d_var_m2s2; + + // initialize jacobian matrix + ekf_J = arma::zeros(N_meas, N_st); + ekf_P_J_t = arma::zeros(N_st, N_meas); + // derivative of pseudorange w.r.t. clk bias - GPS/GAL + ekf_J(arma::span(0, N_gps_ch - 1), i_clkb_G).ones(); + if (N_gal_ch > 0) + { + ekf_J(arma::span(N_gps_ch, N_ch - 1), i_clkb_E).ones(); + } + // derivative of pseudorange rate w.r.t. clk drift + ekf_J(arma::span(N_ch, 2 * N_ch - 1), i_clkd).ones(); + + + // initialize measurement noise covariance + update_meas_cov(rtk_data, aidx, false); + N_meascov_updt_cnt = 0; +} + +void Vtl_Core::update_process(double dt_s) +{ + // update transition matrix + const double T = dt_s; + const double T2 = T * T; + const double T3 = T2 * T; + const double T4 = T3 * T; + double p_v = vtl_kinematic ? T : 0; + + ekf_F(i_px, i_vx) = p_v; + ekf_F(i_py, i_vy) = p_v; + ekf_F(i_pz, i_vz) = p_v; + ekf_F(i_clkb_G, i_clkd) = T; + if (N_gal_ch > 0) + { + ekf_F(i_clkb_E, i_clkd) = T; + } + ekf_F_t = ekf_F.t(); + + + // update process noise matrix + const double q_pp = vtl_sys_acc_noise_var_m2s4 * T4 / 4; + const double q_pv = vtl_sys_acc_noise_var_m2s4 * T3 / 2; + const double q_vv = vtl_sys_acc_noise_var_m2s4 * T2; + + // PVA + arma::mat Q_PV(2, 2, arma::fill::zeros); + Q_PV(i_px, i_px) = q_pp; + Q_PV(i_px, i_vx) = q_pv; + Q_PV(i_vx, i_px) = q_pv; + Q_PV(i_vx, i_vx) = q_vv; + // 3 motion axes (x, y, z) + ekf_Q.submat(i_px, i_px, i_vx, i_vx) = vtl_kinematic ? Q_PV : arma::zeros(2, 2); + ekf_Q.submat(i_py, i_py, i_vy, i_vy) = vtl_kinematic ? Q_PV : arma::zeros(2, 2); + ekf_Q.submat(i_pz, i_pz, i_vz, i_vz) = vtl_kinematic ? Q_PV : arma::zeros(2, 2); + + // Clock + const double clk_b_noise_var_m2 = S_f + vtl_sys_clk_b_noise_var_m2; + const double clk_d_noise_var_m2s2 = S_g + vtl_sys_clk_d_noise_var_m2s2; + const double q_bb = clk_b_noise_var_m2 * T2 + clk_d_noise_var_m2s2 * T4 / 4; + const double q_bd = clk_d_noise_var_m2s2 * T3 / 2; + const double q_dd = clk_d_noise_var_m2s2 * T2; + + ekf_Q(i_clkb_G, i_clkb_G) = q_bb; + ekf_Q(i_clkb_G, i_clkd) = q_bd; + ekf_Q(i_clkd, i_clkb_G) = q_bd; + ekf_Q(i_clkd, i_clkd) = q_dd; + if (N_gal_ch > 0) + { + ekf_Q(i_clkb_E, i_clkb_E) = q_bb; + ekf_Q(i_clkb_E, i_clkb_G) = q_bb; + ekf_Q(i_clkb_G, i_clkb_E) = q_bb; + ekf_Q(i_clkb_E, i_clkd) = q_bd; + ekf_Q(i_clkd, i_clkb_E) = q_bd; + } +} + +void Vtl_Core::compute_prefit(const Vtl_Data &rtk_data) +{ + // observed measurements + ekf_obs_Z(aidx) = rtk_data.obs_pr(aidx); // pseudorange + ekf_obs_Z(aidx + N_ch) = rtk_data.obs_prr(aidx); // pseudorange rate + + // computed measurements + // Geometric distance + const arma::vec dx = rtk_data.sv_p(aidx, arma::uvec{0}) - ekf_X(i_px); // Delta X + const arma::vec dy = rtk_data.sv_p(aidx, arma::uvec{1}) - ekf_X(i_py); // Delta Y + const arma::vec dz = rtk_data.sv_p(aidx, arma::uvec{2}) - ekf_X(i_pz); // Delta Z + rho_squared(aidx) = arma::square(dx) + arma::square(dy) + arma::square(dz); + rho(aidx) = arma::sqrt(rho_squared(aidx)); + // LOS unit vector + u_x(aidx) = dx / rho(aidx); + u_y(aidx) = dy / rho(aidx); + u_z(aidx) = dz / rho(aidx); + + // computed pseudorange + arma::vec clk_bias(aidx.n_elem); + arma::uvec gps_sel = arma::find(aidx < N_gps_ch); + arma::uvec gal_sel = arma::find(aidx >= N_gps_ch); + clk_bias(gps_sel).fill(ekf_X(i_clkb_G)); + if (N_gal_ch > 0) + { + clk_bias(gal_sel).fill(ekf_X(i_clkb_E)); + } + arma::vec sagnac_pr = (rtk_data.sv_p(aidx, arma::uvec{0}) * ekf_X(i_py) - rtk_data.sv_p(aidx, arma::uvec{1}) * ekf_X(i_px)) * GNSS_OMEGA_EARTH_DOT / SPEED_OF_LIGHT_M_S; + ekf_comp_Z(aidx) = rho(aidx) + clk_bias - rtk_data.sv_clk(aidx, arma::uvec{0}) + rtk_data.tropo_bias(aidx) + rtk_data.iono_bias(aidx) + rtk_data.code_bias(aidx) + sagnac_pr; + + + // computed pseudorange rate + const arma::vec vx = rtk_data.sv_v(aidx, arma::uvec{0}) - ekf_X(i_vx); // Velocity X + const arma::vec vy = rtk_data.sv_v(aidx, arma::uvec{1}) - ekf_X(i_vy); // Velocity Y + const arma::vec vz = rtk_data.sv_v(aidx, arma::uvec{2}) - ekf_X(i_vz); // Velocity Z + arma::vec sagnac_prr = (rtk_data.sv_v(aidx, arma::uvec{1}) * ekf_X(i_px) + rtk_data.sv_p(aidx, arma::uvec{1}) * ekf_X(i_vx) - + rtk_data.sv_v(aidx, arma::uvec{0}) * ekf_X(i_py) - rtk_data.sv_p(aidx, arma::uvec{0}) * ekf_X(i_vy)) * + GNSS_OMEGA_EARTH_DOT / SPEED_OF_LIGHT_M_S; + ekf_comp_Z(aidx + N_ch) = (vx % u_x(aidx)) + (vy % u_y(aidx)) + (vz % u_z(aidx)) + + ekf_X(i_clkd) - rtk_data.sv_clk(aidx, arma::uvec{1}) + + sagnac_prr; + + + // measurement innovation (observed minus computed) + ekf_preFit(ext_aidx) = ekf_obs_Z(ext_aidx) - ekf_comp_Z(ext_aidx); +} + +void Vtl_Core::update_jacobian(const Vtl_Data &rtk_data) +{ + // Pre-cache unit vector slices + const arma::colvec ux = u_x(aidx); + const arma::colvec uy = u_y(aidx); + const arma::colvec uz = u_z(aidx); + const arma::colvec rho_v = rho(aidx); + const arma::colvec rho_squared_v = rho_squared(aidx); + + // derivative of pseudorange w.r.t. receiver position + ekf_J(aidx, arma::uvec{i_px}) = -ux; + ekf_J(aidx, arma::uvec{i_py}) = -uy; + ekf_J(aidx, arma::uvec{i_pz}) = -uz; + // derivative of pseudorange rate w.r.t. receiver position + const arma::colvec aux = (rtk_data.sv_v(aidx, arma::uvec{0}) - ekf_X(i_vx)) % (rtk_data.sv_p(aidx, arma::uvec{0}) - ekf_X(i_px)) + + (rtk_data.sv_v(aidx, arma::uvec{1}) - ekf_X(i_vy)) % (rtk_data.sv_p(aidx, arma::uvec{1}) - ekf_X(i_py)) + + (rtk_data.sv_v(aidx, arma::uvec{2}) - ekf_X(i_vz)) % (rtk_data.sv_p(aidx, arma::uvec{2}) - ekf_X(i_pz)); + ekf_J(aidx + N_ch, arma::uvec{i_px}) = (-rho_v % (rtk_data.sv_v(aidx, arma::uvec{0}) - ekf_X(i_vx)) + ux % aux) / rho_squared_v; + ekf_J(aidx + N_ch, arma::uvec{i_py}) = (-rho_v % (rtk_data.sv_v(aidx, arma::uvec{1}) - ekf_X(i_vy)) + uy % aux) / rho_squared_v; + ekf_J(aidx + N_ch, arma::uvec{i_pz}) = (-rho_v % (rtk_data.sv_v(aidx, arma::uvec{2}) - ekf_X(i_vz)) + uz % aux) / rho_squared_v; + // derivative of pseudorange rate w.r.t. receiver velocity + ekf_J(aidx + N_ch, arma::uvec{i_vx}) = -ux; + ekf_J(aidx + N_ch, arma::uvec{i_vy}) = -uy; + ekf_J(aidx + N_ch, arma::uvec{i_vz}) = -uz; +} + +void Vtl_Core::update_meas_cov(const Vtl_Data &rtk_data, const arma::uvec &meas_idx, bool preFit_available) +{ + arma::vec ss_score_norm; + arma::vec inv_Sin2elev; + double meas_cov; + + if (preFit_available) + { + double max_cn0 = rtk_data.CN0_dB_hz.max(); + arma::vec w = rtk_data.CN0_dB_hz(aidx) / max_cn0; + const arma::vec ss_score = ss_downdate_fast(w); + ss_score_norm = ss_score * 10; + } + else + { + arma::vec sin_elev = arma::sin(rtk_data.sv_elev(meas_idx)); + inv_Sin2elev = 1.0 / arma::square(sin_elev); + } + + for (arma::uword i = 0; i < meas_idx.n_elem; i++) + { + meas_cov = preFit_available ? (1 / ss_score_norm(i)) : inv_Sin2elev(i); + ekf_R(meas_idx(i), meas_idx(i)) = vtl_meas_prange_var_m2 * meas_cov; + ekf_R(meas_idx(i) + N_ch, meas_idx(i) + N_ch) = vtl_meas_prange_rate_var_m2s2 * meas_cov; + } +} + +void Vtl_Core::send_vtl_feedback(const Vtl_Data &rtk_data) +{ + trk_cmd_outs.clear(); + + // unavailable signals go back to traditional tracking + const arma::uvec oldidx = arma::find(rtk_data.old_ch == 1); + + size_t ratio = (rtk_data.ionoopt == IONOOPT_IFLC ? 2 : 1); + trk_cmd_outs.reserve((aidx.n_elem + oldidx.n_elem) * ratio); + + vtl_feedback.zeros(); + + TrackingCmd trk_cmd; + + double carrier_lambda = 0; + uint32_t code_freq = 0; + double range_factor = 0; + for (arma::uword i = 0; i < aidx.n_elem; ++i) + { + if (rtk_data.band(aidx(i))) // L5/E5 + { + carrier_lambda = Lambda_GPS_L5; + code_freq = L5E5_CODE_FREQ; + range_factor = RANGE_TO_FREQ_L5E5_FACTOR; + } + else // L1/E1 + { + carrier_lambda = Lambda_GPS_L1; + code_freq = L1E1_CODE_FREQ; + range_factor = RANGE_TO_FREQ_L1E1_FACTOR; + } + + // channel info + trk_cmd.channel_id = aidx(i); + trk_cmd.prn_id = rtk_data.sv_id(aidx(i)); + trk_cmd.ch_sample_counter = rtk_data.ch_sample_counter(aidx(i)); + // PLL + trk_cmd.enable_pll_vtl_feedack = false; + trk_cmd.pll_vtl_freq_hz = -ekf_updt_comp_Z(aidx(i) + N_ch) / carrier_lambda; // pseudorange rate + trk_cmd.carrier_freq_rate_hz_s = 0; // pseudorange acceleration + // DLL + trk_cmd.enable_dll_vtl_feedack = rtk_data.loop_closure(aidx(i)); // VDLL only + trk_cmd.dll_vtl_freq_hz = code_freq - ekf_updt_comp_Z(aidx(i) + N_ch) * range_factor; // pseudorange rate + + trk_cmd_outs.push_back(trk_cmd); + + // when iono-free linear combination is used VTL should control tracking channels of both frequencies + // because both observations are being used. That's not the case when other ionosphere option is used + if (rtk_data.ionoopt == IONOOPT_IFLC) + { + // channel info + trk_cmd.channel_id = rtk_data.rx_ch2(aidx(i)); + trk_cmd.ch_sample_counter = rtk_data.ch2_sample_counter(aidx(i)); + // PLL + trk_cmd.pll_vtl_freq_hz = -ekf_updt_comp_Z(aidx(i) + N_ch) / Lambda_GPS_L5; // pseudorange rate + trk_cmd.carrier_freq_rate_hz_s = 0; // pseudorange acceleration + // DLL + trk_cmd.dll_vtl_freq_hz = L5E5_CODE_FREQ - ekf_updt_comp_Z(aidx(i) + N_ch) * RANGE_TO_FREQ_L5E5_FACTOR; // pseudorange rate + + trk_cmd_outs.push_back(trk_cmd); + } + vtl_feedback(aidx(i)) = trk_cmd.dll_vtl_freq_hz; // log + } + ekf_updt_comp_Z_last = ekf_updt_comp_Z; // store current computed measurement + + for (arma::uword i = 0; i < oldidx.n_elem; ++i) + { + // channel info + trk_cmd.channel_id = oldidx(i); + trk_cmd.prn_id = rtk_data.sv_id(oldidx(i)); + trk_cmd.ch_sample_counter = 0; + // PLL + trk_cmd.enable_pll_vtl_feedack = false; + trk_cmd.pll_vtl_freq_hz = 0; + trk_cmd.carrier_freq_rate_hz_s = 0; + // DLL + trk_cmd.enable_dll_vtl_feedack = false; + trk_cmd.dll_vtl_freq_hz = 0; + trk_cmd_outs.push_back(trk_cmd); + + if (rtk_data.ionoopt == IONOOPT_IFLC) + { + // channel info + trk_cmd.channel_id = rtk_data.rx_ch2(oldidx(i)); + trk_cmd_outs.push_back(trk_cmd); + } + } +} + +const arma::colvec &Vtl_Core::get_rx_pvt() const +{ + return ekf_X; +} + +const std::vector &Vtl_Core::get_trk_cmd_outs() const +{ + return trk_cmd_outs; +} + +const arma::vec Vtl_Core::ss_downdate_fast(const arma::vec &w) +{ + arma::uvec idx = arma::regspace(0, aidx.n_elem - 1); // initial indices into aidx + // Build G matrix directly from LOS vectors + arma::mat G(aidx.n_elem, 3); + G.col(0) = u_x.elem(aidx); + G.col(1) = u_y.elem(aidx); + G.col(2) = u_z.elem(aidx); + + // Weighted normal equations: C = inv(G^t W G) + const arma::mat W = arma::diagmat(w); + const arma::mat WG = W * G; + const arma::mat GTWG = G.t() * WG; + const arma::mat C = arma::pinv(GTWG); + + // Compute S = C * G^t * W + // Weighed Least Squares + const arma::mat S = C * G.t() * W; + + // Compute P diagonal: only need diagonal of W - WGS + // P acts as the residuals of each satellite contribution to the solution + const arma::mat P_diag_vec = W.diag() - arma::diagvec(WG * S); + + // Compute influence score: sum(S^2) / P_diag + arma::vec influence = arma::sum(arma::square(S), 0).t(); + // normalization - to mix the weight given to each satellite and their geometric value + arma::vec score = influence / P_diag_vec; + + // Handle NaNs or infs (e.g., division by zero) + score.elem(arma::find_nonfinite(score)).zeros(); + + return score; +} + +void Vtl_Core::saveVTLdata(const Vtl_Data &rtk_data) +{ + try + { + double tmp_double; + uint32_t tmp_uint32; + // epoch + tmp_uint32 = epoch; + dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // rx time + tmp_double = rtk_data.rx_time; + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // dt + tmp_double = rtk_data.dt_s; + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // ekf rx p + tmp_double = ekf_X(i_px); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = ekf_X(i_py); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = ekf_X(i_pz); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // ekf rx v + tmp_double = ekf_X(i_vx); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = ekf_X(i_vy); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = ekf_X(i_vz); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // ekf rx b gps + tmp_double = ekf_X(i_clkb_G); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + if (N_gal_ch > 0) + { + // ekf rx b gal + tmp_double = ekf_X(i_clkb_E); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // ekf rx d + tmp_double = ekf_X(i_clkd); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // rtk rx p + tmp_double = rtk_data.rx_p(0); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = rtk_data.rx_p(1); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = rtk_data.rx_p(2); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // rtk rx v + tmp_double = rtk_data.rx_v(0); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = rtk_data.rx_v(1); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = rtk_data.rx_v(2); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // rtk rx b gps + tmp_double = rtk_data.rx_clk(0); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // rtk rx b gal + tmp_double = rtk_data.rx_clk(1); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // rtk rx d + tmp_double = rtk_data.rx_clk(2); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // active channels + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.active_ch(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + + // ekf pre-fit + for (int i = 0; i < 2 * N_ch; i++) + { + tmp_double = ekf_preFit(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // ekf post-fit + for (int i = 0; i < 2 * N_ch; i++) + { + tmp_double = ekf_postFit(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + + // ekf measurement covariance + for (int i = 0; i < 2 * N_ch; i++) + { + tmp_double = ekf_R(i, i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + + // ekf process noise + for (int i = 0; i < N_st; i++) + { + tmp_double = ekf_Q(i, i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + + // rtk code pseudorange + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.obs_pr(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // rtk code pseudorange rate + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.obs_prr(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // vtl code pseudorange + for (int i = 0; i < N_ch; i++) + { + tmp_double = ekf_comp_Z(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // vtl code pseudorange rate + for (int i = 0; i < N_ch; i++) + { + tmp_double = ekf_comp_Z(i + N_ch); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // rtk code freq + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.code_freq(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // VTL code freq + for (int i = 0; i < N_ch; i++) + { + tmp_double = vtl_feedback(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + + // SV position X + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.sv_p(i, 0); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // SV position Y + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.sv_p(i, 1); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // SV position Z + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.sv_p(i, 2); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + + // SV clock bias + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.sv_clk(i, 0); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // SV clock drift + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.sv_clk(i, 1); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + + // tropo bias + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.tropo_bias(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // iono bias + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.iono_bias(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + // code bias + for (int i = 0; i < N_ch; i++) + { + tmp_double = rtk_data.code_bias(i); + dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + } + catch (const std::ofstream::failure &e) + { + LOG(WARNING) << "Exception writing VTL dump file " << e.what(); + } +} diff --git a/src/algorithms/PVT/libs/vtl_core.h b/src/algorithms/PVT/libs/vtl_core.h new file mode 100755 index 000000000..850228ec0 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_core.h @@ -0,0 +1,213 @@ +/*! + * \file vtl_core.h + * \brief Class that implements a Vector Tracking Loop (VTL) + * \author Pedro Pereira, 2025. pereirapedrocp@gmail.com + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_VTL_CORE_H +#define GNSS_SDR_VTL_CORE_H + +#include "MATH_CONSTANTS.h" +#include "pvt_conf.h" +#include "rtklib.h" +#include "trackingcmd.h" +#include "vtl_data.h" +#include +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Vtl_Core +{ +public: + /** + * Vector Tracking Loop (VTL) class constructor. + * @param conf PVT configuration for VTL. + */ + explicit Vtl_Core(const Pvt_Conf &conf); + ~Vtl_Core(); + + /** + * Initializes VTL. + * @param conf PVT configuration for VTL. + */ + void vtl_init(const Pvt_Conf &conf); + + /** + * Processes a new epoch of GNSS data. + * @param rtklib_data Input data for the VTL. + * @return True if successful, false otherwise. + */ + bool vtl_work(const Vtl_Data &rtklib_data); + + // Accessors + /** + * Accessor for VTL tracking commands. + * @return Vector of tracking commands. + */ + const std::vector &get_trk_cmd_outs() const; // acessor for tracking comands + /** + * Accessor for VTL receiver PVT. + * @return Receiver paraters estimation. + */ + const arma::colvec &get_rx_pvt() const; // acessor for receiver PVT + + +private: + // VTL configuration + bool vtl_kinematic; + double vtl_init_pos_ecef_var_m2; // initial variance of receiver pos (m^2) + double vtl_init_vel_ecef_var_m2s2; // initial variance of receiver vel ((m/s)^2) + double vtl_init_clk_b_var_m2; // initial variance of receiver clock bias (m^2) + double vtl_init_clk_d_var_m2s2; // initial variance of receiver clock drift ((m/s)^2) + double vtl_sys_acc_noise_var_m2s4; // Receiver acceleration noise PSD + double vtl_meas_prange_var_m2; // nominal variance of pseudorange (m^2) + double vtl_meas_prange_rate_var_m2s2; // nominal variance of pseudorange rate ((m/s)^2) + // Process noise covariance - Clock noise spectral densities + double vtl_sys_clk_b_noise_var_m2; // additive clock bias noise + double vtl_sys_clk_d_noise_var_m2s2; // additive clock drift noise + static constexpr double h_0 = 2e-16; // RWFM noise PSD + static constexpr double h_m2 = 5e-20; // WFM noise PSD + static constexpr double S_f = 0.5 * h_0 * SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S; + static constexpr double S_g = (2.0 * M_PI * M_PI / 3.0) * h_m2 * SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S; + + static constexpr u_int16_t num_recovery_epochs = 5; + static constexpr double default_vtl_dt = 0.02; + + // tracking command from VTL to tracking channels + std::vector trk_cmd_outs; + + // per channel + arma::colvec rho; // geometric range + arma::colvec rho_squared; // geometric range + arma::colvec u_x; // LOS unit vector (x axis) + arma::colvec u_y; // LOS unit vector (y axis) + arma::colvec u_z; // LOS unit vector (z axis) + + // EKF + arma::colvec ekf_X; // state + arma::mat ekf_F; // state transition + arma::mat ekf_F_t; // transpose state transition + arma::mat ekf_S; // residual covariance + arma::mat ekf_K; // gain + arma::mat ekf_J; // jacobian + arma::mat ekf_P; // state estimate covariance + arma::mat ekf_P_J_t; // P * J.t() + arma::mat ekf_P_aux; // state estimate covariance auxiliar + arma::mat ekf_eye; // eye matrix + arma::mat ekf_R; // measurement noise covariance + arma::mat ekf_Q; // process noise + arma::colvec ekf_X_updt; // state update + arma::colvec ekf_obs_Z; // observed measurements + arma::colvec ekf_comp_Z; // computed measurements + arma::colvec ekf_updt_comp_Z; // updated computed measurment + arma::colvec ekf_updt_comp_Z_copy; + arma::colvec ekf_updt_comp_Z_last; // last updated computed measurments + arma::colvec ekf_preFit; // measurements - observed minus computed (omc) + arma::colvec ekf_postFit; // measurements - updated omc + + const u_int8_t N_gps_ch; // number of channels GPS + const u_int8_t N_gal_ch; // number of channels GAL + bool dump_enabled; // dump + std::string dump_filename; + const u_int8_t N_ch; // number of channels + const u_int8_t N_st; // number of states + const u_int8_t N_meas; // number of measurements + u_int64_t epoch; // vtl epoch + const u_int32_t N_meascov_updt = 1000; // recorded prefit epochs + u_int32_t N_meascov_updt_cnt; + std::ofstream dump_file; + const arma::uvec N_st_idx; + + const u_int8_t i_px = 0; // position X + const u_int8_t i_vx = 1; // velocity X + const u_int8_t i_py = 2; // position Y + const u_int8_t i_vy = 3; // velocity Y + const u_int8_t i_pz = 4; // position Z + const u_int8_t i_vz = 5; // velocity Z + const u_int8_t i_clkb_G = 6; // clk bias GPS + const u_int8_t i_clkb_E; // clk bias GAL + const u_int8_t i_clkd; // clk drift + + arma::colvec vtl_feedback; // VTL feedback indicator + + u_int8_t epochs_to_recover; // VTL recovery from PVT fail + + double dt_s; + + // matrix indexants + arma::uvec aidx; + arma::uvec ext_aidx; + + + /** + * Reset VTL state. + * @param rtk_data Input data for the VTL. + */ + void vtl_reset(const Vtl_Data &rtk_data); + + /** + * Update of EKF Transition matrix. + * @param dt_s VTL epoch interval. + */ + void update_process(double dt_s); + + /** + * Update of EKF Jacobian matrix. + * @param rtk_data Input data for the VTL. + */ + void update_jacobian(const Vtl_Data &rtk_data); + + /** + * Compute EKF prefit. + * @param rtk_data Input data for the VTL. + */ + void compute_prefit(const Vtl_Data &rtk_data); + + /** + * Update of EKF Measurement Covariance matrix. + * @param rtk_data Input data for the VTL. + * @param meas_idx Indexant for measurements. + * @param preFit_available LOS unit vector available for satellite selection. + */ + void update_meas_cov(const Vtl_Data &rtk_data, const arma::uvec &meas_idx, bool preFit_available); + + /** + * Send VTL feedback to tracking channels. + * @param rtk_data Input data for the VTL. + */ + void send_vtl_feedback(const Vtl_Data &rtk_data); + + /** + * Satellite selection using fast weighted downdate method. + * @param w Per-satellite weights. + * @return Indexes of remained satellites. + */ + const arma::vec ss_downdate_fast(const arma::vec &w); + + /** + * Save VTL and RTKLib variables in file. + * @param rtk_data Input data for the VTL. + */ + void saveVTLdata(const Vtl_Data &rtk_data); +}; + +/** \} */ +/** \} */ +#endif // GNSS_SDR_VTL_CORE_H \ No newline at end of file diff --git a/src/algorithms/libs/trackingcmd.h b/src/algorithms/libs/trackingcmd.h index cf56ed5f8..c4cc342c4 100644 --- a/src/algorithms/libs/trackingcmd.h +++ b/src/algorithms/libs/trackingcmd.h @@ -28,14 +28,15 @@ class TrackingCmd { public: - TrackingCmd(); - - bool enable_carrier_nco_cmd = false; - bool enable_code_nco_cmd = false; - double code_freq_chips = 0.0; - double carrier_freq_hz = 0.0; + bool enable_pll_vtl_feedack = false; + bool enable_dll_vtl_feedack = false; + bool enable_smooth_pr = false; + double pll_vtl_freq_hz = 0.0; + double dll_vtl_freq_hz = 0.0; + double ch_sample_counter = 0.0; double carrier_freq_rate_hz_s = 0.0; - uint64_t sample_counter = 0UL; + uint32_t channel_id = 0; + uint32_t prn_id = 0; }; /** \} */