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

conf/gnss-sdr_labsat_kf_vtl.conf

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

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc

@@ -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();

src/algorithms/libs/CMakeLists.txt

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

src/algorithms/libs/trackingcmd.cc

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

src/algorithms/libs/trackingcmd.h

@@ -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_ */

src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc

@@ -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.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_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);

src/algorithms/tracking/adapters/CMakeLists.txt

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

src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.cc

@@ -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_;
+}

src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.h

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

src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt

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

src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.h

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

src/algorithms/tracking/libs/CMakeLists.txt

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

src/algorithms/tracking/libs/kf_conf.cc

@@ -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);
+}

src/algorithms/tracking/libs/kf_conf.h

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

src/core/receiver/gnss_block_factory.cc

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

src/core/receiver/gnss_flowgraph.cc

@@ -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"));