mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-10 14:56:03 +00:00
Code Issues Releases Wiki Activity

chore: rebasing vector tracking branch with next branch updates

Browse Source
This commit is contained in:
pedromiguelcp
2025-07-02 16:33:11 +01:00
committed by Carles Fernandez
parent bf63c3e808
commit ef4f3a2a25
7 changed files with 1226 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

262
conf/File_input/MultiCons/gnss-sdr_GPS_Galileo_VTL.conf Executable file
View File

@@ -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

1
src/algorithms/PVT/libs/CMakeLists.txt
View File

@@ -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

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

@@ -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>();
vtl_data->init_storage(d_conf.vtl_gps_channels + d_conf.vtl_gal_channels);
vtl_Core = std::make_unique<Vtl_Core>(d_conf);
vtl_Core->vtl_init(d_conf);
}
}

2
src/algorithms/PVT/libs/rtklib_solver.h Executable file → Normal file
View File

@@ -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 <array>
#include <cstdint>
@@ -162,6 +163,7 @@ private:
// vector tracking
std::unique_ptr<Vtl_Data> vtl_data;
std::unique_ptr<Vtl_Core> vtl_Core;
};

736
src/algorithms/PVT/libs/vtl_core.cc Executable file
View File

@@ -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 <fstream>
#if USE_GLOG_AND_GFLAGS
#include <glog/logging.h>
#else
#include <absl/log/log.h>
#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<arma::uvec>(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<TrackingCmd> &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<arma::uvec>(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<char *>(&tmp_uint32), sizeof(uint32_t));
// rx time
tmp_double = rtk_data.rx_time;
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// dt
tmp_double = rtk_data.dt_s;
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// ekf rx p
tmp_double = ekf_X(i_px);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = ekf_X(i_py);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = ekf_X(i_pz);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// ekf rx v
tmp_double = ekf_X(i_vx);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = ekf_X(i_vy);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = ekf_X(i_vz);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// ekf rx b gps
tmp_double = ekf_X(i_clkb_G);
dump_file.write(reinterpret_cast<char *>(&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<char *>(&tmp_double), sizeof(double));
}
// ekf rx d
tmp_double = ekf_X(i_clkd);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// rtk rx p
tmp_double = rtk_data.rx_p(0);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = rtk_data.rx_p(1);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = rtk_data.rx_p(2);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// rtk rx v
tmp_double = rtk_data.rx_v(0);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = rtk_data.rx_v(1);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = rtk_data.rx_v(2);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// rtk rx b gps
tmp_double = rtk_data.rx_clk(0);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// rtk rx b gal
tmp_double = rtk_data.rx_clk(1);
dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// rtk rx d
tmp_double = rtk_data.rx_clk(2);
dump_file.write(reinterpret_cast<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&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<char *>(&tmp_double), sizeof(double));
}
}
catch (const std::ofstream::failure &e)
{
LOG(WARNING) << "Exception writing VTL dump file " << e.what();
}
}

213
src/algorithms/PVT/libs/vtl_core.h Executable file
View File

@@ -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 <armadillo>
#include <cstdint>
#include <string>
#include <vector>
/** \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<TrackingCmd> &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<TrackingCmd> 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

15
src/algorithms/libs/trackingcmd.h
View File

@@ -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;
};
/** \} */