mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-09 12:16:46 +00:00
Code Issues Releases Wiki Activity

First working version of a complete 5 state Kalman filter for both carrier and code tracking, tested with GPS L1 CA 1ms integration.

Browse Source
This commit is contained in:
Javier Arribas 2020-09-21 21:34:39 +02:00
parent 845385861d
commit 1c09f6b8a5
17 changed files with 3078 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

227
conf/gnss-sdr_labsat_kf_vtl.conf Normal file
View File

@ -0,0 +1,227 @@
; This is a GNSS-SDR configuration file
; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/
; 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=5456000
;GNSS-SDR.internal_fs_sps=16368000
GNSS-SDR.use_acquisition_resampler=true
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=Labsat_Signal_Source
SignalSource.selected_channel=1
;#filename: path to file with the captured GNSS signal samples to be processed
;# Labsat sile source automatically increments the file name when the signal is split in several files
;# the adapter adds "_0000.LS3" to this base path and filename. Next file will be "_0001.LS3" and so on
;# in this example, the first file complete path will be ../signals/GPS_025_
;SignalSource.filename=/media/javier/WDNASNTFS/satgen_30mins/output/output
SignalSource.filename=/home/javier/signals/satgen_30mins/output/output
;SignalSource.filename=/home/javier/signals/dupli/dupli/dupli
SignalSource.item_type=gr_complex
SignalSource.sampling_frequency=16368000
SignalSource.samples=0
SignalSource.repeat=false
SignalSource.dump=false
SignalSource.dump_filename=./out.dat
SignalSource.enable_throttle_control=false
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Signal_Conditioner
;######### DATA_TYPE_ADAPTER CONFIG ############
DataTypeAdapter.implementation=Pass_Through
DataTypeAdapter.item_type=gr_complex
;######### INPUT_FILTER CONFIG ############
InputFilter.implementation=Freq_Xlating_Fir_Filter
InputFilter.dump=false
InputFilter.dump_filename=/media/javier/WDNASNTFS/output_5.456Msps_gr_complex.dat
InputFilter.input_item_type=gr_complex
InputFilter.output_item_type=gr_complex
InputFilter.taps_item_type=float
InputFilter.number_of_taps=5
InputFilter.number_of_bands=2
InputFilter.band1_begin=0.0
InputFilter.band1_end=0.45
InputFilter.band2_begin=0.55
InputFilter.band2_end=1.0
InputFilter.ampl1_begin=1.0
InputFilter.ampl1_end=1.0
InputFilter.ampl2_begin=0.0
InputFilter.ampl2_end=0.0
InputFilter.band1_error=1.0
InputFilter.band2_error=1.0
InputFilter.filter_type=lowpass
InputFilter.grid_density=16
InputFilter.sampling_frequency=16368000
InputFilter.IF=0
InputFilter.decimation_factor=3
;######### CHANNELS GLOBAL CONFIG ############
Channels_1C.count=6
Channels_1B.count=0
Channels_L5.count=0
Channels_5X.count=0
Channels.in_acquisition=1
;Channel0.satellite=3
;#signal:
;# "1C" GPS L1 C/A
;# "1B" GALILEO E1 B (I/NAV OS/CS/SoL)
;# "1G" GLONASS L1 C/A
;# "2S" GPS L2 L2C (M)
;# "5X" GALILEO E5a I+Q
;# "L5" GPS L5
;######### GPS ACQUISITION CONFIG ############
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition_1C.item_type=gr_complex
Acquisition_1C.threshold=3.0
Acquisition_1C.use_CFAR_algorithm=false
Acquisition_1C.blocking=true
Acquisition_1C.doppler_max=5000
Acquisition_1C.doppler_step=125
Acquisition_1C.dump=false
Acquisition_1C.dump_filename=./acq_dump.dat
;######### GALILEO ACQUISITION CONFIG ############
Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition
Acquisition_1B.item_type=gr_complex
Acquisition_1B.threshold=2.8
Acquisition_1B.use_CFAR_algorithm=false
Acquisition_1B.blocking=false
Acquisition_1B.doppler_max=5000
Acquisition_1B.doppler_step=125
Acquisition_1B.dump=false
Acquisition_1B.dump_filename=./acq_dump.dat
;######### TRACKING GPS CONFIG ############
;Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking
;Tracking_1C.item_type=gr_complex
;Tracking_1C.dump=false
;Tracking_1C.dump_filename=./tracking_ch_
;Tracking_1C.pll_bw_hz=35.0;
;Tracking_1C.dll_bw_hz=1.5;
;Tracking_1C.pll_bw_narrow_hz=2.5;
;Tracking_1C.dll_bw_narrow_hz=0.2;
;Tracking_1C.extend_correlation_symbols=1;
;Tracking_1C.dll_filter_order=2;
;Tracking_1C.pll_filter_order=3;
;Tracking_1C.early_late_space_chips=0.5;
;Tracking_1C.early_late_space_narrow_chips=0.15
;### KF tracking
Tracking_1C.implementation=GPS_L1_CA_KF_VTL_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.dump=true
Tracking_1C.dump_filename=./tracking_ch_
Tracking_1C.extend_correlation_symbols=1;
Tracking_1C.early_late_space_chips=0.5;
Tracking_1C.early_late_space_narrow_chips=0.15
Tracking_1C.expected_cn0_dbhz=45.0;
Tracking_1C.enable_dynamic_measurement_covariance=false;
Tracking_1C.use_estimated_cn0=false;
Tracking_1C.carrier_aiding=true;
Tracking_1C.code_phase_sd_chips=0.01;
Tracking_1C.code_rate_sd_chips_s=0.001;
Tracking_1C.carrier_phase_sd_rad=0.001;
Tracking_1C.carrier_freq_sd_hz=0.01;
Tracking_1C.carrier_freq_rate_sd_hz_s=0.1;
Tracking_1C.init_code_phase_sd_chips=1;
Tracking_1C.init_code_rate_sd_chips_s=10;
Tracking_1C.init_carrier_phase_sd_rad=1;
Tracking_1C.init_carrier_freq_sd_hz=10;
Tracking_1C.init_carrier_freq_rate_sd_hz_s=10;
;######### TRACKING GALILEO CONFIG ############
Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking
Tracking_1B.item_type=gr_complex
Tracking_1B.pll_bw_hz=15.0;
Tracking_1B.dll_bw_hz=0.75;
Tracking_1B.early_late_space_chips=0.15;
Tracking_1B.very_early_late_space_chips=0.5;
Tracking_1B.early_late_space_narrow_chips=0.10;
Tracking_1B.very_early_late_space_narrow_chips=0.5;
Tracking_1B.pll_bw_narrow_hz=2.5
Tracking_1B.dll_bw_narrow_hz=0.2
Tracking_1B.extend_correlation_symbols=5
Tracking_1B.track_pilot=true
Tracking_1B.enable_fll_pull_in=true;
;Tracking_1B.pull_in_time_s=60
Tracking_1B.enable_fll_steady_state=false
Tracking_1B.fll_bw_hz=10
Tracking_1B.dump=false
Tracking_1B.dump_filename=tracking_ch_
;######### TELEMETRY DECODER GALILEO CONFIG ############
TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder
TelemetryDecoder_1B.dump=false
;######### TELEMETRY DECODER GPS CONFIG ############
TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder
TelemetryDecoder_1C.dump=false
;######### OBSERVABLES CONFIG ############
;#implementation:
Observables.implementation=Hybrid_Observables
Observables.dump=false
Observables.dump_filename=./observables.dat
Observables.enable_carrier_smoothing=false
Observables.smoothing_factor=200
;######### PVT CONFIG ############
PVT.implementation=RTKLIB_PVT
PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic
PVT.enable_rx_clock_correction=false
PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX
PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad
PVT.output_rate_ms=1000;
PVT.rinexobs_rate_ms=1000;
PVT.display_rate_ms=1000;
PVT.elevation_mask=15;
PVT.flag_rtcm_server=false
PVT.flag_rtcm_tty_port=false
PVT.rtcm_dump_devname=/dev/pts/1
PVT.dump=false
PVT.dump_filename=./PVT
PVT.enable_monitor=false
PVT.monitor_udp_port=1337
PVT.monitor_client_addresses=127.0.0.1
;######### MONITOR CONFIG ############
Monitor.enable_monitor=false
Monitor.decimation_factor=1
Monitor.client_addresses=127.0.0.1
Monitor.udp_port=1234

10
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@ -52,6 +52,7 @@
#include "rinex_printer.h"
#include "rtcm_printer.h"
#include "rtklib_solver.h"
#include "trackingcmd.h"
#include <boost/any.hpp> // for any_cast, any
#include <boost/archive/xml_iarchive.hpp> // for xml_iarchive
#include <boost/archive/xml_oarchive.hpp> // for xml_oarchive
@ -126,6 +127,8 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
{
// Send feedback message to observables block with the receiver clock offset
this->message_port_register_out(pmt::mp("pvt_to_observables"));
// Experimental: VLT commands from PVT to tracking channels
this->message_port_register_out(pmt::mp("pvt_to_trk"));
// Send PVT status to gnss_flowgraph
this->message_port_register_out(pmt::mp("status"));
@ -2143,6 +2146,13 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
if (flag_pvt_valid == true)
{
//experimental VTL tests
// send tracking command
const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd());
trk_cmd_test->carrier_freq_hz = 12345.4;
trk_cmd_test->sample_counter = d_gnss_observables_map.begin()->second.Tracking_sample_counter;
this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
// initialize (if needed) the accumulated phase offset and apply it to the active channels
// required to report accumulated phase cycles comparable to pseudoranges
initialize_and_apply_carrier_phase_offset();

2
src/algorithms/libs/CMakeLists.txt
View File

@ -32,6 +32,7 @@ set(GNSS_SPLIBS_SOURCES
gnss_sdr_create_directory.cc
geofunctions.cc
item_type_helpers.cc
trackingcmd.cc
)
set(GNSS_SPLIBS_HEADERS
@ -59,6 +60,7 @@ set(GNSS_SPLIBS_HEADERS
gnss_circular_deque.h
geofunctions.h
item_type_helpers.h
trackingcmd.h
)
if(ENABLE_OPENCL)

13
src/algorithms/libs/trackingcmd.cc Normal file
View File

@ -0,0 +1,13 @@
/*
* trackingcmd.cc
*
* Created on: 20 ago. 2020
* Author: javier
*/
#include "trackingcmd.h"
TrackingCmd::TrackingCmd()
{
// TODO Auto-generated constructor stub
}

26
src/algorithms/libs/trackingcmd.h Normal file
View File

@ -0,0 +1,26 @@
/*
* trackingcmd.h
*
* Created on: 20 ago. 2020
* Author: javier
*/
#ifndef SRC_ALGORITHMS_LIBS_TRACKINGCMD_H_
#define SRC_ALGORITHMS_LIBS_TRACKINGCMD_H_
#include <cstdint>
class TrackingCmd
{
public:
TrackingCmd();
bool enable_carrier_nco_cmd;
bool enable_code_nco_cmd;
double code_freq_chips;
double carrier_freq_hz;
double carrier_freq_rate_hz_s;
uint64_t sample_counter;
};
#endif /* SRC_ALGORITHMS_LIBS_TRACKINGCMD_H_ */

4
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
View File

@ -142,8 +142,8 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block(
d_T_rx_TOW_set = false;
d_T_status_report_timer_ms = 0;
// rework
d_Rx_clock_buffer.set_capacity(10); // 10*20 ms = 200 ms of data in buffer
d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
d_Rx_clock_buffer.set_capacity(5); // 10*20 ms = 200 ms of data in buffer
d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
d_channel_last_pll_lock = std::vector<bool>(d_nchannels_out, false);
d_channel_last_pseudorange_smooth = std::vector<double>(d_nchannels_out, 0.0);

2
src/algorithms/tracking/adapters/CMakeLists.txt
View File

@ -53,6 +53,7 @@ set(TRACKING_ADAPTER_SOURCES
glonass_l2_ca_dll_pll_c_aid_tracking.cc
beidou_b1i_dll_pll_tracking.cc
beidou_b3i_dll_pll_tracking.cc
gps_l1_ca_kf_vtl_tracking.cc
${OPT_TRACKING_ADAPTERS_SOURCES}
)
@ -72,6 +73,7 @@ set(TRACKING_ADAPTER_HEADERS
glonass_l2_ca_dll_pll_c_aid_tracking.h
beidou_b1i_dll_pll_tracking.h
beidou_b3i_dll_pll_tracking.h
gps_l1_ca_kf_vtl_tracking.h
${OPT_TRACKING_ADAPTERS_HEADERS}
)

142
src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.cc Normal file
View File

@ -0,0 +1,142 @@
/*!
* \file gps_l1_ca_kf_vtl_tracking.h
* \brief Interface of an adapter of a code + carrier Kalman Filter tracking loop with VTL capabilities block
* for GPS L1 C/A to a TrackingInterface
* \author Javier Arribas, 2020. jarribas(at)cttc.es
*
*
*
* -----------------------------------------------------------------------------
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "gps_l1_ca_kf_vtl_tracking.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
#include "display.h"
#include "gnss_sdr_flags.h"
#include "kf_conf.h"
#include <glog/logging.h>
#include <array>
GpsL1CaKfVtlTracking::GpsL1CaKfVtlTracking(
const ConfigurationInterface* configuration, const std::string& role,
unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
{
Kf_Conf trk_params = Kf_Conf();
DLOG(INFO) << "role " << role;
trk_params.SetFromConfiguration(configuration, role);
const auto vector_length = static_cast<int>(std::round(trk_params.fs_in / (GPS_L1_CA_CODE_RATE_CPS / GPS_L1_CA_CODE_LENGTH_CHIPS)));
trk_params.vector_length = vector_length;
if (trk_params.extend_correlation_symbols < 1)
{
trk_params.extend_correlation_symbols = 1;
std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be bigger than 1. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << '\n';
}
else if (trk_params.extend_correlation_symbols > 20)
{
trk_params.extend_correlation_symbols = 20;
std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << '\n';
}
trk_params.track_pilot = configuration->property(role + ".track_pilot", false);
if (trk_params.track_pilot)
{
trk_params.track_pilot = false;
std::cout << TEXT_RED << "WARNING: GPS L1 C/A does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << '\n';
}
trk_params.system = 'G';
const std::array<char, 3> sig_{'1', 'C', '\0'};
std::memcpy(trk_params.signal, sig_.data(), 3);
// ################# Make a GNU Radio Tracking block object ################
if (trk_params.item_type == "gr_complex")
{
item_size_ = sizeof(gr_complex);
tracking_ = kf_vtl_make_tracking(trk_params);
}
else
{
item_size_ = sizeof(gr_complex);
LOG(WARNING) << trk_params.item_type << " unknown tracking item type.";
}
channel_ = 0;
DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
if (in_streams_ > 1)
{
LOG(ERROR) << "This implementation only supports one input stream";
}
if (out_streams_ > 1)
{
LOG(ERROR) << "This implementation only supports one output stream";
}
}
void GpsL1CaKfVtlTracking::stop_tracking()
{
tracking_->stop_tracking();
}
void GpsL1CaKfVtlTracking::start_tracking()
{
tracking_->start_tracking();
}
/*
* Set tracking channel unique ID
*/
void GpsL1CaKfVtlTracking::set_channel(unsigned int channel)
{
channel_ = channel;
tracking_->set_channel(channel);
}
void GpsL1CaKfVtlTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
tracking_->set_gnss_synchro(p_gnss_synchro);
}
void GpsL1CaKfVtlTracking::connect(gr::top_block_sptr top_block)
{
if (top_block)
{ /* top_block is not null */
};
// nothing to connect, now the tracking uses gr_sync_decimator
}
void GpsL1CaKfVtlTracking::disconnect(gr::top_block_sptr top_block)
{
if (top_block)
{ /* top_block is not null */
};
// nothing to disconnect, now the tracking uses gr_sync_decimator
}
gr::basic_block_sptr GpsL1CaKfVtlTracking::get_left_block()
{
return tracking_;
}
gr::basic_block_sptr GpsL1CaKfVtlTracking::get_right_block()
{
return tracking_;
}

93
src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.h Normal file
View File

@ -0,0 +1,93 @@
/*!
* \file gps_l1_ca_kf_vtl_tracking.h
* \brief Interface of an adapter of a code + carrier Kalman Filter tracking loop with VTL capabilities block
* for GPS L1 C/A to a TrackingInterface
* \author Javier Arribas, 2020. jarribas(at)cttc.es
*
*
* -----------------------------------------------------------------------------
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GPS_L1_CA_kf_vtl_TRACKING_H
#define GNSS_SDR_GPS_L1_CA_kf_vtl_TRACKING_H
#include "kf_vtl_tracking.h"
#include "tracking_interface.h"
#include <string>
class ConfigurationInterface;
/*!
* \brief This class implements a code + carrier Kalman Filter tracking loop with VTL capabilities
*/
class GpsL1CaKfVtlTracking : public TrackingInterface
{
public:
GpsL1CaKfVtlTracking(
const ConfigurationInterface* configuration,
const std::string& role,
unsigned int in_streams,
unsigned int out_streams);
~GpsL1CaKfVtlTracking() = default;
inline std::string role() override
{
return role_;
}
//! Returns "GPS_L1_CA_kf_vtl_Tracking"
inline std::string implementation() override
{
return "GPS_L1_CA_KF_VTL_Tracking";
}
inline size_t item_size() override
{
return item_size_;
}
void connect(gr::top_block_sptr top_block) override;
void disconnect(gr::top_block_sptr top_block) override;
gr::basic_block_sptr get_left_block() override;
gr::basic_block_sptr get_right_block() override;
/*!
* \brief Set tracking channel unique ID
*/
void set_channel(unsigned int channel) override;
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to efficiently exchange synchronization data between acquisition and tracking blocks
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override;
void start_tracking() override;
/*!
* \brief Stop running tracking
*/
void stop_tracking() override;
private:
kf_vtl_tracking_sptr tracking_;
size_t item_size_;
unsigned int channel_;
std::string role_;
unsigned int in_streams_;
unsigned int out_streams_;
};
#endif // GNSS_SDR_GPS_L1_CA_kf_vtl_TRACKING_H

2
src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt
View File

@ -41,6 +41,7 @@ set(TRACKING_GR_BLOCKS_SOURCES
glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc
glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc
dll_pll_veml_tracking.cc
kf_vtl_tracking.cc
${OPT_TRACKING_BLOCKS_SOURCES}
)
@ -55,6 +56,7 @@ set(TRACKING_GR_BLOCKS_HEADERS
glonass_l2_ca_dll_pll_c_aid_tracking_cc.h
glonass_l2_ca_dll_pll_c_aid_tracking_sc.h
dll_pll_veml_tracking.h
kf_vtl_tracking.h
${OPT_TRACKING_BLOCKS_HEADERS}
)

2056
src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc Normal file
View File

File diff suppressed because it is too large Load Diff

236
src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.h Normal file
View File

@ -0,0 +1,236 @@
/*!
* \file kf_vtl_tracking.cc
* \brief Implementation of a Kalman filter based tracking with optional Vector Tracking Loop message receiver block.
* \author Javier Arribas, 2020. jarribas(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_kf_vtl_TRACKING_H
#define GNSS_SDR_kf_vtl_TRACKING_H
#include "cpu_multicorrelator_real_codes.h"
#include "exponential_smoother.h"
#include "kf_conf.h"
#include <boost/circular_buffer.hpp>
#include <gnuradio/block.h> // for block
#include <gnuradio/gr_complex.h> // for gr_complex
#include <gnuradio/types.h> // for gr_vector_int, gr_vector...
#include <pmt/pmt.h> // for pmt_t
#include <volk_gnsssdr/volk_gnsssdr_alloc.h> // for volk_gnsssdr::vector
#include <cstddef> // for size_t
#include <cstdint> // for int32_t
#include <fstream> // for ofstream
#include <string> // for string
#include <typeinfo> // for typeid
#include <utility> // for pair
#if GNURADIO_USES_STD_POINTERS
#include <memory>
#else
#include <boost/shared_ptr.hpp>
#endif
#include <armadillo>
class Gnss_Synchro;
class kf_vtl_tracking;
#if GNURADIO_USES_STD_POINTERS
using kf_vtl_tracking_sptr = std::shared_ptr<kf_vtl_tracking>;
#else
using kf_vtl_tracking_sptr = boost::shared_ptr<kf_vtl_tracking>;
#endif
kf_vtl_tracking_sptr kf_vtl_make_tracking(const Kf_Conf &conf_);
/*!
* \brief This class implements a code DLL + carrier PLL tracking block.
*/
class kf_vtl_tracking : public gr::block
{
public:
~kf_vtl_tracking();
void set_channel(uint32_t channel);
void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro);
void start_tracking();
void stop_tracking();
int general_work(int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast(int noutput_items, gr_vector_int &ninput_items_required);
private:
friend kf_vtl_tracking_sptr kf_vtl_make_tracking(const Kf_Conf &conf_);
explicit kf_vtl_tracking(const Kf_Conf &conf_);
void init_kf(double acq_code_phase_chips, double acq_doppler_hz);
void msg_handler_telemetry_to_trk(const pmt::pmt_t &msg);
void msg_handler_pvt_to_trk(const pmt::pmt_t &msg);
void do_correlation_step(const gr_complex *input_samples);
void run_Kf();
void check_carrier_phase_coherent_initialization();
void update_tracking_vars();
void clear_tracking_vars();
void save_correlation_results();
void log_data();
bool cn0_and_tracking_lock_status(double coh_integration_time_s);
bool acquire_secondary();
int32_t save_matfile() const;
Cpu_Multicorrelator_Real_Codes d_multicorrelator_cpu;
Cpu_Multicorrelator_Real_Codes d_correlator_data_cpu; // for data channel
Kf_Conf d_trk_parameters;
Exponential_Smoother d_cn0_smoother;
Exponential_Smoother d_carrier_lock_test_smoother;
Gnss_Synchro *d_acquisition_gnss_synchro;
volk_gnsssdr::vector<float> d_tracking_code;
volk_gnsssdr::vector<float> d_data_code;
volk_gnsssdr::vector<float> d_local_code_shift_chips;
volk_gnsssdr::vector<gr_complex> d_correlator_outs;
volk_gnsssdr::vector<gr_complex> d_Prompt_Data;
volk_gnsssdr::vector<gr_complex> d_Prompt_buffer;
boost::circular_buffer<std::pair<double, double>> d_code_ph_history;
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
const size_t int_type_hash_code = typeid(int).hash_code();
//Kalman Filter class variables
arma::mat F;
arma::mat H;
arma::mat R;
arma::mat Q;
arma::mat P_old_old;
arma::mat P_new_old;
arma::mat P_new_new;
arma::vec x_old_old;
arma::vec x_new_old;
arma::vec x_new_new;
//nominal signal parameters
double d_signal_carrier_freq;
double d_code_period;
double d_code_chip_rate;
//acquisition
double d_acq_code_phase_samples;
double d_acq_carrier_doppler_hz;
double d_current_correlation_time_s;
//carrier and code discriminators output
double d_carr_phase_error_disc_hz;
double d_carr_freq_error_disc_hz;
double d_code_error_disc_chips;
//estimated parameters
//code
double d_code_error_kf_chips;
double d_code_freq_kf_chips_s;
//carrier
double d_carrier_phase_kf_rad;
double d_carrier_doppler_kf_hz;
double d_carrier_doppler_rate_kf_hz_s;
double d_acc_carrier_phase_rad;
double d_T_chip_seconds;
double d_T_prn_seconds;
double d_T_prn_samples;
double d_K_blk_samples;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;
//carrier NCO
double d_carrier_phase_step_rad;
double d_carrier_phase_rate_step_rad;
//code NCO
double d_code_phase_step_chips;
double d_code_phase_rate_step_chips;
double d_rem_code_phase_chips;
double d_rem_code_phase_samples;
gr_complex *d_Very_Early;
gr_complex *d_Early;
gr_complex *d_Prompt;
gr_complex *d_Late;
gr_complex *d_Very_Late;
gr_complex d_VE_accu;
gr_complex d_E_accu;
gr_complex d_P_accu;
gr_complex d_P_accu_old;
gr_complex d_L_accu;
gr_complex d_VL_accu;
gr_complex d_P_data_accu;
std::string d_secondary_code_string;
std::string d_data_secondary_code_string;
std::string d_systemName;
std::string d_signal_type;
std::string d_signal_pretty_name;
std::string d_dump_filename;
std::ofstream d_dump_file;
uint64_t d_sample_counter;
uint64_t d_acq_sample_stamp;
float *d_prompt_data_shift;
float d_rem_carr_phase_rad;
int32_t d_symbols_per_bit;
int32_t d_preamble_length_symbols;
int32_t d_state;
int32_t d_correlation_length_ms;
int32_t d_n_correlator_taps;
int32_t d_current_prn_length_samples;
int32_t d_extend_correlation_symbols_count;
int32_t d_current_symbol;
int32_t d_current_data_symbol;
int32_t d_cn0_estimation_counter;
int32_t d_carrier_lock_fail_counter;
int32_t d_code_lock_fail_counter;
int32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled)
int32_t d_code_length_chips;
uint32_t d_channel;
uint32_t d_secondary_code_length;
uint32_t d_data_secondary_code_length;
bool d_pull_in_transitory;
bool d_corrected_doppler;
bool d_interchange_iq;
bool d_veml;
bool d_cloop;
bool d_secondary;
bool d_dump;
bool d_dump_mat;
bool d_acc_carrier_phase_initialized;
bool d_enable_extended_integration;
};
#endif // GNSS_SDR_kf_vtl_TRACKING_H

2
src/algorithms/tracking/libs/CMakeLists.txt
View File

@ -21,6 +21,7 @@ set(TRACKING_LIB_SOURCES
tracking_FLL_PLL_filter.cc
tracking_loop_filter.cc
dll_pll_conf.cc
kf_conf.cc
bayesian_estimation.cc
exponential_smoother.cc
)
@ -38,6 +39,7 @@ set(TRACKING_LIB_HEADERS
tracking_FLL_PLL_filter.h
tracking_loop_filter.h
dll_pll_conf.h
kf_conf.h
bayesian_estimation.h
exponential_smoother.h
)

151
src/algorithms/tracking/libs/kf_conf.cc Normal file
View File

@ -0,0 +1,151 @@
/*!
* \file Kf_conf.cc
* \brief Class that contains all the configuration parameters for generic
* tracking block based on a DLL and a PLL.
* \author Javier Arribas, 2018. jarribas(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "kf_conf.h"
#include "gnss_sdr_flags.h"
#include "item_type_helpers.h"
#include <glog/logging.h>
Kf_Conf::Kf_Conf()
{
/*KF tracking configuration */
high_dyn = false;
smoother_length = 10;
fs_in = 2000000.0;
vector_length = 0U;
dump = false;
dump_mat = true;
dump_filename = std::string("./Kf_dump.dat");
pull_in_time_s = 10;
bit_synchronization_time_limit_s = pull_in_time_s + 60;
early_late_space_chips = 0.25;
very_early_late_space_chips = 0.5;
early_late_space_narrow_chips = 0.15;
very_early_late_space_narrow_chips = 0.5;
slope = 1.0;
spc = 0.5;
y_intercept = 1.0;
carrier_aiding = true;
extend_correlation_symbols = 1;
cn0_samples = FLAGS_cn0_samples;
cn0_smoother_samples = 200;
cn0_smoother_alpha = 0.002;
carrier_lock_test_smoother_alpha = 0.002;
carrier_lock_test_smoother_samples = 25;
cn0_min = FLAGS_cn0_min;
max_carrier_lock_fail = FLAGS_max_carrier_lock_fail;
max_code_lock_fail = FLAGS_max_lock_fail;
carrier_lock_th = FLAGS_carrier_lock_th;
track_pilot = true;
enable_doppler_correction = false;
system = 'G';
signal[0] = '1';
signal[1] = 'C';
signal[2] = '\0';
item_type = "gr_complex";
expected_cn0_dbhz = 0;
//System covariances (Q)
code_phase_sd_chips = 0;
code_rate_sd_chips_s = 0;
carrier_phase_sd_rad = 0;
carrier_freq_sd_hz = 0;
carrier_freq_rate_sd_hz_s = 0;
//initial Kalman covariance matrix (P)
init_code_phase_sd_chips = 0;
init_code_rate_sd_chips_s = 0;
init_carrier_phase_sd_rad = 0;
init_carrier_freq_sd_hz = 0;
init_carrier_freq_rate_sd_hz_s = 0;
enable_dynamic_measurement_covariance = false;
use_estimated_cn0 = false;
}
void Kf_Conf::SetFromConfiguration(const ConfigurationInterface *configuration,
const std::string &role)
{
item_type = configuration->property(role + ".item_type", item_type);
if (!item_type_valid(item_type))
{
LOG(WARNING) << "Unknown item type: " + item_type << ". Set to gr_complex";
item_type = "gr_complex";
}
double fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", fs_in);
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
high_dyn = configuration->property(role + ".high_dyn", high_dyn);
dump = configuration->property(role + ".dump", dump);
dump_filename = configuration->property(role + ".dump_filename", dump_filename);
dump_mat = configuration->property(role + ".dump_mat", dump_mat);
pull_in_time_s = configuration->property(role + ".pull_in_time_s", pull_in_time_s);
bit_synchronization_time_limit_s = pull_in_time_s + 60;
early_late_space_chips = configuration->property(role + ".early_late_space_chips", early_late_space_chips);
early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", early_late_space_narrow_chips);
very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", very_early_late_space_chips);
very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", very_early_late_space_narrow_chips);
extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", extend_correlation_symbols);
track_pilot = configuration->property(role + ".track_pilot", track_pilot);
cn0_samples = configuration->property(role + ".cn0_samples", cn0_samples);
cn0_min = configuration->property(role + ".cn0_min", cn0_min);
max_code_lock_fail = configuration->property(role + ".max_lock_fail", max_code_lock_fail);
max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", max_carrier_lock_fail);
carrier_lock_th = configuration->property(role + ".carrier_lock_th", carrier_lock_th);
carrier_aiding = configuration->property(role + ".carrier_aiding", carrier_aiding);
// tracking lock tests smoother parameters
cn0_smoother_samples = configuration->property(role + ".cn0_smoother_samples", cn0_smoother_samples);
cn0_smoother_alpha = configuration->property(role + ".cn0_smoother_alpha", cn0_smoother_alpha);
smoother_length = configuration->property(role + ".smoother_length", smoother_length);
if (smoother_length < 1)
{
smoother_length = 1;
LOG(WARNING) << "smoother_length must be bigger than 0. It has been set to 1";
}
carrier_lock_test_smoother_samples = configuration->property(role + ".carrier_lock_test_smoother_samples", carrier_lock_test_smoother_samples);
carrier_lock_test_smoother_alpha = configuration->property(role + ".carrier_lock_test_smoother_alpha", carrier_lock_test_smoother_alpha);
// Kalman filter covariances
//Measurement covariances (R)
expected_cn0_dbhz = configuration->property(role + ".expected_cn0_dbhz", 42.0);
enable_dynamic_measurement_covariance = configuration->property(role + ".enable_dynamic_measurement_covariance", false);
use_estimated_cn0 = configuration->property(role + ".use_estimated_cn0", false);
//System covariances (Q)
code_phase_sd_chips = configuration->property(role + ".code_phase_sd_chips", 0.001);
code_rate_sd_chips_s = configuration->property(role + ".code_rate_sd_chips_s", 0.001);
carrier_phase_sd_rad = configuration->property(role + ".carrier_phase_sd_rad", 0.001);
carrier_freq_sd_hz = configuration->property(role + ".carrier_freq_sd_hz", 0.1);
carrier_freq_rate_sd_hz_s = configuration->property(role + ".carrier_freq_rate_sd_hz_s", 1);
//initial Kalman covariance matrix (P)
init_code_phase_sd_chips = configuration->property(role + ".init_code_phase_sd_chips", 1);
init_code_rate_sd_chips_s = configuration->property(role + ".init_code_rate_sd_chips_s", 100);
init_carrier_phase_sd_rad = configuration->property(role + ".init_carrier_phase_sd_rad", 10);
init_carrier_freq_sd_hz = configuration->property(role + ".init_carrier_freq_sd_hz", 1000);
init_carrier_freq_rate_sd_hz_s = configuration->property(role + ".init_carrier_freq_rate_sd_hz_s", 1000);
}

91
src/algorithms/tracking/libs/kf_conf.h Normal file
View File

@ -0,0 +1,91 @@
/*!
* \file Kf_conf.h
* \brief Class that contains all the configuration parameters for generic tracking block based on a Kalman Filter.
* \author Javier Arribas, 2020. jarribas(at)cttc.es
*
* Class that contains all the configuration parameters for generic tracking block based on a DLL and a PLL.
*
* -----------------------------------------------------------------------------
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_Kf_CONF_H
#define GNSS_SDR_Kf_CONF_H
#include "configuration_interface.h"
#include <cstdint>
#include <string>
class Kf_Conf
{
public:
Kf_Conf();
void SetFromConfiguration(const ConfigurationInterface *configuration, const std::string &role);
std::string item_type;
std::string dump_filename;
double fs_in;
double carrier_lock_th;
float early_late_space_chips;
float very_early_late_space_chips;
float early_late_space_narrow_chips;
float very_early_late_space_narrow_chips;
float slope;
float spc;
float y_intercept;
float cn0_smoother_alpha;
float carrier_lock_test_smoother_alpha;
uint32_t pull_in_time_s;
uint32_t bit_synchronization_time_limit_s;
uint32_t vector_length;
uint32_t smoother_length;
int32_t extend_correlation_symbols;
int32_t cn0_samples;
int32_t cn0_smoother_samples;
int32_t carrier_lock_test_smoother_samples;
int32_t cn0_min;
int32_t max_code_lock_fail;
int32_t max_carrier_lock_fail;
char signal[3]{};
char system;
bool track_pilot;
bool enable_doppler_correction;
bool carrier_aiding;
bool high_dyn;
bool dump;
bool dump_mat;
//KF statistics
//states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s, code_freq_rate_chips_s
//Measurement covariances (R)
double expected_cn0_dbhz;
//System covariances (Q)
double code_phase_sd_chips;
double code_rate_sd_chips_s;
double carrier_phase_sd_rad;
double carrier_freq_sd_hz;
double carrier_freq_rate_sd_hz_s;
//initial Kalman covariance matrix (P)
double init_code_phase_sd_chips;
double init_code_rate_sd_chips_s;
double init_carrier_phase_sd_rad;
double init_carrier_freq_sd_hz;
double init_carrier_freq_rate_sd_hz_s;
bool enable_dynamic_measurement_covariance;
bool use_estimated_cn0;
};
#endif

13
src/core/receiver/gnss_block_factory.cc
View File

@ -70,6 +70,7 @@
#include "gnss_sdr_make_unique.h"
#include "gps_l1_ca_dll_pll_tracking.h"
#include "gps_l1_ca_kf_tracking.h"
#include "gps_l1_ca_kf_vtl_tracking.h"
#include "gps_l1_ca_pcps_acquisition.h"
#include "gps_l1_ca_pcps_acquisition_fine_doppler.h"
#include "gps_l1_ca_pcps_assisted_acquisition.h"
@ -1080,6 +1081,12 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
out_streams);
block = std::move(block_);
}
else if (implementation == "GPS_L1_CA_KF_VTL_Tracking")
{
std::unique_ptr<GNSSBlockInterface> block_ = std::make_unique<GpsL1CaKfVtlTracking>(configuration, role, in_streams,
out_streams);
block = std::move(block_);
}
else if (implementation == "GPS_L1_CA_TCP_CONNECTOR_Tracking")
{
std::unique_ptr<GNSSBlockInterface> block_ = std::make_unique<GpsL1CaTcpConnectorTracking>(configuration, role, in_streams,
@ -1505,6 +1512,12 @@ std::unique_ptr<TrackingInterface> GNSSBlockFactory::GetTrkBlock(
out_streams);
block = std::move(block_);
}
else if (implementation == "GPS_L1_CA_KF_VTL_Tracking")
{
std::unique_ptr<TrackingInterface> block_ = std::make_unique<GpsL1CaKfVtlTracking>(configuration, role, in_streams,
out_streams);
block = std::move(block_);
}
else if (implementation == "GPS_L1_CA_TCP_CONNECTOR_Tracking")
{
std::unique_ptr<TrackingInterface> block_ = std::make_unique<GpsL1CaTcpConnectorTracking>(configuration, role, in_streams,

10
src/core/receiver/gnss_flowgraph.cc
View File

@ -674,6 +674,16 @@ void GNSSFlowgraph::connect()
{
top_block_->connect(observables_->get_right_block(), i, pvt_->get_left_block(), i);
top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("telemetry"), pvt_->get_left_block(), pmt::mp("telemetry"));
//experimental VTL
//TODO: It is currently implemented only in dll_pll_veml_tracking, other configs will fail!
try
{
top_block_->msg_connect(pvt_->get_left_block(), pmt::mp("pvt_to_trk"), channels_.at(i)->get_left_block_trk(), pmt::mp("pvt_to_trk"));
}
catch (std::exception& ex)
{
LOG(WARNING) << "pvt_to_trk message not implemented in " << channels_.at(i)->implementation();
}
}
top_block_->msg_connect(observables_->get_right_block(), pmt::mp("status"), channels_status_, pmt::mp("status"));