From 70a2690a2ae64662191228081a272939614165ac Mon Sep 17 00:00:00 2001 From: Javier Date: Tue, 13 Mar 2018 11:51:33 +0200 Subject: [PATCH 01/23] Adding experimental KF carrier tracking --- conf/gnss-sdr_GPS_L1_nsr_kf.conf | 211 ++++++ .../tracking/adapters/CMakeLists.txt | 1 + .../adapters/gps_l1_ca_kf_tracking.cc | 150 +++++ .../tracking/adapters/gps_l1_ca_kf_tracking.h | 104 +++ .../tracking/gnuradio_blocks/CMakeLists.txt | 1 + .../gps_l1_ca_kf_tracking_cc.cc | 619 ++++++++++++++++++ .../gps_l1_ca_kf_tracking_cc.h | 187 ++++++ src/core/receiver/gnss_block_factory.cc | 13 + 8 files changed, 1286 insertions(+) create mode 100644 conf/gnss-sdr_GPS_L1_nsr_kf.conf create mode 100644 src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc create mode 100644 src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h create mode 100644 src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc create mode 100644 src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h diff --git a/conf/gnss-sdr_GPS_L1_nsr_kf.conf b/conf/gnss-sdr_GPS_L1_nsr_kf.conf new file mode 100644 index 000000000..d2a84cd9f --- /dev/null +++ b/conf/gnss-sdr_GPS_L1_nsr_kf.conf @@ -0,0 +1,211 @@ +; Default configuration file +; You can define your own receiver and invoke it by doing +; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf +; + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second]. +;GNSS-SDR.internal_fs_sps=6826700 +GNSS-SDR.internal_fs_sps=2560000 +;GNSS-SDR.internal_fs_sps=4096000 +;GNSS-SDR.internal_fs_sps=5120000 + +;######### SIGNAL_SOURCE CONFIG ############ +;#implementation: Use [File_Signal_Source] [Nsr_File_Signal_Source] or [UHD_Signal_Source] or [GN3S_Signal_Source] (experimental) +SignalSource.implementation=Nsr_File_Signal_Source + +;#filename: path to file with the captured GNSS signal samples to be processed +SignalSource.filename=/home/javier/signals/ifen/E1L1_FE0_Band0.stream ; <- PUT YOUR FILE HERE + +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +SignalSource.item_type=byte + +;#sampling_frequency: Original Signal sampling frequency in [Hz] +SignalSource.sampling_frequency=20480000 + +;#freq: RF front-end center frequency in [Hz] +SignalSource.freq=1575420000 + +;#samples: Number of samples to be processed. Notice that 0 indicates the entire file. +SignalSource.samples=0 + +;#repeat: Repeat the processing file. Disable this option in this version +SignalSource.repeat=false + +;#dump: Dump the Signal source data to a file. Disable this option in this version +SignalSource.dump=false + +SignalSource.dump_filename=../data/signal_source.dat + + +;#enable_throttle_control: Enabling this option tells the signal source to keep the delay between samples in post processing. +; it helps to not overload the CPU, but the processing time will be longer. +SignalSource.enable_throttle_control=false + + +;######### SIGNAL_CONDITIONER CONFIG ############ +;## It holds blocks to change data type, filter and resample input data. + +;#implementation: Use [Pass_Through] or [Signal_Conditioner] +;#[Pass_Through] disables this block and the [DataTypeAdapter], [InputFilter] and [Resampler] blocks +;#[Signal_Conditioner] enables this block. Then you have to configure [DataTypeAdapter], [InputFilter] and [Resampler] blocks +SignalConditioner.implementation=Signal_Conditioner + +;######### DATA_TYPE_ADAPTER CONFIG ############ +;## Changes the type of input data. +;#implementation: [Pass_Through] disables this block +DataTypeAdapter.implementation=Pass_Through +DataTypeAdapter.item_type=float + +;######### INPUT_FILTER CONFIG ############ +;## Filter the input data. Can be combined with frequency translation for IF signals + +;#implementation: Use [Pass_Through] or [Fir_Filter] or [Freq_Xlating_Fir_Filter] +;#[Freq_Xlating_Fir_Filter] enables FIR filter and a composite frequency translation +;# that shifts IF down to zero Hz. + +InputFilter.implementation=Freq_Xlating_Fir_Filter + +;#dump: Dump the filtered data to a file. +InputFilter.dump=false + +;#dump_filename: Log path and filename. +InputFilter.dump_filename=../data/input_filter.dat + +;#The following options are used in the filter design of Fir_Filter and Freq_Xlating_Fir_Filter implementation. +;#These options are based on parameters of gnuradio's function: gr_remez. +;#These function calculates the optimal (in the Chebyshev/minimax sense) FIR filter inpulse +;#reponse given a set of band edges, the desired reponse on those bands, +;#and the weight given to the error in those bands. + +;#input_item_type: Type and resolution for input signal samples. Use only gr_complex in this version. +InputFilter.input_item_type=float + +;#outut_item_type: Type and resolution for output filtered signal samples. Use only gr_complex in this version. +InputFilter.output_item_type=gr_complex + +;#taps_item_type: Type and resolution for the taps of the filter. Use only float in this version. +InputFilter.taps_item_type=float + +;#number_of_taps: Number of taps in the filter. Increasing this parameter increases the processing time +InputFilter.number_of_taps=5 + +;#number_of _bands: Number of frequency bands in the filter. +InputFilter.number_of_bands=2 + +;#bands: frequency at the band edges [ b1 e1 b2 e2 b3 e3 ...]. +;#Frequency is in the range [0, 1], with 1 being the Nyquist frequency (Fs/2) +;#The number of band_begin and band_end elements must match the number of bands + +InputFilter.band1_begin=0.0 +InputFilter.band1_end=0.45 +InputFilter.band2_begin=0.55 +InputFilter.band2_end=1.0 + +;#ampl: desired amplitude at the band edges [ a(b1) a(e1) a(b2) a(e2) ...]. +;#The number of ampl_begin and ampl_end elements must match the number of bands + +InputFilter.ampl1_begin=1.0 +InputFilter.ampl1_end=1.0 +InputFilter.ampl2_begin=0.0 +InputFilter.ampl2_end=0.0 + +;#band_error: weighting applied to each band (usually 1). +;#The number of band_error elements must match the number of bands +InputFilter.band1_error=1.0 +InputFilter.band2_error=1.0 + +;#filter_type: one of "bandpass", "hilbert" or "differentiator" +InputFilter.filter_type=bandpass + +;#grid_density: determines how accurately the filter will be constructed. +;The minimum value is 16; higher values are slower to compute the filter. +InputFilter.grid_density=16 + +;# Original sampling frequency stored in the signal file +InputFilter.sampling_frequency=20480000 + +;#The following options are used only in Freq_Xlating_Fir_Filter implementation. +;#InputFilter.IF is the intermediate frequency (in Hz) shifted down to zero Hz + +InputFilter.IF=5499998.47412109 + +;# Decimation factor after the frequency tranaslating block +InputFilter.decimation_factor=8 + + +;######### RESAMPLER CONFIG ############ +;## Resamples the input data. + +;#implementation: Use [Pass_Through] or [Direct_Resampler] +;#[Pass_Through] disables this block +;#[Direct_Resampler] enables a resampler that implements a nearest neigbourhood interpolation +Resampler.implementation=Pass_Through + +;######### CHANNELS GLOBAL CONFIG ############ +;#count: Number of available GPS satellite channels. +Channels_1C.count=8 +Channels.in_acquisition=1 +#Channel.signal=1C + + +;######### ACQUISITION GLOBAL CONFIG ############ +Acquisition_1C.dump=false +Acquisition_1C.dump_filename=./acq_dump.dat +Acquisition_1C.item_type=gr_complex +Acquisition_1C.if=0 +Acquisition_1C.sampled_ms=1 +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +;#use_CFAR_algorithm: If enabled, acquisition estimates the input signal power to implement CFAR detection algorithms +;#notice that this affects the Acquisition threshold range! +Acquisition_1C.use_CFAR_algorithm=false; +;#threshold: Acquisition threshold +Acquisition_1C.threshold=10 +;Acquisition_1C.pfa=0.01 +Acquisition_1C.doppler_max=5000 +Acquisition_1C.doppler_step=100 + + +;######### TRACKING GPS CONFIG ############ +Tracking_1C.implementation=GPS_L1_CA_KF_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.if=0 +Tracking_1C.dump=true +Tracking_1C.dump_filename=../data/epl_tracking_ch_ +Tracking_1C.pll_bw_hz=15.0; +Tracking_1C.dll_bw_hz=2.0; +Tracking_1C.order=3; + + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false +TelemetryDecoder_1C.decimation_factor=1; + +;######### OBSERVABLES CONFIG ############ +;#implementation: +Observables.implementation=Hybrid_Observables + +;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] +Observables.dump=false + +;#dump_filename: Log path and filename. +Observables.dump_filename=./observables.dat + +;######### PVT CONFIG ############ +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +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=100 +PVT.display_rate_ms=500 +PVT.dump_filename=./PVT +PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea; +PVT.flag_nmea_tty_port=false; +PVT.nmea_dump_devname=/dev/pts/4 +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 +PVT.dump=true diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index 437626d06..18f98d42c 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -36,6 +36,7 @@ set(TRACKING_ADAPTER_SOURCES glonass_l1_ca_dll_pll_tracking.cc glonass_l1_ca_dll_pll_c_aid_tracking.cc gps_l5i_dll_pll_tracking.cc + gps_l1_ca_kf_tracking.cc ${OPT_TRACKING_ADAPTERS} ) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc new file mode 100644 index 000000000..b649a0413 --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -0,0 +1,150 @@ +/*! + * \file gps_l1_ca_kf_tracking.cc + * \brief Implementation of an adapter of a DLL+PLL tracking loop block + * for GPS L1 C/A to a TrackingInterface + * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com + * Javier Arribas, 2011. jarribas(at)cttc.es + * + * Code DLL + carrier PLL according to the algorithms described in: + * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency + * Approach, Birkhauser, 2007 + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (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. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#include "gps_l1_ca_kf_tracking.h" +#include +#include "GPS_L1_CA.h" +#include "configuration_interface.h" + + +using google::LogMessage; + +GpsL1CaKfTracking::GpsL1CaKfTracking( + ConfigurationInterface* configuration, std::string role, + unsigned int in_streams, unsigned int out_streams) : + role_(role), in_streams_(in_streams), out_streams_(out_streams) +{ + DLOG(INFO) << "role " << role; + //################# CONFIGURATION PARAMETERS ######################## + int fs_in; + int vector_length; + int f_if; + bool dump; + std::string dump_filename; + std::string item_type; + std::string default_item_type = "gr_complex"; + float pll_bw_hz; + float dll_bw_hz; + float early_late_space_chips; + item_type = configuration->property(role + ".item_type", default_item_type); + int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); + fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + f_if = configuration->property(role + ".if", 0); + dump = configuration->property(role + ".dump", false); + pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + std::string default_dump_filename = "./track_ch"; + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); + + //################# MAKE TRACKING GNURadio object ################### + if (item_type.compare("gr_complex") == 0) + { + item_size_ = sizeof(gr_complex); + tracking_ = gps_l1_ca_kf_make_tracking_cc( + f_if, + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + early_late_space_chips); + } + else + { + item_size_ = sizeof(gr_complex); + LOG(WARNING) << item_type << " unknown tracking item type."; + } + channel_ = 0; + DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")"; +} + + +GpsL1CaKfTracking::~GpsL1CaKfTracking() +{} + + +void GpsL1CaKfTracking::start_tracking() +{ + tracking_->start_tracking(); +} + + +/* + * Set tracking channel unique ID + */ +void GpsL1CaKfTracking::set_channel(unsigned int channel) +{ + channel_ = channel; + tracking_->set_channel(channel); +} + + +void GpsL1CaKfTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + tracking_->set_gnss_synchro(p_gnss_synchro); +} + + +void GpsL1CaKfTracking::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 GpsL1CaKfTracking::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 GpsL1CaKfTracking::get_left_block() +{ + return tracking_; +} + + +gr::basic_block_sptr GpsL1CaKfTracking::get_right_block() +{ + return tracking_; +} + diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h new file mode 100644 index 000000000..2b2839475 --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h @@ -0,0 +1,104 @@ +/*! + * \file GPS_L1_CA_KF_Tracking.h + * \brief Interface of an adapter of a DLL+PLL tracking loop block + * for GPS L1 C/A to a TrackingInterface + * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com + * Javier Arribas, 2011. jarribas(at)cttc.es + * + * Code DLL + carrier PLL according to the algorithms described in: + * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency + * Approach, Birkhauser, 2007 + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (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. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ +#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ + +#include +#include "tracking_interface.h" +#include "gps_l1_ca_kf_tracking_cc.h" + + +class ConfigurationInterface; + +/*! + * \brief This class implements a code DLL + carrier PLL tracking loop + */ +class GpsL1CaKfTracking : public TrackingInterface +{ +public: + GpsL1CaKfTracking(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams); + + virtual ~GpsL1CaKfTracking(); + + inline std::string role() override + { + return role_; + } + + //! Returns "GPS_L1_CA_KF_Tracking" + inline std::string implementation() override + { + return "GPS_L1_CA_KF_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; + +private: + gps_l1_ca_kf_tracking_cc_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_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 4c4a2a550..4afd844e1 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -39,6 +39,7 @@ set(TRACKING_GR_BLOCKS_SOURCES glonass_l1_ca_dll_pll_tracking_cc.cc glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc + gps_l1_ca_kf_tracking_cc.cc ${OPT_TRACKING_BLOCKS} ) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc new file mode 100644 index 000000000..834528974 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -0,0 +1,619 @@ +/*! + * \file gps_l1_ca_kf_tracking_cc.cc + * \brief Implementation of a code DLL + carrier PLL tracking block + * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com + * Javier Arribas, 2011. jarribas(at)cttc.es + * + * Code DLL + carrier PLL according to the algorithms described in: + * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency + * Approach, Birkhauser, 2007 + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (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. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "gps_l1_ca_kf_tracking_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include "gps_sdr_signal_processing.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "GPS_L1_CA.h" +#include "control_message_factory.h" + + +/*! + * \todo Include in definition header file + */ +#define CN0_ESTIMATION_SAMPLES 20 +#define MINIMUM_VALID_CN0 25 +#define MAXIMUM_LOCK_FAIL_COUNTER 5000 +#define CARRIER_LOCK_THRESHOLD 0.85 + + +using google::LogMessage; + +gps_l1_ca_kf_tracking_cc_sptr +gps_l1_ca_kf_make_tracking_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) +{ + return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(if_freq, + fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); +} + + + +void Gps_L1_Ca_Kf_Tracking_cc::forecast (int noutput_items, + gr_vector_int &ninput_items_required) +{ + if (noutput_items != 0) + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + } +} + + + +Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) : + gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // Telemetry bit synchronization message port input + this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + this->message_port_register_out(pmt::mp("events")); + + // initialize internal vars + d_dump = dump; + d_if_freq = if_freq; + d_fs_in = fs_in; + d_vector_length = vector_length; + d_dump_filename = dump_filename; + + d_current_prn_length_samples = static_cast(d_vector_length); + + // Initialize tracking ========================================== + d_code_loop_filter.set_DLL_BW(dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); + + //--- DLL variables -------------------------------------------------------- + d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) + + // Initialization of local code replica + // Get space for a vector with the C/A code replica sampled 1x/chip + d_ca_code = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); + + // correlator outputs (scalar) + d_n_correlator_taps = 3; // Early, Prompt, and Late + d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + + multicorrelator_cpu.init(2 * d_current_prn_length_samples, d_n_correlator_taps); + + //--- Perform initializations ------------------------------ + // define initial code frequency basis of NCO + d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ; + // define residual code phase (in chips) + d_rem_code_phase_samples = 0.0; + // define residual carrier phase + d_rem_carr_phase_rad = 0.0; + + // sample synchronization + d_sample_counter = 0; + //d_sample_counter_seconds = 0; + d_acq_sample_stamp = 0; + + d_enable_tracking = false; + d_pull_in = false; + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; + d_carrier_lock_test = 1; + d_CN0_SNV_dB_Hz = 0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; + + systemName["G"] = std::string("GPS"); + systemName["S"] = std::string("SBAS"); + + d_acquisition_gnss_synchro = 0; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + d_acq_carrier_doppler_hz = 0.0; + d_carrier_doppler_hz = 0.0; + d_acc_carrier_phase_rad = 0.0; + d_code_phase_samples = 0.0; + d_rem_code_phase_chips = 0.0; + d_code_phase_step_chips = 0.0; + d_carrier_phase_step_rad = 0.0; + + set_relative_rate(1.0 / static_cast(d_vector_length)); + + // Kalman filter initialization (receiver initialization) + + double CN_dB_Hz=40; + double CN_lin=pow(10,CN_dB_Hz/10.0); + + double sigma2_phase_detector_cycles2; + sigma2_phase_detector_cycles2=(1.0/(2.0*CN_lin*GPS_L1_CA_CODE_PERIOD))*(1.0+1.0/(2.0*CN_lin*GPS_L1_CA_CODE_PERIOD)); + + //covariances (static) + double sigma2_carrier_phase=GPS_TWO_PI/4.0; + double sigma2_doppler=250; + + kf_P_x_ini=arma::zeros(2,2); + kf_P_x_ini(0,0)=sigma2_carrier_phase; + kf_P_x_ini(1,1)=sigma2_doppler; + + kf_R=arma::zeros(1,1); + kf_R(0,0)=sigma2_phase_detector_cycles2; + + //arma::colvec G={pow(GPS_L1_CA_CODE_PERIOD,3)/6.0, pow(GPS_L1_CA_CODE_PERIOD,2)/2.0,GPS_L1_CA_CODE_PERIOD}; + kf_Q=arma::zeros(2,2); + kf_Q(0,0)=1e-14; + kf_Q(1,1)=1e-2; + +// kf_Q=arma::diagmat(pow(GPS_L1_CA_CODE_PERIOD,6)*kf_Q); + //std::cout<<"kf_Q="<Acq_delay_samples; + d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + + long int acq_trk_diff_samples; + double acq_trk_diff_seconds; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); + // Doppler effect + // Fd=(C/(C+Vr))*F + double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ; + // new chip and prn sequence periods based on acq Doppler + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; + d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; + d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + T_chip_mod_seconds = 1/d_code_freq_chips; + T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; + T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + + d_current_prn_length_samples = round(T_prn_mod_samples); + + double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples, delay_correction_samples; + corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); + if (corrected_acq_phase_samples < 0) + { + corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; + } + delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; + + d_acq_code_phase_samples = corrected_acq_phase_samples; + + d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + + // DLL/PLL filter initialization + d_carrier_loop_filter.initialize(); // initialize the carrier filter + d_code_loop_filter.initialize(); // initialize the code filter + + // generate local reference ALWAYS starting at chip 1 (1 sample per chip) + gps_l1_ca_code_gen_float(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); + + multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0; + d_rem_carr_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_rad = 0.0; + + d_code_phase_samples = d_acq_code_phase_samples; + + std::string sys_ = &d_acquisition_gnss_synchro->System; + sys = sys_.substr(0,1); + + // DEBUG OUTPUT + std::cout << "Tracking of GPS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // enable tracking + d_pull_in = true; + d_enable_tracking = true; + + LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; + +} + + +Gps_L1_Ca_Kf_Tracking_cc::~Gps_L1_Ca_Kf_Tracking_cc() +{ + if (d_dump_file.is_open()) + { + try + { + d_dump_file.close(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } + } + try + { + volk_gnsssdr_free(d_local_code_shift_chips); + volk_gnsssdr_free(d_correlator_outs); + volk_gnsssdr_free(d_ca_code); + delete[] d_Prompt_buffer; + multicorrelator_cpu.free(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } +} + + + +int Gps_L1_Ca_Kf_Tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + // process vars + double carr_phase_error_rad = 0.0; + double carr_phase_error_filt_rad = 0.0; + double code_error_chips = 0.0; + double code_error_filt_chips = 0.0; + + // Block input data and block output stream pointers + const gr_complex* in = reinterpret_cast(input_items[0]); + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); + + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + if (d_enable_tracking == true) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Receiver signal alignment + if (d_pull_in == true) + { + int samples_offset; + double acq_trk_shif_correction_samples; + int acq_to_trk_delay_samples; + acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; + d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples + d_pull_in = false; + // take into account the carrier cycles accumulated in the pull in signal alignment + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.fs = d_fs_in; + current_synchro_data.correlation_length_ms = 1; + *out[0] = current_synchro_data; + //Kalman filter initialization reset + kf_P_x=kf_P_x_ini; + //Update Kalman states based on acquisition information + kf_x(0)=0.0; + kf_x(1)=current_synchro_data.Carrier_Doppler_hz; + + consume_each(samples_offset); // shift input to perform alignment with local replica + return 1; + } + + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_current_prn_length_samples); + + // remnant carrier phase to prevent overflow in the code NCO + //d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; + //d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); + + // ################## Kalman Carrier Tracking ###################################### + + //Kalman state prediction (time update) + kf_x_pre=kf_F*kf_x; //state prediction + kf_P_x_pre=kf_F*kf_P_x*kf_F.t()+kf_Q; //state error covariance prediction + + // Update discriminator [rads/Ti] + carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output + + //Kalman estimation (measuremant update) + //kf_y_pre=kf_H*kf_x_pre; //measurement prediction + + kf_P_y=kf_H*kf_P_x_pre*kf_H.t()+kf_R; //innovation covariance matrix + kf_K=(kf_P_x_pre*kf_H.t())*arma::inv(kf_P_y); //Kalman gain + + kf_y(0)=carr_phase_error_rad; //measurement vector + //kf_x=kf_x_pre+kf_K*(kf_y-kf_y_pre); //updated state estimation + kf_x=kf_x_pre+kf_K*kf_y; //updated state estimation + + kf_x(0)=fmod(arma::as_scalar(kf_x(0)), GPS_TWO_PI); + //kf_P_x=kf_P_x_pre-kf_K*kf_H*kf_P_x_pre; //update state estimation error covariance matrix + kf_P_x=(arma::eye(2,2)-kf_K*kf_H)*kf_P_x_pre; //update state estimation error covariance matrix + + d_rem_carr_phase_rad=kf_x(0); //set a new carrier Phase estimation to the NCO + d_carrier_doppler_hz=kf_x(1); //set a new carrier Doppler estimation to the NCO + + carr_phase_error_filt_rad=d_rem_carr_phase_rad; + + // ################## DLL ########################################################## + // New code Doppler frequency estimation based on carrier frequency estimation + d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); + // DLL discriminator + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late + // Code discriminator filter + code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] + double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); + double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; + double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips*T_chip_seconds); //[seconds] + //double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; // [seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + //double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); + //double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples + + //################### NCO COMMANDS ################################################# + // carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + // carrier phase accumulator + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples; + + //################### DLL COMMANDS ################################################# + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample + d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); + + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt + d_cn0_estimation_counter++; + } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); + //std::cout<<"Channel "< 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine + } + } + // ########### Output the tracking data to navigation and PVT ########## + current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = 1; + } + else + { + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + + current_synchro_data.Tracking_sample_counter =d_sample_counter + d_current_prn_length_samples; + current_synchro_data.System = {'G'}; + current_synchro_data.correlation_length_ms = 1; + } + + //assign the GNURadio block output data + current_synchro_data.fs = d_fs_in; + *out[0] = current_synchro_data; + + if(d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + double tmp_double; + unsigned long int tmp_long; + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); + try + { + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + tmp_long = d_sample_counter + d_current_prn_length_samples; + d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); + + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); + + // PLL commands + d_dump_file.write(reinterpret_cast(&carr_phase_error_rad), sizeof(double)); + d_dump_file.write(reinterpret_cast(&carr_phase_error_filt_rad), sizeof(double)); + + // DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); + + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // PRN + unsigned int prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } + + consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates + d_sample_counter += d_current_prn_length_samples; // count for the processed samples + return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false +} + + + +void Gps_L1_Ca_Kf_Tracking_cc::set_channel(unsigned int channel) +{ + d_channel = channel; + LOG(INFO) << "Tracking Channel set to " << d_channel; + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename.append(boost::lexical_cast(d_channel)); + d_dump_filename.append(".dat"); + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } + } +} + + +void Gps_L1_Ca_Kf_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + d_acquisition_gnss_synchro = p_gnss_synchro; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h new file mode 100644 index 000000000..c43274f9b --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -0,0 +1,187 @@ +/*! + * \file gps_l1_ca_dll_pll_tracking_cc.h + * \brief Interface of a code DLL + carrier PLL tracking block + * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com + * Javier Arribas, 2011. jarribas(at)cttc.es + * Cillian O'Driscoll, 2017. cillian.odriscoll(at)gmail.com + * + * Code DLL + carrier PLL according to the algorithms described in: + * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, + * Birkhauser, 2007 + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (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. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H +#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H + +#include +#include +#include +#include +#include "gnss_synchro.h" +#include "tracking_2nd_DLL_filter.h" +#include "tracking_2nd_PLL_filter.h" +#include +#include "cpu_multicorrelator_real_codes.h" + +class Gps_L1_Ca_Kf_Tracking_cc; + +typedef boost::shared_ptr + gps_l1_ca_kf_tracking_cc_sptr; + +gps_l1_ca_kf_tracking_cc_sptr +gps_l1_ca_kf_make_tracking_cc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips); + + + +/*! + * \brief This class implements a DLL + PLL tracking loop block + */ +class Gps_L1_Ca_Kf_Tracking_cc: public gr::block +{ +public: + ~Gps_L1_Ca_Kf_Tracking_cc(); + + void set_channel(unsigned int channel); + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void start_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 gps_l1_ca_kf_tracking_cc_sptr + gps_l1_ca_kf_make_tracking_cc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips); + + Gps_L1_Ca_Kf_Tracking_cc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips); + + // tracking configuration vars + unsigned int d_vector_length; + bool d_dump; + + Gnss_Synchro* d_acquisition_gnss_synchro; + unsigned int d_channel; + + long d_if_freq; + long d_fs_in; + + double d_early_late_spc_chips; + + // remaining code phase and carrier phase between tracking loops + double d_rem_code_phase_samples; + double d_rem_code_phase_chips; + double d_rem_carr_phase_rad; + + // Kalman filter variables + arma::mat kf_P_x_ini; //initial state error covariance matrix + arma::mat kf_P_x; //state error covariance matrix + arma::mat kf_P_x_pre; //Predicted state error covariance matrix + arma::mat kf_P_y; //innovation covariance matrix + arma::mat kf_F; //state transition matrix + arma::mat kf_H; //system matrix + arma::mat kf_R; //measurement error covariance matrix + arma::mat kf_Q; //system error covariance matrix + arma::colvec kf_x; //state vector + arma::colvec kf_x_pre; //predicted state vector + arma::colvec kf_y; //measurement vector + arma::colvec kf_y_pre; //measurement vector + arma::mat kf_K; //Kalman gain matrix + + + // PLL and DLL filter library + Tracking_2nd_DLL_filter d_code_loop_filter; + Tracking_2nd_PLL_filter d_carrier_loop_filter; + + // acquisition + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; + // correlator + int d_n_correlator_taps; + float* d_ca_code; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator_real_codes multicorrelator_cpu; + + // tracking vars + double d_code_freq_chips; + double d_code_phase_step_chips; + double d_carrier_doppler_hz; + double d_carrier_phase_step_rad; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + + //PRN period in samples + int d_current_prn_length_samples; + + //processing samples counters + unsigned long int d_sample_counter; + unsigned long int d_acq_sample_stamp; + + // CN0 estimation and lock detector + int d_cn0_estimation_counter; + gr_complex* d_Prompt_buffer; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; + int d_carrier_lock_fail_counter; + + // control vars + bool d_enable_tracking; + bool d_pull_in; + + // file dump + std::string d_dump_filename; + std::ofstream d_dump_file; + + std::map systemName; + std::string sys; +}; + +#endif //GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index c6228359b..c9edbd8e8 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -98,6 +98,7 @@ #include "sbas_l1_telemetry_decoder.h" #include "hybrid_observables.h" #include "rtklib_pvt.h" +#include "gps_l1_ca_kf_tracking.h" #if ENABLE_FPGA #include "gps_l1_ca_dll_pll_c_aid_tracking_fpga.h" @@ -1290,6 +1291,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GPS_L1_CA_KF_Tracking") == 0) + { + std::unique_ptr block_(new GpsL1CaKfTracking(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0) { std::unique_ptr block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams, @@ -1579,6 +1586,12 @@ std::unique_ptr GNSSBlockFactory::GetTrkBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GPS_L1_CA_KF_Tracking") == 0) + { + std::unique_ptr block_(new GpsL1CaKfTracking(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0) { std::unique_ptr block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams, From fa89da48e61b46f7adf04e846fb42a240ef65011 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 15 Mar 2018 18:56:29 +0100 Subject: [PATCH 02/23] Add KF test, some KF implementation fixes, use flags in tests --- .../adapters/gps_l1_ca_kf_tracking.cc | 41 +- .../gps_l1_ca_kf_tracking_cc.cc | 561 +++++++++++------ .../gps_l1_ca_kf_tracking_cc.h | 79 ++- src/tests/test_main.cc | 1 + .../gps_l1_ca_dll_pll_tracking_test.cc | 21 +- .../tracking/gps_l1_ca_kf_tracking_test.cc | 569 ++++++++++++++++++ 6 files changed, 1025 insertions(+), 247 deletions(-) create mode 100644 src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc index b649a0413..f41b0c4dc 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -37,17 +37,17 @@ #include "gps_l1_ca_kf_tracking.h" -#include +#include "gnss_sdr_flags.h" #include "GPS_L1_CA.h" #include "configuration_interface.h" +#include using google::LogMessage; GpsL1CaKfTracking::GpsL1CaKfTracking( - ConfigurationInterface* configuration, std::string role, - unsigned int in_streams, unsigned int out_streams) : - role_(role), in_streams_(in_streams), out_streams_(out_streams) + ConfigurationInterface* configuration, std::string role, + unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## @@ -66,11 +66,13 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); f_if = configuration->property(role + ".if", 0); dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + //pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + //if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### @@ -78,14 +80,13 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( { item_size_ = sizeof(gr_complex); tracking_ = gps_l1_ca_kf_make_tracking_cc( - f_if, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - early_late_space_chips); + f_if, + fs_in, + vector_length, + dump, + dump_filename, + dll_bw_hz, + early_late_space_chips); } else { @@ -98,7 +99,8 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( GpsL1CaKfTracking::~GpsL1CaKfTracking() -{} +{ +} void GpsL1CaKfTracking::start_tracking() @@ -125,14 +127,18 @@ void GpsL1CaKfTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) void GpsL1CaKfTracking::connect(gr::top_block_sptr top_block) { - if(top_block) { /* top_block is not null */}; + if (top_block) + { /* top_block is not null */ + }; //nothing to connect, now the tracking uses gr_sync_decimator } void GpsL1CaKfTracking::disconnect(gr::top_block_sptr top_block) { - if(top_block) { /* top_block is not null */}; + if (top_block) + { /* top_block is not null */ + }; //nothing to disconnect, now the tracking uses gr_sync_decimator } @@ -147,4 +153,3 @@ gr::basic_block_sptr GpsL1CaKfTracking::get_right_block() { return tracking_; } - diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index 834528974..4b70d7baa 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -35,71 +35,59 @@ */ #include "gps_l1_ca_kf_tracking_cc.h" -#include -#include -#include -#include +#include "gps_sdr_signal_processing.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "gnss_sdr_flags.h" +#include "GPS_L1_CA.h" +#include "control_message_factory.h" #include #include #include #include -#include "gps_sdr_signal_processing.h" -#include "tracking_discriminators.h" -#include "lock_detectors.h" -#include "GPS_L1_CA.h" -#include "control_message_factory.h" - - -/*! - * \todo Include in definition header file - */ -#define CN0_ESTIMATION_SAMPLES 20 -#define MINIMUM_VALID_CN0 25 -#define MAXIMUM_LOCK_FAIL_COUNTER 5000 -#define CARRIER_LOCK_THRESHOLD 0.85 +#include +#include +#include +#include +#include using google::LogMessage; gps_l1_ca_kf_tracking_cc_sptr gps_l1_ca_kf_make_tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float dll_bw_hz, + float early_late_space_chips) { return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(if_freq, - fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); + fs_in, vector_length, dump, dump_filename, dll_bw_hz, early_late_space_chips)); } - -void Gps_L1_Ca_Kf_Tracking_cc::forecast (int noutput_items, - gr_vector_int &ninput_items_required) +void Gps_L1_Ca_Kf_Tracking_cc::forecast(int noutput_items, + gr_vector_int &ninput_items_required) { if (noutput_items != 0) { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call } } - Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) : - gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float dll_bw_hz, + float early_late_space_chips) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); @@ -116,25 +104,24 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( // Initialize tracking ========================================== d_code_loop_filter.set_DLL_BW(dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); //--- DLL variables -------------------------------------------------------- - d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) + d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) // Initialization of local code replica // Get space for a vector with the C/A code replica sampled 1x/chip - d_ca_code = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); + d_ca_code = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); // correlator outputs (scalar) - d_n_correlator_taps = 3; // Early, Prompt, and Late - d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_n_correlator_taps = 3; // Early, Prompt, and Late + d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); for (int n = 0; n < d_n_correlator_taps; n++) { - d_correlator_outs[n] = gr_complex(0,0); + d_correlator_outs[n] = gr_complex(0, 0); } - d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[0] = -d_early_late_spc_chips; d_local_code_shift_chips[1] = 0.0; d_local_code_shift_chips[2] = d_early_late_spc_chips; @@ -158,11 +145,11 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; + d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; + d_carrier_lock_threshold = FLAGS_carrier_lock_th; systemName["G"] = std::string("GPS"); systemName["S"] = std::string("SBAS"); @@ -182,43 +169,42 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( // Kalman filter initialization (receiver initialization) - double CN_dB_Hz=40; - double CN_lin=pow(10,CN_dB_Hz/10.0); + double CN_dB_Hz = 40; + double CN_lin = pow(10, CN_dB_Hz / 10.0); double sigma2_phase_detector_cycles2; - sigma2_phase_detector_cycles2=(1.0/(2.0*CN_lin*GPS_L1_CA_CODE_PERIOD))*(1.0+1.0/(2.0*CN_lin*GPS_L1_CA_CODE_PERIOD)); + sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)); //covariances (static) - double sigma2_carrier_phase=GPS_TWO_PI/4.0; - double sigma2_doppler=250; + double sigma2_carrier_phase = GPS_TWO_PI / 4; + double sigma2_doppler = 250; - kf_P_x_ini=arma::zeros(2,2); - kf_P_x_ini(0,0)=sigma2_carrier_phase; - kf_P_x_ini(1,1)=sigma2_doppler; + kf_P_x_ini = arma::zeros(2, 2); + kf_P_x_ini(0, 0) = sigma2_carrier_phase; + kf_P_x_ini(1, 1) = sigma2_doppler; - kf_R=arma::zeros(1,1); - kf_R(0,0)=sigma2_phase_detector_cycles2; + kf_R = arma::zeros(1, 1); + kf_R(0, 0) = sigma2_phase_detector_cycles2; //arma::colvec G={pow(GPS_L1_CA_CODE_PERIOD,3)/6.0, pow(GPS_L1_CA_CODE_PERIOD,2)/2.0,GPS_L1_CA_CODE_PERIOD}; - kf_Q=arma::zeros(2,2); - kf_Q(0,0)=1e-14; - kf_Q(1,1)=1e-2; + kf_Q = arma::zeros(2, 2); + kf_Q(0, 0) = 1e-12; + kf_Q(1, 1) = 1e-2; -// kf_Q=arma::diagmat(pow(GPS_L1_CA_CODE_PERIOD,6)*kf_Q); + // kf_Q=arma::diagmat(pow(GPS_L1_CA_CODE_PERIOD,6)*kf_Q); //std::cout<<"kf_Q="<(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect @@ -245,7 +231,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() double T_prn_mod_samples; d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); - T_chip_mod_seconds = 1/d_code_freq_chips; + T_chip_mod_seconds = 1 / d_code_freq_chips; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); @@ -268,9 +254,8 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - // DLL/PLL filter initialization - d_carrier_loop_filter.initialize(); // initialize the carrier filter - d_code_loop_filter.initialize(); // initialize the code filter + // DLL filter initialization + d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) gps_l1_ca_code_gen_float(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); @@ -278,7 +263,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); for (int n = 0; n < d_n_correlator_taps; n++) { - d_correlator_outs[n] = gr_complex(0,0); + d_correlator_outs[n] = gr_complex(0, 0); } d_carrier_lock_fail_counter = 0; @@ -290,7 +275,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_code_phase_samples = d_acq_code_phase_samples; std::string sys_ = &d_acquisition_gnss_synchro->System; - sys = sys_.substr(0,1); + sys = sys_.substr(0, 1); // DEBUG OUTPUT std::cout << "Tracking of GPS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; @@ -301,9 +286,8 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_enable_tracking = true; LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz - << " Code Phase correction [samples]=" << delay_correction_samples - << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; - + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; } @@ -312,32 +296,43 @@ Gps_L1_Ca_Kf_Tracking_cc::~Gps_L1_Ca_Kf_Tracking_cc() if (d_dump_file.is_open()) { try - { + { d_dump_file.close(); - } - catch(const std::exception & ex) - { + } + catch (const std::exception &ex) + { LOG(WARNING) << "Exception in destructor " << ex.what(); - } + } + } + if (d_dump) + { + if (d_channel == 0) + { + std::cout << "Writing .mat files ..."; + } + Gps_L1_Ca_Kf_Tracking_cc::save_matfile(); + if (d_channel == 0) + { + std::cout << " done." << std::endl; + } } try - { + { volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_ca_code); delete[] d_Prompt_buffer; multicorrelator_cpu.free(); - } - catch(const std::exception & ex) - { + } + catch (const std::exception &ex) + { LOG(WARNING) << "Exception in destructor " << ex.what(); - } + } } - -int Gps_L1_Ca_Kf_Tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars double carr_phase_error_rad = 0.0; @@ -346,7 +341,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work (int noutput_items __attribute__((unu double code_error_filt_chips = 0.0; // Block input data and block output stream pointers - const gr_complex* in = reinterpret_cast(input_items[0]); + const gr_complex *in = reinterpret_cast(input_items[0]); Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder @@ -366,7 +361,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work (int noutput_items __attribute__((unu acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; - d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples + d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples d_pull_in = false; // take into account the carrier cycles accumulated in the pull in signal alignment d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; @@ -376,12 +371,12 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work (int noutput_items __attribute__((unu current_synchro_data.correlation_length_ms = 1; *out[0] = current_synchro_data; //Kalman filter initialization reset - kf_P_x=kf_P_x_ini; + kf_P_x = kf_P_x_ini; //Update Kalman states based on acquisition information - kf_x(0)=0.0; - kf_x(1)=current_synchro_data.Carrier_Doppler_hz; + kf_x(0) = d_carrier_phase_step_rad * samples_offset; + kf_x(1) = current_synchro_data.Carrier_Doppler_hz; - consume_each(samples_offset); // shift input to perform alignment with local replica + consume_each(samples_offset); // shift input to perform alignment with local replica return 1; } @@ -389,94 +384,86 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work (int noutput_items __attribute__((unu // perform carrier wipe-off and compute Early, Prompt and Late correlation multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_current_prn_length_samples); - - // remnant carrier phase to prevent overflow in the code NCO - //d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; - //d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_current_prn_length_samples); // ################## Kalman Carrier Tracking ###################################### //Kalman state prediction (time update) - kf_x_pre=kf_F*kf_x; //state prediction - kf_P_x_pre=kf_F*kf_P_x*kf_F.t()+kf_Q; //state error covariance prediction + kf_x_pre = kf_F * kf_x; //state prediction + kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q; //state error covariance prediction // Update discriminator [rads/Ti] - carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output + carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output - //Kalman estimation (measuremant update) - //kf_y_pre=kf_H*kf_x_pre; //measurement prediction + //Kalman estimation (measuremant update) + double sigma2_phase_detector_cycles2; + double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0); + sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)); + kf_R(0, 0) = sigma2_phase_detector_cycles2; - kf_P_y=kf_H*kf_P_x_pre*kf_H.t()+kf_R; //innovation covariance matrix - kf_K=(kf_P_x_pre*kf_H.t())*arma::inv(kf_P_y); //Kalman gain + kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix + kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain - kf_y(0)=carr_phase_error_rad; //measurement vector - //kf_x=kf_x_pre+kf_K*(kf_y-kf_y_pre); //updated state estimation - kf_x=kf_x_pre+kf_K*kf_y; //updated state estimation + kf_y(0) = carr_phase_error_rad; // measurement vector + kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation - kf_x(0)=fmod(arma::as_scalar(kf_x(0)), GPS_TWO_PI); - //kf_P_x=kf_P_x_pre-kf_K*kf_H*kf_P_x_pre; //update state estimation error covariance matrix - kf_P_x=(arma::eye(2,2)-kf_K*kf_H)*kf_P_x_pre; //update state estimation error covariance matrix + kf_P_x = (arma::eye(2, 2) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix - d_rem_carr_phase_rad=kf_x(0); //set a new carrier Phase estimation to the NCO - d_carrier_doppler_hz=kf_x(1); //set a new carrier Doppler estimation to the NCO + d_rem_carr_phase_rad = kf_x(0); // set a new carrier Phase estimation to the NCO + d_carrier_doppler_hz = kf_x(1); // set a new carrier Doppler estimation to the NCO - carr_phase_error_filt_rad=d_rem_carr_phase_rad; + carr_phase_error_filt_rad = d_rem_carr_phase_rad; // ################## DLL ########################################################## - // New code Doppler frequency estimation based on carrier frequency estimation + // New code Doppler frequency estimation based on carrier frequency estimation d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] early and late // Code discriminator filter - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] + code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips*T_chip_seconds); //[seconds] - //double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; // [seconds] + double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds); // [seconds] - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### // keep alignment parameters for the next input buffer // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - //double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); - //double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples //################### NCO COMMANDS ################################################# // carrier phase step (NCO phase increment per sample) [rads/sample] d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); // carrier phase accumulator - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples; + d_acc_carrier_phase_rad = -kf_x(0); //################### DLL COMMANDS ################################################# // code phase step (Code resampler phase increment per sample) [chips/sample] d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); // remnant code phase [chips] - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + if (d_cn0_estimation_counter < FLAGS_cn0_samples) { // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt + d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt d_cn0_estimation_counter++; } else { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); - //std::cout<<"Channel "< 0) d_carrier_lock_fail_counter--; } - if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock d_carrier_lock_fail_counter = 0; - d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine + d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine } } // ########### Output the tracking data to navigation and PVT ########## @@ -508,19 +495,19 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work (int noutput_items __attribute__((unu { for (int n = 0; n < d_n_correlator_taps; n++) { - d_correlator_outs[n] = gr_complex(0,0); + d_correlator_outs[n] = gr_complex(0, 0); } - current_synchro_data.Tracking_sample_counter =d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; current_synchro_data.System = {'G'}; current_synchro_data.correlation_length_ms = 1; } - //assign the GNURadio block output data + // assign the GNU Radio block output data current_synchro_data.fs = d_fs_in; *out[0] = current_synchro_data; - if(d_dump) + if (d_dump) { // MULTIPLEXED FILE RECORDING - Record results to file float prompt_I; @@ -534,58 +521,264 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work (int noutput_items __attribute__((unu tmp_P = std::abs(d_correlator_outs[1]); tmp_L = std::abs(d_correlator_outs[2]); try - { + { // EPR - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp tmp_long = d_sample_counter + d_current_prn_length_samples; - d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); + d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); // PLL commands - d_dump_file.write(reinterpret_cast(&carr_phase_error_rad), sizeof(double)); - d_dump_file.write(reinterpret_cast(&carr_phase_error_filt_rad), sizeof(double)); + d_dump_file.write(reinterpret_cast(&carr_phase_error_rad), sizeof(double)); + d_dump_file.write(reinterpret_cast(&carr_phase_error_filt_rad), sizeof(double)); // DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); // AUX vars (for debug purposes) tmp_double = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = static_cast(d_sample_counter); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN unsigned int prn_ = d_acquisition_gnss_synchro->PRN; - d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); - } + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } catch (const std::ifstream::failure &e) - { + { LOG(WARNING) << "Exception writing trk dump file " << e.what(); - } + } } - consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates - d_sample_counter += d_current_prn_length_samples; // count for the processed samples - return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false + consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates + d_sample_counter += d_current_prn_length_samples; // count for the processed samples + return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false } +int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() +{ + // READ DUMP FILE + std::ifstream::pos_type size; + int number_of_double_vars = 11; + int number_of_float_vars = 5; + int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + + sizeof(float) * number_of_float_vars + sizeof(unsigned int); + std::ifstream dump_file; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << std::endl; + return 1; + } + // count number of epochs and rewind + long int num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + float *abs_E = new float[num_epoch]; + float *abs_P = new float[num_epoch]; + float *abs_L = new float[num_epoch]; + float *Prompt_I = new float[num_epoch]; + float *Prompt_Q = new float[num_epoch]; + unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch]; + double *acc_carrier_phase_rad = new double[num_epoch]; + double *carrier_doppler_hz = new double[num_epoch]; + double *code_freq_chips = new double[num_epoch]; + double *carr_error_hz = new double[num_epoch]; + double *carr_error_filt_hz = new double[num_epoch]; + double *code_error_chips = new double[num_epoch]; + double *code_error_filt_chips = new double[num_epoch]; + double *CN0_SNV_dB_Hz = new double[num_epoch]; + double *carrier_lock_test = new double[num_epoch]; + double *aux1 = new double[num_epoch]; + double *aux2 = new double[num_epoch]; + unsigned int *PRN = new unsigned int[num_epoch]; + + try + { + if (dump_file.is_open()) + { + for (long int i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); + dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&aux1[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << std::endl; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] code_freq_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = d_dump_filename; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != NULL) + { + size_t dims[2] = {1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] code_freq_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; + return 0; +} + void Gps_L1_Ca_Kf_Tracking_cc::set_channel(unsigned int channel) { @@ -597,23 +790,23 @@ void Gps_L1_Ca_Kf_Tracking_cc::set_channel(unsigned int channel) if (d_dump_file.is_open() == false) { try - { + { d_dump_filename.append(boost::lexical_cast(d_channel)); d_dump_filename.append(".dat"); - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); - } + } catch (const std::ifstream::failure &e) - { + { LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); - } + } } } } -void Gps_L1_Ca_Kf_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +void Gps_L1_Ca_Kf_Tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { d_acquisition_gnss_synchro = p_gnss_synchro; } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index c43274f9b..9b2e66ff4 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -51,24 +51,21 @@ class Gps_L1_Ca_Kf_Tracking_cc; typedef boost::shared_ptr - gps_l1_ca_kf_tracking_cc_sptr; + gps_l1_ca_kf_tracking_cc_sptr; gps_l1_ca_kf_tracking_cc_sptr gps_l1_ca_kf_make_tracking_cc(long if_freq, - long fs_in, unsigned - int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - + long fs_in, unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float early_late_space_chips); /*! * \brief This class implements a DLL + PLL tracking loop block */ -class Gps_L1_Ca_Kf_Tracking_cc: public gr::block +class Gps_L1_Ca_Kf_Tracking_cc : public gr::block { public: ~Gps_L1_Ca_Kf_Tracking_cc(); @@ -77,30 +74,26 @@ public: void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); void start_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); + 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); + void forecast(int noutput_items, gr_vector_int& ninput_items_required); private: friend gps_l1_ca_kf_tracking_cc_sptr gps_l1_ca_kf_make_tracking_cc(long if_freq, - long fs_in, unsigned - int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); + long fs_in, unsigned int vector_length, + bool dump, + std::string dump_filename, + float dll_bw_hz, + float early_late_space_chips); Gps_L1_Ca_Kf_Tracking_cc(long if_freq, - long fs_in, unsigned - int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); + long fs_in, unsigned int vector_length, + bool dump, + std::string dump_filename, + float dll_bw_hz, + float early_late_space_chips); // tracking configuration vars unsigned int d_vector_length; @@ -120,24 +113,24 @@ private: double d_rem_carr_phase_rad; // Kalman filter variables - arma::mat kf_P_x_ini; //initial state error covariance matrix - arma::mat kf_P_x; //state error covariance matrix - arma::mat kf_P_x_pre; //Predicted state error covariance matrix - arma::mat kf_P_y; //innovation covariance matrix - arma::mat kf_F; //state transition matrix - arma::mat kf_H; //system matrix - arma::mat kf_R; //measurement error covariance matrix - arma::mat kf_Q; //system error covariance matrix - arma::colvec kf_x; //state vector - arma::colvec kf_x_pre; //predicted state vector - arma::colvec kf_y; //measurement vector - arma::colvec kf_y_pre; //measurement vector - arma::mat kf_K; //Kalman gain matrix + arma::mat kf_P_x_ini; //initial state error covariance matrix + arma::mat kf_P_x; //state error covariance matrix + arma::mat kf_P_x_pre; //Predicted state error covariance matrix + arma::mat kf_P_y; //innovation covariance matrix + arma::mat kf_F; //state transition matrix + arma::mat kf_H; //system matrix + arma::mat kf_R; //measurement error covariance matrix + arma::mat kf_Q; //system error covariance matrix + arma::colvec kf_x; //state vector + arma::colvec kf_x_pre; //predicted state vector + arma::colvec kf_y; //measurement vector + arma::colvec kf_y_pre; //measurement vector + arma::mat kf_K; //Kalman gain matrix // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; - Tracking_2nd_PLL_filter d_carrier_loop_filter; + //Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition double d_acq_code_phase_samples; @@ -182,6 +175,8 @@ private: std::map systemName; std::string sys; + + int save_matfile(); }; -#endif //GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H +#endif //GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 6fdffbfc2..f7c17fed3 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -147,6 +147,7 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc" #if MODERN_ARMADILLO #include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc" +#include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc" #include "unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc" #include "unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc" #endif diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc index d0ff41697..b4fe6b852 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc @@ -51,6 +51,7 @@ #include "tracking_dump_reader.h" #include "signal_generator_flags.h" #include "gnuplot_i.h" +#include "gnss_sdr_flags.h" #include "test_flags.h" DEFINE_bool(plot_gps_l1_tracking_test, false, "Plots results of GpsL1CADllPllTrackingTest with gnuplot"); @@ -222,8 +223,22 @@ void GpsL1CADllPllTrackingTest::configure_receiver() // Set Tracking config->set_property("Tracking_1C.implementation", implementation); config->set_property("Tracking_1C.item_type", "gr_complex"); - config->set_property("Tracking_1C.pll_bw_hz", "20.0"); - config->set_property("Tracking_1C.dll_bw_hz", "2.0"); + if (FLAGS_pll_bw_hz != 0.0) + { + config->set_property("Tracking_1C.pll_bw_hz", std::to_string(FLAGS_pll_bw_hz)); + } + else + { + config->set_property("Tracking_1C.pll_bw_hz", "20.0"); + } + if (FLAGS_dll_bw_hz != 0.0) + { + config->set_property("Tracking_1C.dll_bw_hz", std::to_string(FLAGS_dll_bw_hz)); + } + else + { + config->set_property("Tracking_1C.dll_bw_hz", "2.0"); + } config->set_property("Tracking_1C.early_late_space_chips", "0.5"); config->set_property("Tracking_1C.extend_correlation_ms", "1"); config->set_property("Tracking_1C.dump", "true"); @@ -501,7 +516,7 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults) check_results_acc_carrier_phase(true_timestamp_s, true_acc_carrier_phase_cycles, trk_timestamp_s, trk_acc_carrier_phase_cycles); std::chrono::duration elapsed_seconds = end - start; - std::cout << "Signal tracking completed in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; + std::cout << "Signal tracking completed in " << elapsed_seconds.count() << " seconds." << std::endl; if (FLAGS_plot_gps_l1_tracking_test == true) { diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc new file mode 100644 index 000000000..2b44fc972 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc @@ -0,0 +1,569 @@ +/*! + * \file gps_l1_ca_kf_tracking_test.cc + * \brief This class implements a tracking test for GPS_L1_CA_KF_Tracking + * implementation based on some input parameters. + * \author Carles Fernandez, 2018 + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2012-2017 (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. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "GPS_L1_CA.h" +#include "gnss_block_factory.h" +#include "tracking_interface.h" +#include "in_memory_configuration.h" +#include "tracking_true_obs_reader.h" +#include "tracking_dump_reader.h" +#include "signal_generator_flags.h" +#include "gnuplot_i.h" +#include "test_flags.h" +#include "gnss_sdr_flags.h" + +DEFINE_bool(plot_gps_l1_kf_tracking_test, false, "Plots results of GpsL1CAKfTrackingTest with gnuplot"); + + +// ######## GNURADIO BLOCK MESSAGE RECEVER ######### +class GpsL1CAKfTrackingTest_msg_rx; + +typedef boost::shared_ptr GpsL1CAKfTrackingTest_msg_rx_sptr; + +GpsL1CAKfTrackingTest_msg_rx_sptr GpsL1CAKfTrackingTest_msg_rx_make(); + +class GpsL1CAKfTrackingTest_msg_rx : public gr::block +{ +private: + friend GpsL1CAKfTrackingTest_msg_rx_sptr GpsL1CAKfTrackingTest_msg_rx_make(); + void msg_handler_events(pmt::pmt_t msg); + GpsL1CAKfTrackingTest_msg_rx(); + +public: + int rx_message; + ~GpsL1CAKfTrackingTest_msg_rx(); //!< Default destructor +}; + + +GpsL1CAKfTrackingTest_msg_rx_sptr GpsL1CAKfTrackingTest_msg_rx_make() +{ + return GpsL1CAKfTrackingTest_msg_rx_sptr(new GpsL1CAKfTrackingTest_msg_rx()); +} + + +void GpsL1CAKfTrackingTest_msg_rx::msg_handler_events(pmt::pmt_t msg) +{ + try + { + long int message = pmt::to_long(msg); + rx_message = message; + } + catch (boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + rx_message = 0; + } +} + + +GpsL1CAKfTrackingTest_msg_rx::GpsL1CAKfTrackingTest_msg_rx() : gr::block("GpsL1CAKfTrackingTest_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ + this->message_port_register_in(pmt::mp("events")); + this->set_msg_handler(pmt::mp("events"), boost::bind(&GpsL1CAKfTrackingTest_msg_rx::msg_handler_events, this, _1)); + rx_message = 0; +} + + +GpsL1CAKfTrackingTest_msg_rx::~GpsL1CAKfTrackingTest_msg_rx() +{ +} + + +// ########################################################### + +class GpsL1CAKfTrackingTest : public ::testing::Test +{ +public: + std::string generator_binary; + std::string p1; + std::string p2; + std::string p3; + std::string p4; + std::string p5; + + std::string implementation = "GPS_L1_CA_KF_Tracking"; // "GPS_L1_CA_KF_Tracking"; + + const int baseband_sampling_freq = FLAGS_fs_gen_sps; + + std::string filename_rinex_obs = FLAGS_filename_rinex_obs; + std::string filename_raw_data = FLAGS_filename_raw_data; + + int configure_generator(); + int generate_signal(); + void check_results_doppler(arma::vec& true_time_s, + arma::vec& true_value, + arma::vec& meas_time_s, + arma::vec& meas_value); + void check_results_acc_carrier_phase(arma::vec& true_time_s, + arma::vec& true_value, + arma::vec& meas_time_s, + arma::vec& meas_value); + void check_results_codephase(arma::vec& true_time_s, + arma::vec& true_value, + arma::vec& meas_time_s, + arma::vec& meas_value); + + GpsL1CAKfTrackingTest() + { + factory = std::make_shared(); + config = std::make_shared(); + item_size = sizeof(gr_complex); + gnss_synchro = Gnss_Synchro(); + } + + ~GpsL1CAKfTrackingTest() + { + } + + void configure_receiver(); + + gr::top_block_sptr top_block; + std::shared_ptr factory; + std::shared_ptr config; + Gnss_Synchro gnss_synchro; + size_t item_size; +}; + + +int GpsL1CAKfTrackingTest::configure_generator() +{ + // Configure signal generator + generator_binary = FLAGS_generator_binary; + + p1 = std::string("-rinex_nav_file=") + FLAGS_rinex_nav_file; + if (FLAGS_dynamic_position.empty()) + { + p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(FLAGS_duration * 10); + } + else + { + p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position); + } + p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output + p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples + p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); // Baseband sampling frequency [MSps] + return 0; +} + + +int GpsL1CAKfTrackingTest::generate_signal() +{ + int child_status; + + char* const parmList[] = {&generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], &p4[0], &p5[0], NULL}; + + int pid; + if ((pid = fork()) == -1) + perror("fork err"); + else if (pid == 0) + { + execv(&generator_binary[0], parmList); + std::cout << "Return not expected. Must be an execv err." << std::endl; + std::terminate(); + } + + waitpid(pid, &child_status, 0); + + std::cout << "Signal and Observables RINEX and RAW files created." << std::endl; + return 0; +} + + +void GpsL1CAKfTrackingTest::configure_receiver() +{ + gnss_synchro.Channel_ID = 0; + gnss_synchro.System = 'G'; + std::string signal = "1C"; + signal.copy(gnss_synchro.Signal, 2, 0); + gnss_synchro.PRN = FLAGS_test_satellite_PRN; + + config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(baseband_sampling_freq)); + // Set Tracking + config->set_property("Tracking_1C.implementation", implementation); + config->set_property("Tracking_1C.item_type", "gr_complex"); + if (FLAGS_dll_bw_hz != 0.0) + { + config->set_property("Tracking_1C.dll_bw_hz", std::to_string(FLAGS_dll_bw_hz)); + } + else + { + config->set_property("Tracking_1C.dll_bw_hz", "2.0"); + } + config->set_property("Tracking_1C.early_late_space_chips", "0.5"); + config->set_property("Tracking_1C.extend_correlation_ms", "1"); + config->set_property("Tracking_1C.dump", "true"); + config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); +} + + +void GpsL1CAKfTrackingTest::check_results_doppler(arma::vec& true_time_s, + arma::vec& true_value, + arma::vec& meas_time_s, + arma::vec& meas_value) +{ + // 1. True value interpolation to match the measurement times + arma::vec true_value_interp; + arma::uvec true_time_s_valid = find(true_time_s > 0); + true_time_s = true_time_s(true_time_s_valid); + true_value = true_value(true_time_s_valid); + arma::uvec meas_time_s_valid = find(meas_time_s > 0); + meas_time_s = meas_time_s(meas_time_s_valid); + meas_value = meas_value(meas_time_s_valid); + + arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp); + + // 2. RMSE + arma::vec err; + + err = meas_value - true_value_interp; + arma::vec err2 = arma::square(err); + double rmse = sqrt(arma::mean(err2)); + + // 3. Mean err and variance + double error_mean = arma::mean(err); + double error_var = arma::var(err); + + // 4. Peaks + double max_error = arma::max(err); + double min_error = arma::min(err); + + // 5. report + std::streamsize ss = std::cout.precision(); + std::cout << std::setprecision(10) << "TRK Doppler RMSE=" << rmse + << ", mean=" << error_mean + << ", stdev=" << sqrt(error_var) << " (max,min)=" << max_error << "," << min_error << " [Hz]" << std::endl; + std::cout.precision(ss); +} + + +void GpsL1CAKfTrackingTest::check_results_acc_carrier_phase(arma::vec& true_time_s, + arma::vec& true_value, + arma::vec& meas_time_s, + arma::vec& meas_value) +{ + // 1. True value interpolation to match the measurement times + arma::vec true_value_interp; + arma::uvec true_time_s_valid = find(true_time_s > 0); + true_time_s = true_time_s(true_time_s_valid); + true_value = true_value(true_time_s_valid); + arma::uvec meas_time_s_valid = find(meas_time_s > 0); + meas_time_s = meas_time_s(meas_time_s_valid); + meas_value = meas_value(meas_time_s_valid); + + arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp); + + // 2. RMSE + arma::vec err; + err = meas_value - true_value_interp; + arma::vec err2 = arma::square(err); + double rmse = sqrt(arma::mean(err2)); + + // 3. Mean err and variance + double error_mean = arma::mean(err); + double error_var = arma::var(err); + + // 4. Peaks + double max_error = arma::max(err); + double min_error = arma::min(err); + + // 5. report + std::streamsize ss = std::cout.precision(); + std::cout << std::setprecision(10) << "TRK acc carrier phase RMSE=" << rmse + << ", mean=" << error_mean + << ", stdev=" << sqrt(error_var) << " (max,min)=" << max_error << "," << min_error << " [Hz]" << std::endl; + std::cout.precision(ss); +} + + +void GpsL1CAKfTrackingTest::check_results_codephase(arma::vec& true_time_s, + arma::vec& true_value, + arma::vec& meas_time_s, + arma::vec& meas_value) +{ + // 1. True value interpolation to match the measurement times + arma::vec true_value_interp; + arma::uvec true_time_s_valid = find(true_time_s > 0); + true_time_s = true_time_s(true_time_s_valid); + true_value = true_value(true_time_s_valid); + arma::uvec meas_time_s_valid = find(meas_time_s > 0); + meas_time_s = meas_time_s(meas_time_s_valid); + meas_value = meas_value(meas_time_s_valid); + + arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp); + + // 2. RMSE + arma::vec err; + + err = meas_value - true_value_interp; + arma::vec err2 = arma::square(err); + double rmse = sqrt(arma::mean(err2)); + + // 3. Mean err and variance + double error_mean = arma::mean(err); + double error_var = arma::var(err); + + // 4. Peaks + double max_error = arma::max(err); + double min_error = arma::min(err); + + // 5. report + std::streamsize ss = std::cout.precision(); + std::cout << std::setprecision(10) << "TRK code phase RMSE=" << rmse + << ", mean=" << error_mean + << ", stdev=" << sqrt(error_var) << " (max,min)=" << max_error << "," << min_error << " [Chips]" << std::endl; + std::cout.precision(ss); +} + + +TEST_F(GpsL1CAKfTrackingTest, ValidationOfResults) +{ + // Configure the signal generator + configure_generator(); + + // Generate signal raw signal samples and observations RINEX file + if (FLAGS_disable_generator == false) + { + generate_signal(); + } + + std::chrono::time_point start, end; + + configure_receiver(); + + // open true observables log file written by the simulator + tracking_true_obs_reader true_obs_data; + int test_satellite_PRN = FLAGS_test_satellite_PRN; + std::cout << "Testing satellite PRN=" << test_satellite_PRN << std::endl; + std::string true_obs_file = std::string("./gps_l1_ca_obs_prn"); + true_obs_file.append(std::to_string(test_satellite_PRN)); + true_obs_file.append(".dat"); + ASSERT_EQ(true_obs_data.open_obs_file(true_obs_file), true) << "Failure opening true observables file"; + + top_block = gr::make_top_block("Tracking test"); + + std::shared_ptr trk_ = factory->GetBlock(config, "Tracking_1C", implementation, 1, 1); + std::shared_ptr tracking = std::dynamic_pointer_cast(trk_); //std::make_shared(config.get(), "Tracking_1C", 1, 1); + + boost::shared_ptr msg_rx = GpsL1CAKfTrackingTest_msg_rx_make(); + + // load acquisition data based on the first epoch of the true observations + ASSERT_EQ(true_obs_data.read_binary_obs(), true) + << "Failure reading true tracking dump file." << std::endl + << "Maybe sat PRN #" + std::to_string(FLAGS_test_satellite_PRN) + + " is not available?"; + + // restart the epoch counter + true_obs_data.restart(); + + std::cout << "Initial Doppler [Hz]=" << true_obs_data.doppler_l1_hz << " Initial code delay [Chips]=" << true_obs_data.prn_delay_chips << std::endl; + gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; + gnss_synchro.Acq_doppler_hz = true_obs_data.doppler_l1_hz; + gnss_synchro.Acq_samplestamp_samples = 0; + + ASSERT_NO_THROW({ + tracking->set_channel(gnss_synchro.Channel_ID); + }) << "Failure setting channel."; + + ASSERT_NO_THROW({ + tracking->set_gnss_synchro(&gnss_synchro); + }) << "Failure setting gnss_synchro."; + + ASSERT_NO_THROW({ + tracking->connect(top_block); + }) << "Failure connecting tracking to the top_block."; + + ASSERT_NO_THROW({ + std::string file = "./" + filename_raw_data; + const char* file_name = file.c_str(); + gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false); + gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); + gr::blocks::null_sink::sptr sink = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); + top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); + top_block->connect(gr_interleaved_char_to_complex, 0, tracking->get_left_block(), 0); + top_block->connect(tracking->get_right_block(), 0, sink, 0); + top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + }) << "Failure connecting the blocks of tracking test."; + + tracking->start_tracking(); + + EXPECT_NO_THROW({ + start = std::chrono::system_clock::now(); + top_block->run(); // Start threads and wait + end = std::chrono::system_clock::now(); + }) << "Failure running the top_block."; + + // check results + // load the true values + long int nepoch = true_obs_data.num_epochs(); + std::cout << "True observation epochs=" << nepoch << std::endl; + + arma::vec true_timestamp_s = arma::zeros(nepoch, 1); + arma::vec true_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); + arma::vec true_Doppler_Hz = arma::zeros(nepoch, 1); + arma::vec true_prn_delay_chips = arma::zeros(nepoch, 1); + arma::vec true_tow_s = arma::zeros(nepoch, 1); + + long int epoch_counter = 0; + while (true_obs_data.read_binary_obs()) + { + true_timestamp_s(epoch_counter) = true_obs_data.signal_timestamp_s; + true_acc_carrier_phase_cycles(epoch_counter) = true_obs_data.acc_carrier_phase_cycles; + true_Doppler_Hz(epoch_counter) = true_obs_data.doppler_l1_hz; + true_prn_delay_chips(epoch_counter) = true_obs_data.prn_delay_chips; + true_tow_s(epoch_counter) = true_obs_data.tow; + epoch_counter++; + } + + //load the measured values + tracking_dump_reader trk_dump; + + ASSERT_EQ(trk_dump.open_obs_file(std::string("./tracking_ch_0.dat")), true) + << "Failure opening tracking dump file"; + + nepoch = trk_dump.num_epochs(); + std::cout << "Measured observation epochs=" << nepoch << std::endl; + + arma::vec trk_timestamp_s = arma::zeros(nepoch, 1); + arma::vec trk_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); + arma::vec trk_Doppler_Hz = arma::zeros(nepoch, 1); + arma::vec trk_prn_delay_chips = arma::zeros(nepoch, 1); + + std::vector prompt; + std::vector early; + std::vector late; + std::vector promptI; + std::vector promptQ; + + epoch_counter = 0; + while (trk_dump.read_binary_obs()) + { + trk_timestamp_s(epoch_counter) = static_cast(trk_dump.PRN_start_sample_count) / static_cast(baseband_sampling_freq); + trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI; + trk_Doppler_Hz(epoch_counter) = trk_dump.carrier_doppler_hz; + + double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast(baseband_sampling_freq), 1.0e-3) / 1.0e-3); + + trk_prn_delay_chips(epoch_counter) = delay_chips; + epoch_counter++; + prompt.push_back(trk_dump.abs_P); + early.push_back(trk_dump.abs_E); + late.push_back(trk_dump.abs_L); + promptI.push_back(trk_dump.prompt_I); + promptQ.push_back(trk_dump.prompt_Q); + } + + // Align initial measurements and cut the tracking pull-in transitory + double pull_in_offset_s = 1.0; + arma::uvec initial_meas_point = arma::find(trk_timestamp_s >= (true_timestamp_s(0) + pull_in_offset_s), 1, "first"); + + trk_timestamp_s = trk_timestamp_s.subvec(initial_meas_point(0), trk_timestamp_s.size() - 1); + trk_acc_carrier_phase_cycles = trk_acc_carrier_phase_cycles.subvec(initial_meas_point(0), trk_acc_carrier_phase_cycles.size() - 1); + trk_Doppler_Hz = trk_Doppler_Hz.subvec(initial_meas_point(0), trk_Doppler_Hz.size() - 1); + trk_prn_delay_chips = trk_prn_delay_chips.subvec(initial_meas_point(0), trk_prn_delay_chips.size() - 1); + + check_results_doppler(true_timestamp_s, true_Doppler_Hz, trk_timestamp_s, trk_Doppler_Hz); + check_results_codephase(true_timestamp_s, true_prn_delay_chips, trk_timestamp_s, trk_prn_delay_chips); + check_results_acc_carrier_phase(true_timestamp_s, true_acc_carrier_phase_cycles, trk_timestamp_s, trk_acc_carrier_phase_cycles); + + std::chrono::duration elapsed_seconds = end - start; + std::cout << "Signal tracking completed in " << elapsed_seconds.count() << " seconds." << std::endl; + + if (FLAGS_plot_gps_l1_kf_tracking_test == true) + { + const std::string gnuplot_executable(FLAGS_gnuplot_executable); + if (gnuplot_executable.empty()) + { + std::cout << "WARNING: Although the flag plot_gps_l1_tracking_test has been set to TRUE," << std::endl; + std::cout << "gnuplot has not been found in your system." << std::endl; + std::cout << "Test results will not be plotted." << std::endl; + } + else + { + try + { + boost::filesystem::path p(gnuplot_executable); + boost::filesystem::path dir = p.parent_path(); + std::string gnuplot_path = dir.native(); + Gnuplot::set_GNUPlotPath(gnuplot_path); + + std::vector timevec; + double t = 0.0; + for (auto it = prompt.begin(); it != prompt.end(); it++) + { + timevec.push_back(t); + t = t + GPS_L1_CA_CODE_PERIOD; + } + Gnuplot g1("linespoints"); + g1.set_title("GPS L1 C/A signal tracking correlators' output (satellite PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")"); + g1.set_grid(); + g1.set_xlabel("Time [s]"); + g1.set_ylabel("Correlators' output"); + g1.cmd("set key box opaque"); + unsigned int decimate = static_cast(FLAGS_plot_decimate); + g1.plot_xy(timevec, prompt, "Prompt", decimate); + g1.plot_xy(timevec, early, "Early", decimate); + g1.plot_xy(timevec, late, "Late", decimate); + g1.savetops("Correlators_outputs"); + g1.savetopdf("Correlators_outputs", 18); + g1.showonscreen(); // window output + + Gnuplot g2("points"); + g2.set_title("Constellation diagram (satellite PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")"); + g2.set_grid(); + g2.set_xlabel("Inphase"); + g2.set_ylabel("Quadrature"); + g2.cmd("set size ratio -1"); + g2.plot_xy(promptI, promptQ); + g2.savetops("Constellation"); + g2.savetopdf("Constellation", 18); + g2.showonscreen(); // window output + } + catch (const GnuplotException& ge) + { + std::cout << ge.what() << std::endl; + } + } + } +} From bd850eb5f32cacb960af1504297d4d5102616b75 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 12:04:03 +0200 Subject: [PATCH 03/23] Fix merge # Conflicts: # src/algorithms/tracking/adapters/CMakeLists.txt # src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt --- src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 5a427e8a6..5915c950f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -37,8 +37,12 @@ set(TRACKING_GR_BLOCKS_SOURCES glonass_l1_ca_dll_pll_tracking_cc.cc glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc - gps_l1_ca_kf_tracking_cc.cc - ${OPT_TRACKING_BLOCKS} + gps_l1_ca_kf_tracking_cc.cc + glonass_l2_ca_dll_pll_tracking_cc.cc + 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 + ${OPT_TRACKING_BLOCKS} ) include_directories( From 2b3de84750a3f8e5cbddfbc61508ae11616e1387 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 16:37:36 +0200 Subject: [PATCH 04/23] Save work on KF tracking --- .../gps_l1_ca_kf_tracking_cc.cc | 133 +++++++++++------- .../gps_l1_ca_kf_tracking_cc.h | 2 + 2 files changed, 81 insertions(+), 54 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index 4b70d7baa..2e603d028 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -164,6 +164,8 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( d_rem_code_phase_chips = 0.0; d_code_phase_step_chips = 0.0; d_carrier_phase_step_rad = 0.0; + code_error_chips = 0.0; + code_error_filt_chips = 0.0; set_relative_rate(1.0 / static_cast(d_vector_length)); @@ -177,7 +179,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( //covariances (static) double sigma2_carrier_phase = GPS_TWO_PI / 4; - double sigma2_doppler = 250; + double sigma2_doppler = 250; /// !! kf_P_x_ini = arma::zeros(2, 2); kf_P_x_ini(0, 0) = sigma2_carrier_phase; @@ -439,7 +441,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus // carrier phase step (NCO phase increment per sample) [rads/sample] d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); // carrier phase accumulator - d_acc_carrier_phase_rad = -kf_x(0); + d_acc_carrier_phase_rad -= kf_x(0); //################### DLL COMMANDS ################################################# // code phase step (Code resampler phase increment per sample) [chips/sample] @@ -513,6 +515,9 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + double tmp_float; double tmp_double; unsigned long int tmp_long; prompt_I = d_correlator_outs[1].real(); @@ -523,40 +528,44 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus try { // EPR + d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); // PROMPT I and Q (to analyze navigation symbols) d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp - tmp_long = d_sample_counter + d_current_prn_length_samples; - d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // PLL commands - d_dump_file.write(reinterpret_cast(&carr_phase_error_rad), sizeof(double)); - d_dump_file.write(reinterpret_cast(&carr_phase_error_filt_rad), sizeof(double)); - + tmp_float = 0.0; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = 0.0; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - + tmp_float = code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // AUX vars (for debug purposes) - tmp_double = d_rem_code_phase_samples; + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // PRN unsigned int prn_ = d_acquisition_gnss_synchro->PRN; d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); @@ -577,8 +586,8 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() { // READ DUMP FILE std::ifstream::pos_type size; - int number_of_double_vars = 11; - int number_of_float_vars = 5; + int number_of_double_vars = 1; + int number_of_float_vars = 17; int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(unsigned int); std::ifstream dump_file; @@ -604,22 +613,24 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() { return 1; } + float *abs_VE = new float[num_epoch]; float *abs_E = new float[num_epoch]; float *abs_P = new float[num_epoch]; float *abs_L = new float[num_epoch]; + float *abs_VL = new float[num_epoch]; float *Prompt_I = new float[num_epoch]; float *Prompt_Q = new float[num_epoch]; unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch]; - double *acc_carrier_phase_rad = new double[num_epoch]; - double *carrier_doppler_hz = new double[num_epoch]; - double *code_freq_chips = new double[num_epoch]; - double *carr_error_hz = new double[num_epoch]; - double *carr_error_filt_hz = new double[num_epoch]; - double *code_error_chips = new double[num_epoch]; - double *code_error_filt_chips = new double[num_epoch]; - double *CN0_SNV_dB_Hz = new double[num_epoch]; - double *carrier_lock_test = new double[num_epoch]; - double *aux1 = new double[num_epoch]; + float *acc_carrier_phase_rad = new float[num_epoch]; + float *carrier_doppler_hz = new float[num_epoch]; + float *code_freq_chips = new float[num_epoch]; + float *carr_error_hz = new float[num_epoch]; + float *carr_error_filt_hz = new float[num_epoch]; + float *code_error_chips = new float[num_epoch]; + float *code_error_filt_chips = new float[num_epoch]; + float *CN0_SNV_dB_Hz = new float[num_epoch]; + float *carrier_lock_test = new float[num_epoch]; + float *aux1 = new float[num_epoch]; double *aux2 = new double[num_epoch]; unsigned int *PRN = new unsigned int[num_epoch]; @@ -629,22 +640,24 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() { for (long int i = 0; i < num_epoch; i++) { + dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); - dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&aux1[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); } @@ -654,9 +667,11 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; + delete[] abs_VE; delete[] abs_E; delete[] abs_P; delete[] abs_L; + delete[] abs_VL; delete[] Prompt_I; delete[] Prompt_Q; delete[] PRN_start_sample_count; @@ -685,6 +700,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() if (reinterpret_cast(matfp) != NULL) { size_t dims[2] = {1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -697,6 +716,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -709,43 +732,43 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -758,9 +781,11 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() Mat_VarFree(matvar); } Mat_Close(matfp); + delete[] abs_VE; delete[] abs_E; delete[] abs_P; delete[] abs_L; + delete[] abs_VL; delete[] Prompt_I; delete[] Prompt_Q; delete[] PRN_start_sample_count; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index 9b2e66ff4..59b424918 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -149,6 +149,8 @@ private: double d_carrier_phase_step_rad; double d_acc_carrier_phase_rad; double d_code_phase_samples; + double code_error_chips; + double code_error_filt_chips; //PRN period in samples int d_current_prn_length_samples; From 6e19c0c63de004ec2972013611117ae883a1cc90 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 10 Apr 2018 10:52:19 +0200 Subject: [PATCH 05/23] Add work on the Kalman filter --- .../adapters/gps_l1_ca_kf_tracking.cc | 24 +- .../tracking/adapters/gps_l1_ca_kf_tracking.h | 33 +- .../gps_l1_ca_kf_tracking_cc.cc | 534 +++++++++--------- .../gps_l1_ca_kf_tracking_cc.h | 22 +- .../tracking/gps_l1_ca_kf_tracking_test.cc | 7 +- 5 files changed, 310 insertions(+), 310 deletions(-) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc index f41b0c4dc..d1a6d56d0 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -1,18 +1,20 @@ /*! * \file gps_l1_ca_kf_tracking.cc - * \brief Implementation of an adapter of a DLL+PLL tracking loop block - * for GPS L1 C/A to a TrackingInterface - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com - * Javier Arribas, 2011. jarribas(at)cttc.es + * \brief Implementation of an adapter of a DLL + Kalman carrier + * tracking loop block for GPS L1 C/A signals + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * \author Jordi Vila-Valls 2018. jvila(at)cttc.es + * \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es * - * Code DLL + carrier PLL according to the algorithms described in: - * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, - * A Software-Defined GPS and Galileo Receiver. A Single-Frequency - * Approach, Birkhauser, 2007 + * Reference: + * J. Vila-Valls, P. Closas, M. Navarro and C. Fernández-Prades, + * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital + * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine, + * Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260 * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -66,13 +68,11 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); f_if = configuration->property(role + ".if", 0); dump = configuration->property(role + ".dump", false); - //pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); - //if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h index 2b2839475..a687b8a9e 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h @@ -1,18 +1,20 @@ /*! * \file GPS_L1_CA_KF_Tracking.h - * \brief Interface of an adapter of a DLL+PLL tracking loop block - * for GPS L1 C/A to a TrackingInterface - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com - * Javier Arribas, 2011. jarribas(at)cttc.es + * \brief Interface of an adapter of a DLL + Kalman carrier + * tracking loop block for GPS L1 C/A signals + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * \author Jordi Vila-Valls 2018. jvila(at)cttc.es + * \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es * - * Code DLL + carrier PLL according to the algorithms described in: - * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, - * A Software-Defined GPS and Galileo Receiver. A Single-Frequency - * Approach, Birkhauser, 2007 + * Reference: + * J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades, + * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital + * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine, + * Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260 * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -38,10 +40,9 @@ #ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ #define GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ -#include -#include "tracking_interface.h" #include "gps_l1_ca_kf_tracking_cc.h" - +#include "tracking_interface.h" +#include class ConfigurationInterface; @@ -52,9 +53,9 @@ class GpsL1CaKfTracking : public TrackingInterface { public: GpsL1CaKfTracking(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams); + std::string role, + unsigned int in_streams, + unsigned int out_streams); virtual ~GpsL1CaKfTracking(); @@ -101,4 +102,4 @@ private: unsigned int out_streams_; }; -#endif // GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ +#endif // GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index 2e603d028..abb6717de 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -1,17 +1,20 @@ /*! * \file gps_l1_ca_kf_tracking_cc.cc - * \brief Implementation of a code DLL + carrier PLL tracking block - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com - * Javier Arribas, 2011. jarribas(at)cttc.es + * \brief Implementation of a processing block of a DLL + Kalman carrier + * tracking loop for GPS L1 C/A signals + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * \author Jordi Vila-Valls 2018. jvila(at)cttc.es + * \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es * - * Code DLL + carrier PLL according to the algorithms described in: - * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, - * A Software-Defined GPS and Galileo Receiver. A Single-Frequency - * Approach, Birkhauser, 2007 + * Reference: + * J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades, + * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital + * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine, + * Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260 * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -188,14 +191,10 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_R = arma::zeros(1, 1); kf_R(0, 0) = sigma2_phase_detector_cycles2; - //arma::colvec G={pow(GPS_L1_CA_CODE_PERIOD,3)/6.0, pow(GPS_L1_CA_CODE_PERIOD,2)/2.0,GPS_L1_CA_CODE_PERIOD}; kf_Q = arma::zeros(2, 2); - kf_Q(0, 0) = 1e-12; + kf_Q(0, 0) = 1e-14; kf_Q(1, 1) = 1e-2; - // kf_Q=arma::diagmat(pow(GPS_L1_CA_CODE_PERIOD,6)*kf_Q); - //std::cout<<"kf_Q="<(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); - // Doppler effect - // Fd=(C/(C+Vr))*F + // Doppler effect Fd = (C / (C + Vr)) * F double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ; // new chip and prn sequence periods based on acq Doppler double T_chip_mod_seconds; @@ -333,255 +331,6 @@ Gps_L1_Ca_Kf_Tracking_cc::~Gps_L1_Ca_Kf_Tracking_cc() } -int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) -{ - // process vars - double carr_phase_error_rad = 0.0; - double carr_phase_error_filt_rad = 0.0; - double code_error_chips = 0.0; - double code_error_filt_chips = 0.0; - - // Block input data and block output stream pointers - const gr_complex *in = reinterpret_cast(input_items[0]); - Gnss_Synchro **out = reinterpret_cast(&output_items[0]); - - // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder - Gnss_Synchro current_synchro_data = Gnss_Synchro(); - - if (d_enable_tracking == true) - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Receiver signal alignment - if (d_pull_in == true) - { - int samples_offset; - double acq_trk_shif_correction_samples; - int acq_to_trk_delay_samples; - acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; - d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples - d_pull_in = false; - // take into account the carrier cycles accumulated in the pull in signal alignment - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.fs = d_fs_in; - current_synchro_data.correlation_length_ms = 1; - *out[0] = current_synchro_data; - //Kalman filter initialization reset - kf_P_x = kf_P_x_ini; - //Update Kalman states based on acquisition information - kf_x(0) = d_carrier_phase_step_rad * samples_offset; - kf_x(1) = current_synchro_data.Carrier_Doppler_hz; - - consume_each(samples_offset); // shift input to perform alignment with local replica - return 1; - } - - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); - multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_current_prn_length_samples); - - // ################## Kalman Carrier Tracking ###################################### - - //Kalman state prediction (time update) - kf_x_pre = kf_F * kf_x; //state prediction - kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q; //state error covariance prediction - - // Update discriminator [rads/Ti] - carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output - - //Kalman estimation (measuremant update) - double sigma2_phase_detector_cycles2; - double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0); - sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)); - kf_R(0, 0) = sigma2_phase_detector_cycles2; - - kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix - kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain - - kf_y(0) = carr_phase_error_rad; // measurement vector - kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation - - kf_P_x = (arma::eye(2, 2) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix - - d_rem_carr_phase_rad = kf_x(0); // set a new carrier Phase estimation to the NCO - d_carrier_doppler_hz = kf_x(1); // set a new carrier Doppler estimation to the NCO - - carr_phase_error_filt_rad = d_rem_carr_phase_rad; - - // ################## DLL ########################################################## - // New code Doppler frequency estimation based on carrier frequency estimation - d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); - // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] early and late - // Code discriminator filter - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] - double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); - double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds); // [seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - - //################### NCO COMMANDS ################################################# - // carrier phase step (NCO phase increment per sample) [rads/sample] - d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - // carrier phase accumulator - d_acc_carrier_phase_rad -= kf_x(0); - - //################### DLL COMMANDS ################################################# - // code phase step (Code resampler phase increment per sample) [chips/sample] - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - // remnant code phase [chips] - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample - d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); - - // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < FLAGS_cn0_samples) - { - // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt - d_cn0_estimation_counter++; - } - else - { - d_cn0_estimation_counter = 0; - // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); - // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock - d_carrier_lock_fail_counter = 0; - d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - } - // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); - current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = 1; - } - else - { - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; - current_synchro_data.System = {'G'}; - current_synchro_data.correlation_length_ms = 1; - } - - // assign the GNU Radio block output data - current_synchro_data.fs = d_fs_in; - *out[0] = current_synchro_data; - - if (d_dump) - { - // MULTIPLEXED FILE RECORDING - Record results to file - float prompt_I; - float prompt_Q; - float tmp_E, tmp_P, tmp_L; - float tmp_VE = 0.0; - float tmp_VL = 0.0; - double tmp_float; - double tmp_double; - unsigned long int tmp_long; - prompt_I = d_correlator_outs[1].real(); - prompt_Q = d_correlator_outs[1].imag(); - tmp_E = std::abs(d_correlator_outs[0]); - tmp_P = std::abs(d_correlator_outs[1]); - tmp_L = std::abs(d_correlator_outs[2]); - try - { - // EPR - d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - tmp_float = d_acc_carrier_phase_rad; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // carrier and code frequency - tmp_float = d_carrier_doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_code_freq_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // PLL commands - tmp_float = 0.0; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = 0.0; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // DLL commands - tmp_float = code_error_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = code_error_filt_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // CN0 and carrier lock test - tmp_float = d_CN0_SNV_dB_Hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_carrier_lock_test; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // PRN - unsigned int prn_ = d_acquisition_gnss_synchro->PRN; - d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing trk dump file " << e.what(); - } - } - - consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates - d_sample_counter += d_current_prn_length_samples; // count for the processed samples - return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false -} - - int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() { // READ DUMP FILE @@ -700,7 +449,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() if (reinterpret_cast(matfp) != NULL) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -716,7 +465,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -807,12 +556,13 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() void Gps_L1_Ca_Kf_Tracking_cc::set_channel(unsigned int channel) { + gr::thread::scoped_lock l(d_setlock); d_channel = channel; LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) + if (d_dump) { - if (d_dump_file.is_open() == false) + if (!d_dump_file.is_open()) { try { @@ -835,3 +585,251 @@ void Gps_L1_Ca_Kf_Tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { d_acquisition_gnss_synchro = p_gnss_synchro; } + + +int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + // process vars + double carr_phase_error_rad = 0.0; + double code_error_chips = 0.0; + double code_error_filt_chips = 0.0; + + // Block input data and block output stream pointers + const gr_complex *in = reinterpret_cast(input_items[0]); + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); + + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + if (d_enable_tracking == true) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Receiver signal alignment + if (d_pull_in == true) + { + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + unsigned long int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + double acq_trk_shif_correction_samples = static_cast(d_current_prn_length_samples) - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + int samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + if (samples_offset < 0) + { + samples_offset = 0; + } + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_acq_code_phase_samples; + + d_sample_counter += samples_offset; // count for the processed samples + d_pull_in = false; + + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.fs = d_fs_in; + current_synchro_data.correlation_length_ms = 1; + *out[0] = current_synchro_data; + // Kalman filter initialization reset + kf_P_x = kf_P_x_ini; + // Update Kalman states based on acquisition information + kf_x(0) = d_carrier_phase_step_rad * samples_offset; + kf_x(1) = current_synchro_data.Carrier_Doppler_hz; + + consume_each(samples_offset); // shift input to perform alignment with local replica + return 1; + } + + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // Perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_current_prn_length_samples); + + // ################## Kalman Carrier Tracking ###################################### + + // Kalman state prediction (time update) + kf_x_pre = kf_F * kf_x; //state prediction + kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q; //state error covariance prediction + + // Update discriminator [rads/Ti] + carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output + + // Kalman estimation (measurement update) + double sigma2_phase_detector_cycles2; + double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0); + sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)); + kf_R(0, 0) = sigma2_phase_detector_cycles2; + + kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix + kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain + + kf_y(0) = carr_phase_error_rad; // measurement vector + kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation + + kf_P_x = (arma::eye(2, 2) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix + + d_rem_carr_phase_rad = kf_x(0); // set a new carrier Phase estimation to the NCO + d_carrier_doppler_hz = kf_x(1); // set a new carrier Doppler estimation to the NCO + + // ################## DLL ########################################################## + // New code Doppler frequency estimation based on carrier frequency estimation + d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); + // DLL discriminator + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] early and late + // Code discriminator filter + code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] + double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); + double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; + double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds); // [seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = static_cast(round(K_blk_samples)); // round to a discrete number of samples + + //################### NCO COMMANDS ################################################# + // carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / static_cast(d_fs_in); + // carrier phase accumulator + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + + //################### DLL COMMANDS ################################################# + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); // rounding error < 1 sample + d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); + + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < FLAGS_cn0_samples) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt + d_cn0_estimation_counter++; + } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_enable_tracking = false; + } + } + // ########### Output the tracking data to navigation and PVT ########## + current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = 1; + } + else + { + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0, 0); + } + + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.System = {'G'}; + current_synchro_data.correlation_length_ms = 1; + } + + // assign the GNU Radio block output data + current_synchro_data.fs = d_fs_in; + *out[0] = current_synchro_data; + + if (d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + float tmp_float; + double tmp_double; + unsigned long int tmp_long; + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); + try + { + // EPR + d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // carrier and code frequency + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // PLL commands + tmp_float = static_cast(carr_phase_error_rad * GPS_TWO_PI); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = static_cast(d_rem_carr_phase_rad * GPS_TWO_PI); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // CN0 and carrier lock test + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // AUX vars (for debug purposes) + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // PRN + unsigned int prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } + + consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates + d_sample_counter += d_current_prn_length_samples; // count for the processed samples + return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false +} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index 59b424918..3e33491af 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -1,18 +1,20 @@ /*! - * \file gps_l1_ca_dll_pll_tracking_cc.h - * \brief Interface of a code DLL + carrier PLL tracking block - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com - * Javier Arribas, 2011. jarribas(at)cttc.es - * Cillian O'Driscoll, 2017. cillian.odriscoll(at)gmail.com + * \file gps_l1_ca_kf_tracking_cc.cc + * \brief Interface of a processing block of a DLL + Kalman carrier + * tracking loop for GPS L1 C/A signals + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * \author Jordi Vila-Valls 2018. jvila(at)cttc.es + * \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es * - * Code DLL + carrier PLL according to the algorithms described in: - * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, - * A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, - * Birkhauser, 2007 + * Reference: + * J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades, + * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital + * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine, + * Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260 * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc index 2b44fc972..65e820921 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc @@ -7,7 +7,7 @@ * * ------------------------------------------------------------------------- * - * Copyright (C) 2012-2017 (see AUTHORS file for a list of contributors) + * Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -123,7 +123,7 @@ public: std::string p4; std::string p5; - std::string implementation = "GPS_L1_CA_KF_Tracking"; // "GPS_L1_CA_KF_Tracking"; + std::string implementation = "GPS_L1_CA_KF_Tracking"; const int baseband_sampling_freq = FLAGS_fs_gen_sps; @@ -251,7 +251,6 @@ void GpsL1CAKfTrackingTest::check_results_doppler(arma::vec& true_time_s, arma::uvec meas_time_s_valid = find(meas_time_s > 0); meas_time_s = meas_time_s(meas_time_s_valid); meas_value = meas_value(meas_time_s_valid); - arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp); // 2. RMSE @@ -464,6 +463,7 @@ TEST_F(GpsL1CAKfTrackingTest, ValidationOfResults) nepoch = trk_dump.num_epochs(); std::cout << "Measured observation epochs=" << nepoch << std::endl; + //trk_dump.restart(); arma::vec trk_timestamp_s = arma::zeros(nepoch, 1); arma::vec trk_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); @@ -482,7 +482,6 @@ TEST_F(GpsL1CAKfTrackingTest, ValidationOfResults) trk_timestamp_s(epoch_counter) = static_cast(trk_dump.PRN_start_sample_count) / static_cast(baseband_sampling_freq); trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI; trk_Doppler_Hz(epoch_counter) = trk_dump.carrier_doppler_hz; - double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast(baseband_sampling_freq), 1.0e-3) / 1.0e-3); trk_prn_delay_chips(epoch_counter) = delay_chips; From 584e214a5f6e243d7599f139a124798f53157630 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 7 May 2018 16:19:36 +0200 Subject: [PATCH 06/23] Update CN0 estimation call --- .../tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index abb6717de..cc05dde31 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -714,7 +714,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD); // Carrier lock indicator d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); // Loss of lock detection From d565d655212a6b01ff3200d79c94d58a5af31689 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Tue, 3 Jul 2018 13:31:53 -0400 Subject: [PATCH 07/23] Update KF tracking to use acquisition doppler bin size for initial doppler state covariance --- .../acquisition/gnuradio_blocks/pcps_acquisition.cc | 2 ++ .../gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc | 11 +++++++++-- .../gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h | 1 + src/core/system_parameters/gnss_synchro.h | 1 + 4 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index 3513a41da..f4aea64d8 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -222,6 +222,7 @@ void pcps_acquisition::init() d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0; @@ -282,6 +283,7 @@ void pcps_acquisition::set_state(int state) { d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; + d_gnss_synchro->Acq_doppler_step = 0; d_gnss_synchro->Acq_samplestamp_samples = 0; d_well_count = 0; d_mag = 0.0; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index cc05dde31..ee6d29e4f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -174,7 +174,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( // Kalman filter initialization (receiver initialization) - double CN_dB_Hz = 40; + double CN_dB_Hz = 30; double CN_lin = pow(10, CN_dB_Hz / 10.0); double sigma2_phase_detector_cycles2; @@ -182,7 +182,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( //covariances (static) double sigma2_carrier_phase = GPS_TWO_PI / 4; - double sigma2_doppler = 250; /// !! + double sigma2_doppler = 450; kf_P_x_ini = arma::zeros(2, 2); kf_P_x_ini(0, 0) = sigma2_carrier_phase; @@ -217,6 +217,13 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + d_acq_carrier_doppler_step_hz = static_cast(d_acquisition_gnss_synchro->Acq_doppler_step); + + // Correct Kalman filter covariance according to acq doppler step size (3 sigma) + if (d_acquisition_gnss_synchro->Acq_doppler_step > 0) + { + kf_P_x_ini(1, 1) = pow(2, d_acq_carrier_doppler_step_hz / 3.0); + } long int acq_trk_diff_samples; double acq_trk_diff_seconds; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index 3e33491af..0d4ed779d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -135,6 +135,7 @@ private: //Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition + double d_acq_carrier_doppler_step_hz; double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; // correlator diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 7d572dffa..90dd378d3 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -49,6 +49,7 @@ public: // Acquisition double Acq_delay_samples; //!< Set by Acquisition processing block double Acq_doppler_hz; //!< Set by Acquisition processing block + unsigned int Acq_doppler_step; //!< Set by Acquisition processing block unsigned long int Acq_samplestamp_samples; //!< Set by Acquisition processing block bool Flag_valid_acquisition; //!< Set by Acquisition processing block //Tracking From 0dd99e3c5d0242603039471eefa4c1476aee146a Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Tue, 3 Jul 2018 14:47:17 -0400 Subject: [PATCH 08/23] Add configurable model order to KF tracking (order 2, order 3) --- .../adapters/gps_l1_ca_kf_tracking.cc | 4 ++ .../gps_l1_ca_kf_tracking_cc.cc | 39 ++++++++++++++++--- .../gps_l1_ca_kf_tracking_cc.h | 10 +++-- 3 files changed, 45 insertions(+), 8 deletions(-) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc index d1a6d56d0..e124d411a 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -53,6 +53,7 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## + int order; int fs_in; int vector_length; int f_if; @@ -63,7 +64,9 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( float pll_bw_hz; float dll_bw_hz; float early_late_space_chips; + item_type = configuration->property(role + ".item_type", default_item_type); + order = configuration->property(role + ".order", 2); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); f_if = configuration->property(role + ".if", 0); @@ -80,6 +83,7 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( { item_size_ = sizeof(gr_complex); tracking_ = gps_l1_ca_kf_make_tracking_cc( + order, f_if, fs_in, vector_length, diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index ee6d29e4f..14d0bd16b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -59,6 +59,7 @@ using google::LogMessage; gps_l1_ca_kf_tracking_cc_sptr gps_l1_ca_kf_make_tracking_cc( + unsigned int order, long if_freq, long fs_in, unsigned int vector_length, @@ -67,7 +68,7 @@ gps_l1_ca_kf_make_tracking_cc( float dll_bw_hz, float early_late_space_chips) { - return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(if_freq, + return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(order, if_freq, fs_in, vector_length, dump, dump_filename, dll_bw_hz, early_late_space_chips)); } @@ -83,6 +84,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::forecast(int noutput_items, Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( + unsigned int order, long if_freq, long fs_in, unsigned int vector_length, @@ -97,6 +99,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( this->message_port_register_out(pmt::mp("events")); // initialize internal vars + d_order = order; d_dump = dump; d_if_freq = if_freq; d_fs_in = fs_in; @@ -182,7 +185,8 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( //covariances (static) double sigma2_carrier_phase = GPS_TWO_PI / 4; - double sigma2_doppler = 450; + double sigma2_doppler = 450; + double sigma2_doppler_rate = 1.0 / 24.0; kf_P_x_ini = arma::zeros(2, 2); kf_P_x_ini(0, 0) = sigma2_carrier_phase; @@ -192,8 +196,8 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_R(0, 0) = sigma2_phase_detector_cycles2; kf_Q = arma::zeros(2, 2); - kf_Q(0, 0) = 1e-14; - kf_Q(1, 1) = 1e-2; + kf_Q(0, 0) = pow(4, GPS_L1_CA_CODE_PERIOD); + kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD; kf_F = arma::zeros(2, 2); kf_F(0, 0) = 1.0; @@ -206,6 +210,31 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_x = arma::zeros(2, 1); kf_y = arma::zeros(1, 1); + + // order three + if (d_order == 3) + { + kf_P_x_ini = arma::resize(kf_P_x_ini, 3, 3); + kf_P_x_ini(2, 2) = sigma2_doppler_rate; + + kf_Q = arma::zeros(3, 3); + kf_Q(0, 0) = pow(6, GPS_L1_CA_CODE_PERIOD); + kf_Q(1, 1) = pow(4, GPS_L1_CA_CODE_PERIOD); + kf_Q(2, 2) = pow(2, GPS_L1_CA_CODE_PERIOD); + + kf_F = arma::resize(kf_F, 3, 3); + kf_F(0, 2) = 0.25 * GPS_TWO_PI * pow(2, GPS_L1_CA_CODE_PERIOD); + kf_F(1, 2) = GPS_L1_CA_CODE_PERIOD; + kf_F(2, 0) = 0.0; + kf_F(2, 1) = 0.0; + kf_F(2, 2) = 1.0; + + kf_H = arma::resize(kf_H, 1, 3); + kf_H(0, 2) = 0.0; + + kf_x = arma::resize(kf_x, 3, 1); + kf_x(2, 0) = -0.25; + } } @@ -674,7 +703,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_y(0) = carr_phase_error_rad; // measurement vector kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation - kf_P_x = (arma::eye(2, 2) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix + kf_P_x = (arma::eye(size(kf_P_x_pre)) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix d_rem_carr_phase_rad = kf_x(0); // set a new carrier Phase estimation to the NCO d_carrier_doppler_hz = kf_x(1); // set a new carrier Doppler estimation to the NCO diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index 0d4ed779d..12f80ac3d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -56,7 +56,8 @@ typedef boost::shared_ptr gps_l1_ca_kf_tracking_cc_sptr; gps_l1_ca_kf_tracking_cc_sptr -gps_l1_ca_kf_make_tracking_cc(long if_freq, +gps_l1_ca_kf_make_tracking_cc(unsigned int order, + long if_freq, long fs_in, unsigned int vector_length, bool dump, std::string dump_filename, @@ -83,14 +84,16 @@ public: private: friend gps_l1_ca_kf_tracking_cc_sptr - gps_l1_ca_kf_make_tracking_cc(long if_freq, + gps_l1_ca_kf_make_tracking_cc(unsigned int order, + long if_freq, long fs_in, unsigned int vector_length, bool dump, std::string dump_filename, float dll_bw_hz, float early_late_space_chips); - Gps_L1_Ca_Kf_Tracking_cc(long if_freq, + Gps_L1_Ca_Kf_Tracking_cc(unsigned int order, + long if_freq, long fs_in, unsigned int vector_length, bool dump, std::string dump_filename, @@ -98,6 +101,7 @@ private: float early_late_space_chips); // tracking configuration vars + unsigned int d_order; unsigned int d_vector_length; bool d_dump; From 486ac195db380a6c23b6d7f3770674d53bbbb9d3 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Mon, 9 Jul 2018 11:46:34 -0400 Subject: [PATCH 09/23] Add bayesian_estimation library to tracking/libs which will be used to perform noise adaptation to Kalman filter based tracking --- .../gps_l1_ca_kf_tracking_cc.h | 4 + src/algorithms/tracking/libs/CMakeLists.txt | 1 + .../tracking/libs/bayesian_estimation.cc | 167 ++++++++++++++++++ .../tracking/libs/bayesian_estimation.h | 86 +++++++++ 4 files changed, 258 insertions(+) create mode 100644 src/algorithms/tracking/libs/bayesian_estimation.cc create mode 100644 src/algorithms/tracking/libs/bayesian_estimation.h diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index 12f80ac3d..db690e52b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -49,6 +49,7 @@ #include "tracking_2nd_PLL_filter.h" #include #include "cpu_multicorrelator_real_codes.h" +#include "bayesian_estimation.h" class Gps_L1_Ca_Kf_Tracking_cc; @@ -133,6 +134,9 @@ private: arma::colvec kf_y_pre; //measurement vector arma::mat kf_K; //Kalman gain matrix + // Bayesian estimator + Bayesian_estimator cov_est; + // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index fdb35a4e9..209504767 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -44,6 +44,7 @@ set(TRACKING_LIB_SOURCES tracking_FLL_PLL_filter.cc tracking_loop_filter.cc dll_pll_conf.cc + bayesian_estimation.cc ) if(ENABLE_FPGA) diff --git a/src/algorithms/tracking/libs/bayesian_estimation.cc b/src/algorithms/tracking/libs/bayesian_estimation.cc new file mode 100644 index 000000000..abeb0b575 --- /dev/null +++ b/src/algorithms/tracking/libs/bayesian_estimation.cc @@ -0,0 +1,167 @@ +/*! + * \file bayesian_estimation.cc + * \brief Interface of a library with Bayesian noise statistic estimation + * + * Bayesian_estimator is a Bayesian estimator which attempts to estimate + * the properties of a stochastic process based on a sequence of + * discrete samples of the sequence. + * + * [1] TODO: Refs + * + * \authors
    + *
  • Gerald LaMountain, 2018. gerald(at)ece.neu.edu + *
  • Jordi Vila-Valls 2018. jvila(at)cttc.es + *
+ * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "bayesian_estimation.h" +#include + +Bayesian_estimator::Bayesian_estimator() +{ + kappa_prior = 0; + nu_prior = 0; +} + +Bayesian_estimator::Bayesian_estimator(int ny) +{ + mu_prior = arma::zeros(ny,1); + kappa_prior = 0; + nu_prior = 0; + Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); +} + +Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) +{ + mu_prior = mu_prior_0; + kappa_prior = kappa_prior_0; + nu_prior = nu_prior_0; + Psi_prior = Psi_prior_0; +} + +Bayesian_estimator::~Bayesian_estimator() +{ +} + +/* + * Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in + * the class structure, and update the priors according to the computed posteriors + */ +void Bayesian_estimator::update_sequential(arma::vec data) +{ + int K = data.n_cols; + int ny = data.n_rows; + + if (mu_prior.is_empty()) + { + mu_prior = arma::zeros(ny,1); + } + + if (Psi_prior.is_empty()) + { + Psi_prior = arma::zeros(ny,ny); + } + + arma::vec y_mean = arma::mean(data, 1); + arma::mat Psi_N = arma::zeros(ny, ny); + + for (int kk = 0; kk < K; kk++) + { + Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t()); + } + + arma::vec mu_posterior = (kappa_prior*mu_prior + K*y_mean) / (kappa_prior + K); + int kappa_posterior = kappa_prior + K; + int nu_posterior = nu_prior + K; + arma::mat Psi_posterior = Psi_prior + Psi_N + (kappa_prior*K)/(kappa_prior + K)*(y_mean - mu_prior)*((y_mean - mu_prior).t()); + + mu_est = mu_posterior; + if ((nu_posterior - ny - 1) > 0) + { + Psi_est = Psi_posterior / (nu_posterior - ny - 1); + } + else + { + Psi_est = Psi_posterior / (nu_posterior + ny + 1); + } + + mu_prior = mu_posterior; + kappa_prior = kappa_posterior; + nu_prior = nu_posterior; + Psi_prior = Psi_posterior; +} + + +/* + * Perform Bayesian noise estimation using a new set of normal-inverse-Wishart priors + * and update the priors according to the computed posteriors + */ +void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) +{ + + int K = data.n_cols; + int ny = data.n_rows; + + arma::vec y_mean = arma::mean(data, 1); + arma::mat Psi_N = arma::zeros(ny, ny); + + for (int kk = 0; kk < K; kk++) + { + Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t()); + } + + arma::vec mu_posterior = (kappa_prior_0*mu_prior_0 + K*y_mean) / (kappa_prior_0 + K); + int kappa_posterior = kappa_prior_0 + K; + int nu_posterior = nu_prior_0 + K; + arma::mat Psi_posterior = Psi_prior_0 + Psi_N + (kappa_prior_0*K)/(kappa_prior_0 + K)*(y_mean - mu_prior_0)*((y_mean - mu_prior_0).t()); + + mu_est = mu_posterior; + if ((nu_posterior - ny - 1) > 0) + { + Psi_est = Psi_posterior / (nu_posterior - ny - 1); + } + else + { + Psi_est = Psi_posterior / (nu_posterior + ny + 1); + } + + mu_prior = mu_posterior; + kappa_prior = kappa_posterior; + nu_prior = nu_posterior; + Psi_prior = Psi_posterior; + +} + +arma::vec Bayesian_estimator::get_mu_est() +{ + return mu_est; +} + +arma::mat Bayesian_estimator::get_Psi_est() +{ + return Psi_est; +} + diff --git a/src/algorithms/tracking/libs/bayesian_estimation.h b/src/algorithms/tracking/libs/bayesian_estimation.h new file mode 100644 index 000000000..5cc524e44 --- /dev/null +++ b/src/algorithms/tracking/libs/bayesian_estimation.h @@ -0,0 +1,86 @@ +/*! + * \file bayesian_estimation.h + * \brief Interface of a library with Bayesian noise statistic estimation + * + * Bayesian_estimator is a Bayesian estimator which attempts to estimate + * the properties of a stochastic process based on a sequence of + * discrete samples of the sequence. + * + * [1] TODO: Refs + * + * \authors
    + *
  • Gerald LaMountain, 2018. gerald(at)ece.neu.edu + *
  • Jordi Vila-Valls 2018. jvila(at)cttc.es + *
+ * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_BAYESIAN_ESTIMATION_H_ +#define GNSS_SDR_BAYESIAN_ESTIMATION_H_ + +#include +#include + +/*! \brief Bayesian_estimator is an estimator of noise characteristics (i.e. mean, covariance) + * + * Bayesian_estimator is an estimator which performs estimation of noise characteristics from + * a sequence of identically and independently distributed (IID) samples of a stationary + * stochastic process by way of Bayesian inference using conjugate priors. The posterior + * distribution is assumed to be Gaussian with mean \mathbf{\mu} and covariance \hat{\mathbf{C}}, + * which has a conjugate prior given by a normal-inverse-Wishart distribution with paramemters + * \mathbf{\mu}_{0}, \kappa_{0}, \nu_{0}, and \mathbf{\Psi}. + * + * [1] TODO: Ref1 + * + */ + +class Bayesian_estimator +{ + +public: + Bayesian_estimator(); + Bayesian_estimator(int ny); + Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); + ~Bayesian_estimator(); + + void update_sequential(arma::vec data); + void update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); + + arma::vec get_mu_est(); + arma::mat get_Psi_est(); + +private: + + arma::vec mu_est; + arma::mat Psi_est; + + arma::vec mu_prior; + int kappa_prior; + int nu_prior; + arma::mat Psi_prior; + +}; + +#endif From 16848069ee7dccd57f6a2a22d5b0f98e5ec182cd Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 20 Jul 2018 13:19:41 +0200 Subject: [PATCH 10/23] Fix building --- src/algorithms/tracking/adapters/CMakeLists.txt | 1 + src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt | 1 + src/algorithms/tracking/libs/CMakeLists.txt | 1 + 3 files changed, 3 insertions(+) diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index f956278c4..06d2c7dc2 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -50,6 +50,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/algorithms/tracking/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/tracking/libs ${CMAKE_SOURCE_DIR}/src/algorithms/libs + ${ARMADILLO_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} ${GNURADIO_RUNTIME_INCLUDE_DIRS} diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 4afe2b374..b1a67d6bb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -49,6 +49,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/tracking/libs ${CMAKE_SOURCE_DIR}/src/algorithms/libs + ${ARMADILLO_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 209504767..46f7ea370 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -57,6 +57,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/libs + ${ARMADILLO_INCLUDE_DIRS} ${VOLK_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} From 032e73e727fe8e17c788173531500b44fe048539 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Mon, 13 Aug 2018 21:15:58 -0400 Subject: [PATCH 11/23] Updates to 3-state Kalman filter parameters to improve tracking --- .../gps_l1_ca_kf_tracking_cc.cc | 50 ++++++++++++++----- .../gps_l1_ca_kf_tracking_cc.h | 1 + 2 files changed, 39 insertions(+), 12 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index 14d0bd16b..b3af2680f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -165,6 +165,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( d_acq_code_phase_samples = 0.0; d_acq_carrier_doppler_hz = 0.0; d_carrier_doppler_hz = 0.0; + d_carrier_dopplerrate_hz2 = 0.0; d_acc_carrier_phase_rad = 0.0; d_code_phase_samples = 0.0; d_rem_code_phase_chips = 0.0; @@ -186,7 +187,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( //covariances (static) double sigma2_carrier_phase = GPS_TWO_PI / 4; double sigma2_doppler = 450; - double sigma2_doppler_rate = 1.0 / 24.0; + double sigma2_doppler_rate = pow(4.0 * GPS_TWO_PI, 2) / 12.0; kf_P_x_ini = arma::zeros(2, 2); kf_P_x_ini(0, 0) = sigma2_carrier_phase; @@ -196,7 +197,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_R(0, 0) = sigma2_phase_detector_cycles2; kf_Q = arma::zeros(2, 2); - kf_Q(0, 0) = pow(4, GPS_L1_CA_CODE_PERIOD); + kf_Q(0, 0) = pow(GPS_L1_CA_CODE_PERIOD, 4); kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD; kf_F = arma::zeros(2, 2); @@ -210,6 +211,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_x = arma::zeros(2, 1); kf_y = arma::zeros(1, 1); + kf_P_y = arma::zeros(1, 1); // order three if (d_order == 3) @@ -218,12 +220,12 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_P_x_ini(2, 2) = sigma2_doppler_rate; kf_Q = arma::zeros(3, 3); - kf_Q(0, 0) = pow(6, GPS_L1_CA_CODE_PERIOD); - kf_Q(1, 1) = pow(4, GPS_L1_CA_CODE_PERIOD); - kf_Q(2, 2) = pow(2, GPS_L1_CA_CODE_PERIOD); + kf_Q(0, 0) = pow(GPS_L1_CA_CODE_PERIOD, 4); + kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD; + kf_Q(2, 2) = GPS_L1_CA_CODE_PERIOD; kf_F = arma::resize(kf_F, 3, 3); - kf_F(0, 2) = 0.25 * GPS_TWO_PI * pow(2, GPS_L1_CA_CODE_PERIOD); + kf_F(0, 2) = 0.5 * GPS_TWO_PI * pow(GPS_L1_CA_CODE_PERIOD, 2); kf_F(1, 2) = GPS_L1_CA_CODE_PERIOD; kf_F(2, 0) = 0.0; kf_F(2, 1) = 0.0; @@ -233,10 +235,10 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_H(0, 2) = 0.0; kf_x = arma::resize(kf_x, 3, 1); - kf_x(2, 0) = -0.25; + kf_x(2, 0) = 0.0; } -} +} void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() { @@ -251,7 +253,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() // Correct Kalman filter covariance according to acq doppler step size (3 sigma) if (d_acquisition_gnss_synchro->Acq_doppler_step > 0) { - kf_P_x_ini(1, 1) = pow(2, d_acq_carrier_doppler_step_hz / 3.0); + kf_P_x_ini(1, 1) = pow(d_acq_carrier_doppler_step_hz / 3.0, 2); } long int acq_trk_diff_samples; @@ -288,6 +290,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_acq_code_phase_samples = corrected_acq_phase_samples; d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_dopplerrate_hz2 = 0; d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); // DLL filter initialization @@ -372,7 +375,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() // READ DUMP FILE std::ifstream::pos_type size; int number_of_double_vars = 1; - int number_of_float_vars = 17; + int number_of_float_vars = 18; int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(unsigned int); std::ifstream dump_file; @@ -408,6 +411,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch]; float *acc_carrier_phase_rad = new float[num_epoch]; float *carrier_doppler_hz = new float[num_epoch]; + float *carrier_dopplerrate_hz2 = new float[num_epoch]; float *code_freq_chips = new float[num_epoch]; float *carr_error_hz = new float[num_epoch]; float *carr_error_filt_hz = new float[num_epoch]; @@ -435,6 +439,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_dopplerrate_hz2[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); @@ -462,6 +467,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() delete[] PRN_start_sample_count; delete[] acc_carrier_phase_rad; delete[] carrier_doppler_hz; + delete[] carrier_dopplerrate_hz2; delete[] code_freq_chips; delete[] carr_error_hz; delete[] carr_error_filt_hz; @@ -525,6 +531,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("carrier_dopplerrate_hz2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_dopplerrate_hz2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -576,6 +586,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() delete[] PRN_start_sample_count; delete[] acc_carrier_phase_rad; delete[] carrier_doppler_hz; + delete[] carrier_dopplerrate_hz2; delete[] code_freq_chips; delete[] carr_error_hz; delete[] carr_error_filt_hz; @@ -667,7 +678,11 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_P_x = kf_P_x_ini; // Update Kalman states based on acquisition information kf_x(0) = d_carrier_phase_step_rad * samples_offset; - kf_x(1) = current_synchro_data.Carrier_Doppler_hz; + kf_x(1) = d_carrier_doppler_hz; + if (kf_x.n_elem > 2) + { + kf_x(2) = d_carrier_dopplerrate_hz2; + } consume_each(samples_offset); // shift input to perform alignment with local replica return 1; @@ -705,8 +720,17 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_P_x = (arma::eye(size(kf_P_x_pre)) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix + // Store Kalman filter results d_rem_carr_phase_rad = kf_x(0); // set a new carrier Phase estimation to the NCO d_carrier_doppler_hz = kf_x(1); // set a new carrier Doppler estimation to the NCO + if (kf_x.n_elem > 2) + { + d_carrier_dopplerrate_hz2 = kf_x(2); + } + else + { + d_carrier_dopplerrate_hz2 = 0; + } // ################## DLL ########################################################## // New code Doppler frequency estimation based on carrier frequency estimation @@ -833,9 +857,11 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus // carrier and code frequency tmp_float = d_carrier_doppler_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_dopplerrate_hz2; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_code_freq_chips; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // PLL commands + // Kalman commands tmp_float = static_cast(carr_phase_error_rad * GPS_TWO_PI); d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = static_cast(d_rem_carr_phase_rad * GPS_TWO_PI); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index db690e52b..e6b0e87ce 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -157,6 +157,7 @@ private: double d_code_freq_chips; double d_code_phase_step_chips; double d_carrier_doppler_hz; + double d_carrier_dopplerrate_hz2; double d_carrier_phase_step_rad; double d_acc_carrier_phase_rad; double d_code_phase_samples; From e42467a06868561449328d926a75057aee46c431 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Mon, 13 Aug 2018 21:18:01 -0400 Subject: [PATCH 12/23] Updates to integration of bayesian_estimation library into GPS L1 kalman tracking block --- .../adapters/gps_l1_ca_kf_tracking.cc | 18 +++- .../gps_l1_ca_kf_tracking_cc.cc | 86 ++++++++++++++++--- .../gps_l1_ca_kf_tracking_cc.h | 35 ++++++-- .../tracking/libs/bayesian_estimation.cc | 26 +++++- .../tracking/libs/bayesian_estimation.h | 5 +- 5 files changed, 148 insertions(+), 22 deletions(-) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc index e124d411a..4c06d5580 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -64,6 +64,11 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( float pll_bw_hz; float dll_bw_hz; float early_late_space_chips; + bool bce_run; + unsigned int bce_ptrans; + unsigned int bce_strans; + int bce_nu; + int bce_kappa; item_type = configuration->property(role + ".item_type", default_item_type); order = configuration->property(role + ".order", 2); @@ -78,6 +83,12 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); + bce_run = configuration->property(role + ".bce_run", false); + bce_ptrans = configuration->property(role + ".p_transient", 0); + bce_strans = configuration->property(role + ".s_transient", 0); + bce_nu = configuration->property(role + ".bce_nu", 0); + bce_kappa = configuration->property(role + ".bce_kappa", 0); + //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { @@ -90,7 +101,12 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( dump, dump_filename, dll_bw_hz, - early_late_space_chips); + early_late_space_chips, + bce_run, + bce_ptrans, + bce_strans, + bce_nu, + bce_kappa); } else { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index b3af2680f..0f6b3d9d5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -66,10 +66,16 @@ gps_l1_ca_kf_make_tracking_cc( bool dump, std::string dump_filename, float dll_bw_hz, - float early_late_space_chips) + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa) { return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(order, if_freq, - fs_in, vector_length, dump, dump_filename, dll_bw_hz, early_late_space_chips)); + fs_in, vector_length, dump, dump_filename, dll_bw_hz, early_late_space_chips, + bce_run, bce_ptrans, bce_strans, bce_nu, bce_kappa)); } @@ -91,7 +97,12 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( bool dump, std::string dump_filename, float dll_bw_hz, - float early_late_space_chips) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input @@ -140,6 +151,8 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( d_rem_code_phase_samples = 0.0; // define residual carrier phase d_rem_carr_phase_rad = 0.0; + // define residual carrier phase covariance + d_carr_phase_sigma2 = 0.0; // sample synchronization d_sample_counter = 0; @@ -238,6 +251,17 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_x(2, 0) = 0.0; } + // Bayesian covariance estimator initialization + kf_iter = 0; + bayes_run = bce_run; + bayes_ptrans = bce_ptrans; + bayes_strans = bce_strans; + + bayes_kappa = bce_kappa; + bayes_nu = bce_nu; + kf_R_est = kf_R; + + bayes_estimator.init(arma::zeros(1,1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R)*(bayes_nu + 2)); } void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() @@ -254,6 +278,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() if (d_acquisition_gnss_synchro->Acq_doppler_step > 0) { kf_P_x_ini(1, 1) = pow(d_acq_carrier_doppler_step_hz / 3.0, 2); + bayes_estimator.init(arma::zeros(1,1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R)*(bayes_nu + 2)); } long int acq_trk_diff_samples; @@ -310,6 +335,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_rem_carr_phase_rad = 0.0; d_rem_code_phase_chips = 0.0; d_acc_carrier_phase_rad = 0.0; + d_carr_phase_sigma2 = 0.0; d_code_phase_samples = d_acq_code_phase_samples; @@ -375,7 +401,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() // READ DUMP FILE std::ifstream::pos_type size; int number_of_double_vars = 1; - int number_of_float_vars = 18; + int number_of_float_vars = 19; int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(unsigned int); std::ifstream dump_file; @@ -414,6 +440,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() float *carrier_dopplerrate_hz2 = new float[num_epoch]; float *code_freq_chips = new float[num_epoch]; float *carr_error_hz = new float[num_epoch]; + float *carr_noise_sigma2 = new float[num_epoch]; float *carr_error_filt_hz = new float[num_epoch]; float *code_error_chips = new float[num_epoch]; float *code_error_filt_chips = new float[num_epoch]; @@ -442,6 +469,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() dump_file.read(reinterpret_cast(&carrier_dopplerrate_hz2[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_noise_sigma2[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); @@ -470,6 +498,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() delete[] carrier_dopplerrate_hz2; delete[] code_freq_chips; delete[] carr_error_hz; + delete[] carr_noise_sigma2; delete[] carr_error_filt_hz; delete[] code_error_chips; delete[] code_error_filt_chips; @@ -543,6 +572,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("carr_noise_sigma2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_noise_sigma2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -589,6 +622,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() delete[] carrier_dopplerrate_hz2; delete[] code_freq_chips; delete[] carr_error_hz; + delete[] carr_noise_sigma2; delete[] carr_error_filt_hz; delete[] code_error_chips; delete[] code_error_filt_chips; @@ -638,7 +672,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - double carr_phase_error_rad = 0.0; + d_carr_phase_error_rad = 0.0; double code_error_chips = 0.0; double code_error_filt_chips = 0.0; @@ -684,6 +718,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_x(2) = d_carrier_dopplerrate_hz2; } + // Covariance estimation initialization reset + kf_iter = 0; + bayes_estimator.init(arma::zeros(1,1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R)*(bayes_nu + 2)); + consume_each(samples_offset); // shift input to perform alignment with local replica return 1; } @@ -704,20 +742,35 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q; //state error covariance prediction // Update discriminator [rads/Ti] - carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output + d_carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output // Kalman estimation (measurement update) double sigma2_phase_detector_cycles2; double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0); sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)); + + kf_y(0) = d_carr_phase_error_rad; // measurement vector kf_R(0, 0) = sigma2_phase_detector_cycles2; - kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix - kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain - - kf_y(0) = carr_phase_error_rad; // measurement vector - kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation + if (bayes_run && (kf_iter >= bayes_ptrans)) + { + bayes_estimator.update_sequential(kf_y); + } + if (bayes_run && (kf_iter >= (bayes_ptrans + bayes_strans))) + { + // TODO: Resolve segmentation fault + kf_P_y = bayes_estimator.get_Psi_est(); + kf_R_est = kf_P_y - kf_H * kf_P_x_pre * kf_H.t(); + } + else + { + kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix + kf_R_est = kf_R; + } + // Kalman filter update step + kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain + kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation kf_P_x = (arma::eye(size(kf_P_x_pre)) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix // Store Kalman filter results @@ -731,6 +784,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus { d_carrier_dopplerrate_hz2 = 0; } + d_carr_phase_sigma2 = kf_R_est(0, 0); // ################## DLL ########################################################## // New code Doppler frequency estimation based on carrier frequency estimation @@ -780,7 +834,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { + //if (d_channel == 1) + //std::cout << "Carrier Lock Test Fail in channel " << d_channel << ": " << d_carrier_lock_test << " < " << d_carrier_lock_threshold << "," << nfail++ << std::endl; d_carrier_lock_fail_counter++; + //nfail++; } else { @@ -805,6 +862,9 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.correlation_length_ms = 1; + + kf_iter++; + } else { @@ -862,7 +922,9 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus tmp_float = d_code_freq_chips; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // Kalman commands - tmp_float = static_cast(carr_phase_error_rad * GPS_TWO_PI); + tmp_float = static_cast(d_carr_phase_error_rad * GPS_TWO_PI); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = static_cast(d_carr_phase_sigma2); d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = static_cast(d_rem_carr_phase_rad * GPS_TWO_PI); d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index e6b0e87ce..718613820 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -63,7 +63,12 @@ gps_l1_ca_kf_make_tracking_cc(unsigned int order, bool dump, std::string dump_filename, float pll_bw_hz, - float early_late_space_chips); + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa); /*! @@ -91,7 +96,12 @@ private: bool dump, std::string dump_filename, float dll_bw_hz, - float early_late_space_chips); + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa); Gps_L1_Ca_Kf_Tracking_cc(unsigned int order, long if_freq, @@ -99,7 +109,12 @@ private: bool dump, std::string dump_filename, float dll_bw_hz, - float early_late_space_chips); + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa); // tracking configuration vars unsigned int d_order; @@ -124,19 +139,27 @@ private: arma::mat kf_P_x; //state error covariance matrix arma::mat kf_P_x_pre; //Predicted state error covariance matrix arma::mat kf_P_y; //innovation covariance matrix + arma::mat kf_F; //state transition matrix arma::mat kf_H; //system matrix arma::mat kf_R; //measurement error covariance matrix arma::mat kf_Q; //system error covariance matrix + arma::colvec kf_x; //state vector arma::colvec kf_x_pre; //predicted state vector arma::colvec kf_y; //measurement vector - arma::colvec kf_y_pre; //measurement vector arma::mat kf_K; //Kalman gain matrix // Bayesian estimator - Bayesian_estimator cov_est; + Bayesian_estimator bayes_estimator; + arma::mat kf_R_est; //measurement error covariance + unsigned int bayes_ptrans; + unsigned int bayes_strans; + int bayes_nu; + int bayes_kappa; + bool bayes_run; + unsigned int kf_iter; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; @@ -160,6 +183,8 @@ private: double d_carrier_dopplerrate_hz2; double d_carrier_phase_step_rad; double d_acc_carrier_phase_rad; + double d_carr_phase_error_rad; + double d_carr_phase_sigma2; double d_code_phase_samples; double code_error_chips; double code_error_filt_chips; diff --git a/src/algorithms/tracking/libs/bayesian_estimation.cc b/src/algorithms/tracking/libs/bayesian_estimation.cc index abeb0b575..ed1065d44 100644 --- a/src/algorithms/tracking/libs/bayesian_estimation.cc +++ b/src/algorithms/tracking/libs/bayesian_estimation.cc @@ -42,8 +42,14 @@ Bayesian_estimator::Bayesian_estimator() { + int ny = 1; + mu_prior = arma::zeros(ny,1); kappa_prior = 0; nu_prior = 0; + Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); + + mu_est = mu_prior; + Psi_est = Psi_prior; } Bayesian_estimator::Bayesian_estimator(int ny) @@ -52,6 +58,9 @@ Bayesian_estimator::Bayesian_estimator(int ny) kappa_prior = 0; nu_prior = 0; Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); + + mu_est = mu_prior; + Psi_est = Psi_prior; } Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) @@ -60,12 +69,26 @@ Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, kappa_prior = kappa_prior_0; nu_prior = nu_prior_0; Psi_prior = Psi_prior_0; + + mu_est = mu_prior; + Psi_est = Psi_prior; } Bayesian_estimator::~Bayesian_estimator() { } +void Bayesian_estimator::init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) +{ + mu_prior = mu_prior_0; + kappa_prior = kappa_prior_0; + nu_prior = nu_prior_0; + Psi_prior = Psi_prior_0; + + mu_est = mu_prior; + Psi_est = Psi_prior; +} + /* * Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in * the class structure, and update the priors according to the computed posteriors @@ -152,10 +175,9 @@ void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0, kappa_prior = kappa_posterior; nu_prior = nu_posterior; Psi_prior = Psi_posterior; - } -arma::vec Bayesian_estimator::get_mu_est() +arma::mat Bayesian_estimator::get_mu_est() { return mu_est; } diff --git a/src/algorithms/tracking/libs/bayesian_estimation.h b/src/algorithms/tracking/libs/bayesian_estimation.h index 5cc524e44..44370cacb 100644 --- a/src/algorithms/tracking/libs/bayesian_estimation.h +++ b/src/algorithms/tracking/libs/bayesian_estimation.h @@ -65,14 +65,15 @@ public: Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); ~Bayesian_estimator(); + void init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); + void update_sequential(arma::vec data); void update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); - arma::vec get_mu_est(); + arma::mat get_mu_est(); arma::mat get_Psi_est(); private: - arma::vec mu_est; arma::mat Psi_est; From 0fd98b037920ba3b20e3a9541ee80649416409c7 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Mon, 13 Aug 2018 21:21:24 -0400 Subject: [PATCH 13/23] Add Matlab parsing and plotting functions for Kalman filter tracking block --- conf/gnss-sdr-kalman-bayes.conf | 63 +++++++ src/utils/matlab/gps_l1_ca_kf_plot_sample.m | 93 +++++++++++ .../libs/gps_l1_ca_kf_read_tracking_dump.m | 158 ++++++++++++++++++ src/utils/matlab/libs/plotKalman.m | 135 +++++++++++++++ 4 files changed, 449 insertions(+) create mode 100644 conf/gnss-sdr-kalman-bayes.conf create mode 100644 src/utils/matlab/gps_l1_ca_kf_plot_sample.m create mode 100644 src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m create mode 100644 src/utils/matlab/libs/plotKalman.m diff --git a/conf/gnss-sdr-kalman-bayes.conf b/conf/gnss-sdr-kalman-bayes.conf new file mode 100644 index 000000000..051d080de --- /dev/null +++ b/conf/gnss-sdr-kalman-bayes.conf @@ -0,0 +1,63 @@ +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second]. +GNSS-SDR.internal_fs_sps=2000000 +GNSS-SDR.internal_fs_hz=2000000 + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=File_Signal_Source +SignalSource.filename=/home/glamountain/gnss-sdr/data/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN.dat +SignalSource.item_type=ishort +SignalSource.sampling_frequency=4000000 +SignalSource.freq=1575420000 +SignalSource.samples=0 + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner +DataTypeAdapter.implementation=Ishort_To_Complex +InputFilter.implementation=Pass_Through +InputFilter.item_type=gr_complex +Resampler.implementation=Direct_Resampler +Resampler.sample_freq_in=4000000 +Resampler.sample_freq_out=2000000 +Resampler.item_type=gr_complex + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1C.count=8 +Channels.in_acquisition=1 +Channel.signal=1C + +;######### ACQUISITION GLOBAL CONFIG ############ +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.item_type=gr_complex +Acquisition_1C.threshold=0.008 +Acquisition_1C.doppler_max=10000 +Acquisition_1C.doppler_step=250 +Acquisition_1C.dump=false +Acquisition_1C.dump_filename=../data/kalman/acq_dump + +;######### TRACKING GLOBAL CONFIG ############ +Tracking_1C.implementation=GPS_L1_CA_KF_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.pll_bw_hz=40.0; +Tracking_1C.dll_bw_hz=4.0; +Tracking_1C.order=3; +Tracking_1C.dump=true +Tracking_1C.dump_filename=../data/kalman/epl_tracking_ch_ +Tracking_1C.bce_run = true; +Tracking_1C.p_transient = 0; +Tracking_1C.s_transient = 100; + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder + +;######### OBSERVABLES CONFIG ############ +Observables.implementation=GPS_L1_CA_Observables + +;######### PVT CONFIG ############ +PVT.implementation=GPS_L1_CA_PVT +PVT.averaging_depth=100 +PVT.flag_averaging=true +PVT.output_rate_ms=10 +PVT.display_rate_ms=500 diff --git a/src/utils/matlab/gps_l1_ca_kf_plot_sample.m b/src/utils/matlab/gps_l1_ca_kf_plot_sample.m new file mode 100644 index 000000000..f8fcb0ed7 --- /dev/null +++ b/src/utils/matlab/gps_l1_ca_kf_plot_sample.m @@ -0,0 +1,93 @@ +% Reads GNSS-SDR Tracking dump binary file using the provided +% function and plots some internal variables +% Javier Arribas, 2011. jarribas(at)cttc.es +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (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. +% +% GNSS-SDR is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +close all; +clear all; + +if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') + addpath('./libs') +end + + +samplingFreq = 6625000; %[Hz] +channels = 8; +first_channel = 0; +code_period = 0.001; + +path = '/archive/'; %% CHANGE THIS PATH +figpath = [path]; + +for N=1:1:channels + tracking_log_path = [path 'epl_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename + GNSS_tracking(N) = gps_l1_ca_kf_read_tracking_dump(tracking_log_path); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).carrFreqRate = GNSS_tracking(N).carrier_dopplerrate_hz2.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; + trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; + + trackResults(N).I_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).PRN = GNSS_tracking(N).PRN.'; + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + + kalmanResults(N).PRN = GNSS_tracking(N).PRN.'; + kalmanResults(N).innovation = GNSS_tracking(N).carr_error.'; + kalmanResults(N).state1 = GNSS_tracking(N).carr_nco.'; + kalmanResults(N).state2 = GNSS_tracking(N).carrier_doppler_hz.'; + kalmanResults(N).state3 = GNSS_tracking(N).carrier_dopplerrate_hz2.'; + kalmanResults(N).r_noise_cov = GNSS_tracking(N).carr_noise_sigma2.'; + kalmanResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E); + settings.codePeriod = code_period; + settings.timeStartInSeconds = 20; + + %plotTracking(N, trackResults, settings) + plotKalman(N, kalmanResults, settings) + + saveas(gcf, [figpath 'epl_tracking_ch_' num2str(N) '_PRN_' num2str(trackResults(N).PRN(end)) '.png'], 'png') +end + + diff --git a/src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m b/src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m new file mode 100644 index 000000000..e1f1c8687 --- /dev/null +++ b/src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m @@ -0,0 +1,158 @@ +% Usage: gps_l1_ca_kf_read_tracking_dump (filename, [count]) +% +% Opens GNSS-SDR tracking binary log file .dat and returns the contents + +% Read GNSS-SDR Tracking dump binary file into MATLAB. +% Javier Arribas, 2011. jarribas(at)cttc.es +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (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. +% +% GNSS-SDR is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +function [GNSS_tracking] = gps_l1_ca_kf_read_tracking_dump (filename, count) + +m = nargchk (1,2,nargin); + +num_float_vars = 19; +num_unsigned_long_int_vars = 1; +num_double_vars = 1; +num_unsigned_int_vars = 1; + +if(~isempty(strfind(computer('arch'), '64'))) + % 64-bit computer + double_size_bytes = 8; + unsigned_long_int_size_bytes = 8; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; +else + double_size_bytes = 8; + unsigned_long_int_size_bytes = 4; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; +end + +skip_bytes_each_read = float_size_bytes * num_float_vars + unsigned_long_int_size_bytes * num_unsigned_long_int_vars + ... + double_size_bytes * num_double_vars + num_unsigned_int_vars*unsigned_int_size_bytes; + +bytes_shift = 0; + +if (m) + usage (m); +end + +if (nargin < 2) + count = Inf; +end +%loops_counter = fread (f, count, 'uint32',4*12); +f = fopen (filename, 'rb'); +if (f < 0) +else + v1 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v2 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v3 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v4 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v5 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v6 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v7 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v8 = fread (f, count, 'long', skip_bytes_each_read - unsigned_long_int_size_bytes); + bytes_shift = bytes_shift + unsigned_long_int_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v9 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v10 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v11 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v12 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v13 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v14 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v15 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v18 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v19 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v20 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next double + v21 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes); + bytes_shift = bytes_shift + double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next unsigned int + v22 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes); + fclose (f); + + GNSS_tracking.VE = v1; + GNSS_tracking.E = v2; + GNSS_tracking.P = v3; + GNSS_tracking.L = v4; + GNSS_tracking.VL = v5; + GNSS_tracking.prompt_I = v6; + GNSS_tracking.prompt_Q = v7; + GNSS_tracking.PRN_start_sample = v8; + GNSS_tracking.acc_carrier_phase_rad = v9; + GNSS_tracking.carrier_doppler_hz = v10; + GNSS_tracking.carrier_dopplerrate_hz2 = v11; + GNSS_tracking.code_freq_hz = v12; + GNSS_tracking.carr_error = v13; + GNSS_tracking.carr_noise_sigma2 = v14; + GNSS_tracking.carr_nco = v15; + GNSS_tracking.code_error = v16; + GNSS_tracking.code_nco = v17; + GNSS_tracking.CN0_SNV_dB_Hz = v18; + GNSS_tracking.carrier_lock_test = v19; + GNSS_tracking.var1 = v20; + GNSS_tracking.var2 = v21; + GNSS_tracking.PRN = v22; +end \ No newline at end of file diff --git a/src/utils/matlab/libs/plotKalman.m b/src/utils/matlab/libs/plotKalman.m new file mode 100644 index 000000000..e05fd0d52 --- /dev/null +++ b/src/utils/matlab/libs/plotKalman.m @@ -0,0 +1,135 @@ +function plotKalman(channelList, trackResults, settings) +% This function plots the tracking results for the given channel list. +% +% plotTracking(channelList, trackResults, settings) +% +% Inputs: +% channelList - list of channels to be plotted. +% trackResults - tracking results from the tracking function. +% settings - receiver settings. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% Protection - if the list contains incorrect channel numbers +channelList = intersect(channelList, 1:settings.numberOfChannels); + +%=== For all listed channels ============================================== +for channelNr = channelList + + %% Select (or create) and clear the figure ================================ + % The number 200 is added just for more convenient handling of the open + % figure windows, when many figures are closed and reopened. + % Figures drawn or opened by the user, will not be "overwritten" by + % this function. + + figure(channelNr +200); + clf(channelNr +200); + set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... + ' (PRN ', ... + num2str(trackResults(channelNr).PRN(end-1)), ... + ') results']); + + timeStart = settings.timeStartInSeconds; + + %% Draw axes ============================================================== + % Row 1 + handles(1, 1) = subplot(4, 2, 1); + handles(1, 2) = subplot(4, 2, 2); + % Row 2 + handles(2, 1) = subplot(4, 2, 3); + handles(2, 2) = subplot(4, 2, 4); + % Row 3 + handles(3, 1) = subplot(4, 2, [5 6]); + % Row 4 + handles(4, 1) = subplot(4, 2, [7 8]); + + %% Plot all figures ======================================================= + + timeAxisInSeconds = (1:settings.msToProcess)/1000; + + %----- CNo for signal---------------------------------- + plot (handles(1, 1), timeAxisInSeconds, ... + trackResults(channelNr).CNo(1:settings.msToProcess), 'b'); + + grid (handles(1, 1)); + axis (handles(1, 1), 'tight'); + xlabel(handles(1, 1), 'Time (s)'); + ylabel(handles(1, 1), 'CNo (dB-Hz)'); + title (handles(1, 1), 'Carrier to Noise Ratio'); + + %----- PLL discriminator filtered---------------------------------- + plot (handles(1, 2), timeAxisInSeconds, ... + trackResults(channelNr).state1(1:settings.msToProcess), 'b'); + + grid (handles(1, 2)); + axis (handles(1, 2), 'tight'); + xlim (handles(1, 2), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(1, 2), 'Time (s)'); + ylabel(handles(1, 2), 'Phase Amplitude'); + title (handles(1, 2), 'Filtered Carrier Phase'); + + %----- Carrier Frequency -------------------------------- + plot (handles(2, 1), timeAxisInSeconds(2:end), ... + trackResults(channelNr).state2(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]); + + grid (handles(2, 1)); + axis (handles(2, 1)); + xlim (handles(2, 1), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(2, 1), 'Time (s)'); + ylabel(handles(2, 1), 'Freq (hz)'); + title (handles(2, 1), 'Filtered Doppler Frequency'); + + %----- Carrier Frequency Rate -------------------------------- + plot (handles(2, 2), timeAxisInSeconds(2:end), ... + trackResults(channelNr).state3(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]); + + grid (handles(2, 2)); + axis (handles(2, 2)); + xlim (handles(2, 2), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(2, 2), 'Time (s)'); + ylabel(handles(2, 2), 'Freq (hz)'); + title (handles(2, 2), 'Filtered Doppler Frequency Rate'); + + %----- PLL discriminator unfiltered-------------------------------- + plot (handles(3, 1), timeAxisInSeconds, ... + trackResults(channelNr).innovation, 'r'); + + grid (handles(3, 1)); + axis (handles(3, 1), 'auto'); + xlim (handles(3, 1), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(3, 1), 'Time (s)'); + ylabel(handles(3, 1), 'Amplitude'); + title (handles(3, 1), 'Raw PLL discriminator (Innovation)'); + + + %----- PLL discriminator covariance -------------------------------- + plot (handles(4, 1), timeAxisInSeconds, ... + trackResults(channelNr).r_noise_cov, 'r'); + + grid (handles(4, 1)); + axis (handles(4, 1), 'auto'); + xlim (handles(4, 1), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(4, 1), 'Time (s)'); + ylabel(handles(4, 1), 'Variance'); + title (handles(4, 1), 'Estimated Noise Variance'); +end % for channelNr = channelList From 05aea34e7b8e888ad668609c4ca766ada304cbac Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Tue, 14 Aug 2018 00:54:00 -0400 Subject: [PATCH 14/23] Add a unit test for bayesian_estimation.cc as part of trk_test which tests that all outputs are positive over a large number of iterations --- src/tests/CMakeLists.txt | 3 +- .../tracking/bayesian_estimation_test.cc | 80 +++++++++++++++++++ 2 files changed, 82 insertions(+), 1 deletion(-) create mode 100644 src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 54b3ac685..359f9d9fb 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -659,7 +659,8 @@ endif(NOT ${GTEST_DIR_LOCAL}) add_executable(trk_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc ) + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc ) target_link_libraries(trk_test ${Boost_LIBRARIES} ${GFlags_LIBS} diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc new file mode 100644 index 000000000..b9f9c9356 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc @@ -0,0 +1,80 @@ +/*! + * \file bayesian_estimation_positivity_test.cc + * \brief This file implements timing tests for the Bayesian covariance estimator + * \author Gerald LaMountain, 20168. gerald(at)ece.neu.edu + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "bayesian_estimation.h" + +#define BAYESIAN_TEST_N_TRIALS 100 +#define BAYESIAN_TEST_ITER 10000 + +TEST(BayesianEstimationPositivityTest, BayesianPositivityTest) +{ + Bayesian_estimator bayes; + std::chrono::time_point start, end; + std::chrono::duration elapsed_seconds(0); + + arma::vec bayes_mu = arma::zeros(1, 1); + int bayes_nu = 0; + int bayes_kappa = 0; + arma::mat bayes_Psi = arma::ones(1, 1); + + arma::vec input = arma::zeros(1, 1); + arma::mat output = arma::ones(1, 1); + + //--- Perform initializations ------------------------------ + + std::random_device r; + std::default_random_engine e1(r()); + std::normal_distribution normal_dist(0, 5); + + for (int k = 0; k < BAYESIAN_TEST_N_TRIALS; k++) + { + bayes.init(bayes_mu, bayes_kappa, bayes_nu, bayes_Psi); + + + for (int n = 0; n < BAYESIAN_TEST_ITER; n++) + { + input(0) = (double)(normal_dist(e1)); + bayes.update_sequential(input); + + output = bayes.get_Psi_est(); + ASSERT_EQ(output(0) > 0, true); + } + } +} From 5d86e68b603cf91e1380ce638d677a3c5695b362 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 17 Aug 2018 18:10:53 +0200 Subject: [PATCH 15/23] Improve initializations and minor fixes --- .../galileo_e1b_telemetry_decoder_cc.cc | 10 +++---- .../galileo_e5a_telemetry_decoder_cc.cc | 12 ++++---- .../gps_l1_ca_telemetry_decoder_cc.cc | 6 ++-- .../gps_l5_telemetry_decoder_cc.cc | 4 +-- src/core/system_parameters/GPS_L1_CA.h | 2 +- .../galileo_navigation_message.cc | 12 ++++---- .../glonass_gnav_navigation_message.cc | 30 +++++++++---------- .../gps_cnav_navigation_message.cc | 12 ++++---- .../gps_navigation_message.cc | 14 ++++----- 9 files changed, 51 insertions(+), 51 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc index 30484d8ff..769902600 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc @@ -314,7 +314,7 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute consume_each(1); d_flag_preamble = false; - uint32_t required_symbols = GALILEO_INAV_PAGE_SYMBOLS + d_symbols_per_preamble; + uint32_t required_symbols = static_cast(GALILEO_INAV_PAGE_SYMBOLS) + static_cast(d_symbols_per_preamble); if (d_symbol_history.size() > required_symbols) { @@ -427,7 +427,7 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute { // TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay d_TOW_at_Preamble_ms = static_cast(d_nav.TOW_5 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS); d_nav.flag_TOW_5 = false; } @@ -435,20 +435,20 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute { // TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay d_TOW_at_Preamble_ms = static_cast(d_nav.TOW_6 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS); d_nav.flag_TOW_6 = false; } else { // this page has no timing information - d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; + d_TOW_at_current_symbol_ms += static_cast(GALILEO_E1_CODE_PERIOD_MS); // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; } } else // if there is not a new preamble, we define the TOW of the current symbol { if (d_nav.flag_TOW_set == true) { - d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms += static_cast(GALILEO_E1_CODE_PERIOD_MS); } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc index 2c7ac4f34..b033627e7 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc @@ -438,37 +438,37 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute if (d_nav.flag_TOW_1 == true) { d_TOW_at_Preamble_ms = static_cast(d_nav.FNAV_TOW_1 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_nav.flag_TOW_1 = false; } else if (d_nav.flag_TOW_2 == true) { d_TOW_at_Preamble_ms = static_cast(d_nav.FNAV_TOW_2 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_nav.flag_TOW_2 = false; } else if (d_nav.flag_TOW_3 == true) { d_TOW_at_Preamble_ms = static_cast(d_nav.FNAV_TOW_3 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_nav.flag_TOW_3 = false; } else if (d_nav.flag_TOW_4 == true) { d_TOW_at_Preamble_ms = static_cast(d_nav.FNAV_TOW_4 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_nav.flag_TOW_4 = false; } else { - d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms += static_cast(GALILEO_E5a_CODE_PERIOD_MS); } } else // if there is not a new preamble, we define the TOW of the current symbol { if (d_nav.flag_TOW_set == true) { - d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms += static_cast(GALILEO_E5a_CODE_PERIOD_MS); } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index f540f20a1..73254cd6a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -82,7 +82,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( n++; } } - d_stat = 0; + d_stat = 0U; d_flag_frame_sync = false; d_prev_GPS_frame_4bytes = 0; d_TOW_at_Preamble_ms = 0; @@ -93,8 +93,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( flag_PLL_180_deg_phase_locked = false; d_preamble_time_samples = 0ULL; d_TOW_at_current_symbol_ms = 0; - d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size - d_symbol_history.clear(); // Clear all the elements in the buffer + d_symbol_history.set_capacity(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); d_crc_error_synchronization_counter = 0; d_current_subframe_symbol = 0; } @@ -103,6 +102,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( gps_l1_ca_telemetry_decoder_cc::~gps_l1_ca_telemetry_decoder_cc() { volk_gnsssdr_free(d_preambles_symbols); + d_symbol_history.clear(); if (d_dump_file.is_open() == true) { try diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc index ed235933f..8aafd598a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -64,8 +64,8 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( DLOG(INFO) << "GPS L5 TELEMETRY PROCESSING: satellite " << d_satellite; d_channel = 0; d_flag_valid_word = false; - d_TOW_at_current_symbol_ms = 0; - d_TOW_at_Preamble_ms = 0; + d_TOW_at_current_symbol_ms = 0U; + d_TOW_at_Preamble_ms = 0U; // initialize the CNAV frame decoder (libswiftcnav) cnav_msg_decoder_init(&d_cnav_decoder); for (int32_t aux = 0; aux < GPS_L5i_NH_CODE_LENGTH; aux++) diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index 244a2afef..e3a3871f7 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -54,7 +54,7 @@ const double GPS_L1_FREQ_HZ = FREQ1; //!< L1 [Hz] const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s] const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds] -const uint32_t GPS_L1_CA_CODE_PERIOD_MS = 1; //!< GPS L1 C/A code period [ms] +const uint32_t GPS_L1_CA_CODE_PERIOD_MS = 1U; //!< GPS L1 C/A code period [ms] const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period [seconds] /*! diff --git a/src/core/system_parameters/galileo_navigation_message.cc b/src/core/system_parameters/galileo_navigation_message.cc index aeedb1669..3fb375712 100644 --- a/src/core/system_parameters/galileo_navigation_message.cc +++ b/src/core/system_parameters/galileo_navigation_message.cc @@ -229,11 +229,11 @@ Galileo_Navigation_Message::Galileo_Navigation_Message() } -bool Galileo_Navigation_Message::CRC_test(std::bitset bits, boost::uint32_t checksum) +bool Galileo_Navigation_Message::CRC_test(std::bitset bits, uint32_t checksum) { CRC_Galileo_INAV_type CRC_Galileo; - boost::uint32_t crc_computed; + uint32_t crc_computed; // Galileo INAV frame for CRC is not an integer multiple of bytes // it needs to be filled with zeroes at the start of the frame. // This operation is done in the transformation from bits to bytes @@ -290,7 +290,7 @@ uint64_t Galileo_Navigation_Message::read_page_type_unsigned(std::bitset= 1 and satellite_slot_number <= 5) { - frame_ID = 1; + frame_ID = 1U; } else if (satellite_slot_number >= 6 and satellite_slot_number <= 10) { - frame_ID = 2; + frame_ID = 2U; } else if (satellite_slot_number >= 11 and satellite_slot_number <= 15) { - frame_ID = 3; + frame_ID = 3U; } else if (satellite_slot_number >= 16 and satellite_slot_number <= 20) { - frame_ID = 4; + frame_ID = 4U; } else if (satellite_slot_number >= 21 and satellite_slot_number <= 24) { - frame_ID = 5; + frame_ID = 5U; } else { LOG(WARNING) << "GLONASS GNAV: Invalid Satellite Slot Number"; - frame_ID = 0; + frame_ID = 0U; } return frame_ID; @@ -313,8 +313,8 @@ uint32_t Glonass_Gnav_Navigation_Message::get_frame_number(uint32_t satellite_sl int32_t Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) { int32_t J = 0; - d_string_ID = 0; - d_frame_ID = 0; + d_string_ID = 0U; + d_frame_ID = 0U; // Unpack bytes to bits std::bitset string_bits(frame_string); diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 8d7b70921..98d02d59d 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -99,7 +99,7 @@ uint64_t Gps_CNAV_Navigation_Message::read_navigation_unsigned(std::bitset Date: Fri, 17 Aug 2018 19:33:17 +0200 Subject: [PATCH 16/23] Access elements of d_symbol_history without bound checking --- .../gps_l1_ca_telemetry_decoder_cc.cc | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 73254cd6a..e4a12bd43 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -321,7 +321,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ // record the oldest subframe symbol before inserting a new symbol into the circular buffer if (d_current_subframe_symbol < GPS_SUBFRAME_MS and d_symbol_history.size() > 0) { - d_subframe_symbols[d_current_subframe_symbol] = d_symbol_history.at(0).Prompt_I; + d_subframe_symbols[d_current_subframe_symbol] = d_symbol_history[0].Prompt_I; d_current_subframe_symbol++; } @@ -337,9 +337,9 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ // std::cout << "-------\n"; for (uint32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++) { - if (d_symbol_history.at(i).Flag_valid_symbol_output == true) + if (d_symbol_history[i].Flag_valid_symbol_output == true) { - if (d_symbol_history.at(i).Prompt_I < 0) // symbols clipping + if (d_symbol_history[i].Prompt_I < 0) // symbols clipping { corr_value -= d_preambles_symbols[i]; } @@ -358,18 +358,18 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ if (d_stat == 0) { // record the preamble sample stamp - d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the preamble sample stamp - DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history.at(0).Tracking_sample_counter=" << d_symbol_history.at(0).Tracking_sample_counter; + d_preamble_time_samples = d_symbol_history[0].Tracking_sample_counter; // record the preamble sample stamp + DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history[0].Tracking_sample_counter=" << d_symbol_history[0].Tracking_sample_counter; d_stat = 1; // enter into frame pre-detection status } else if (d_stat == 1) // check 6 seconds of preamble separation { - preamble_diff_ms = std::round(((static_cast(d_symbol_history.at(0).Tracking_sample_counter) - static_cast(d_preamble_time_samples)) / static_cast(d_symbol_history.at(0).fs)) * 1000.0); + preamble_diff_ms = std::round(((static_cast(d_symbol_history[0].Tracking_sample_counter) - static_cast(d_preamble_time_samples)) / static_cast(d_symbol_history[0].fs)) * 1000.0); if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) % GPS_SUBFRAME_MS == 0) { DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite; d_flag_preamble = true; - d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the PRN start sample index associated to the preamble + d_preamble_time_samples = d_symbol_history[0].Tracking_sample_counter; // record the PRN start sample index associated to the preamble if (!d_flag_frame_sync) { d_flag_frame_sync = true; @@ -383,7 +383,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ flag_PLL_180_deg_phase_locked = false; } DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " - << static_cast(d_preamble_time_samples) / static_cast(d_symbol_history.at(0).fs) << " [s]"; + << static_cast(d_preamble_time_samples) / static_cast(d_symbol_history[0].fs) << " [s]"; } // try to decode the subframe: @@ -407,7 +407,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ { if (d_stat == 1) { - preamble_diff_ms = round(((static_cast(d_symbol_history.at(0).Tracking_sample_counter) - static_cast(d_preamble_time_samples)) / static_cast(d_symbol_history.at(0).fs)) * 1000.0); + preamble_diff_ms = round(((static_cast(d_symbol_history[0].Tracking_sample_counter) - static_cast(d_preamble_time_samples)) / static_cast(d_symbol_history[0].fs)) * 1000.0); if (preamble_diff_ms > GPS_SUBFRAME_MS) { DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms; From 388d1623ed39ba3e85bdaa70168bf44000aa6842 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 21 Aug 2018 14:57:22 +0200 Subject: [PATCH 17/23] Pass parameters by reference instead of by value (improves effieciency) --- .../tracking/libs/bayesian_estimation.cc | 94 +++++++++---------- .../tracking/libs/bayesian_estimation.h | 14 ++- 2 files changed, 52 insertions(+), 56 deletions(-) diff --git a/src/algorithms/tracking/libs/bayesian_estimation.cc b/src/algorithms/tracking/libs/bayesian_estimation.cc index ed1065d44..536ca1803 100644 --- a/src/algorithms/tracking/libs/bayesian_estimation.cc +++ b/src/algorithms/tracking/libs/bayesian_estimation.cc @@ -42,84 +42,84 @@ Bayesian_estimator::Bayesian_estimator() { - int ny = 1; - mu_prior = arma::zeros(ny,1); + int ny = 1; + mu_prior = arma::zeros(ny, 1); kappa_prior = 0; - nu_prior = 0; - Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); + nu_prior = 0; + Psi_prior = arma::eye(ny, ny) * (nu_prior + ny + 1); - mu_est = mu_prior; - Psi_est = Psi_prior; + mu_est = mu_prior; + Psi_est = Psi_prior; } Bayesian_estimator::Bayesian_estimator(int ny) { - mu_prior = arma::zeros(ny,1); + mu_prior = arma::zeros(ny, 1); kappa_prior = 0; - nu_prior = 0; - Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); + nu_prior = 0; + Psi_prior = arma::eye(ny, ny) * (nu_prior + ny + 1); - mu_est = mu_prior; - Psi_est = Psi_prior; + mu_est = mu_prior; + Psi_est = Psi_prior; } -Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) +Bayesian_estimator::Bayesian_estimator(const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0) { - mu_prior = mu_prior_0; + mu_prior = mu_prior_0; kappa_prior = kappa_prior_0; - nu_prior = nu_prior_0; - Psi_prior = Psi_prior_0; + nu_prior = nu_prior_0; + Psi_prior = Psi_prior_0; - mu_est = mu_prior; - Psi_est = Psi_prior; + mu_est = mu_prior; + Psi_est = Psi_prior; } Bayesian_estimator::~Bayesian_estimator() { } -void Bayesian_estimator::init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) +void Bayesian_estimator::init(const arma::mat& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0) { - mu_prior = mu_prior_0; + mu_prior = mu_prior_0; kappa_prior = kappa_prior_0; - nu_prior = nu_prior_0; - Psi_prior = Psi_prior_0; + nu_prior = nu_prior_0; + Psi_prior = Psi_prior_0; - mu_est = mu_prior; - Psi_est = Psi_prior; + mu_est = mu_prior; + Psi_est = Psi_prior; } /* * Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in * the class structure, and update the priors according to the computed posteriors */ -void Bayesian_estimator::update_sequential(arma::vec data) +void Bayesian_estimator::update_sequential(const arma::vec& data) { - int K = data.n_cols; + int K = data.n_cols; int ny = data.n_rows; if (mu_prior.is_empty()) { - mu_prior = arma::zeros(ny,1); + mu_prior = arma::zeros(ny, 1); } if (Psi_prior.is_empty()) { - Psi_prior = arma::zeros(ny,ny); + Psi_prior = arma::zeros(ny, ny); } arma::vec y_mean = arma::mean(data, 1); - arma::mat Psi_N = arma::zeros(ny, ny); + arma::mat Psi_N = arma::zeros(ny, ny); for (int kk = 0; kk < K; kk++) { - Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t()); + Psi_N = Psi_N + (data.col(kk) - y_mean) * ((data.col(kk) - y_mean).t()); } - arma::vec mu_posterior = (kappa_prior*mu_prior + K*y_mean) / (kappa_prior + K); - int kappa_posterior = kappa_prior + K; - int nu_posterior = nu_prior + K; - arma::mat Psi_posterior = Psi_prior + Psi_N + (kappa_prior*K)/(kappa_prior + K)*(y_mean - mu_prior)*((y_mean - mu_prior).t()); + arma::vec mu_posterior = (kappa_prior * mu_prior + K * y_mean) / (kappa_prior + K); + int kappa_posterior = kappa_prior + K; + int nu_posterior = nu_prior + K; + arma::mat Psi_posterior = Psi_prior + Psi_N + (kappa_prior * K) / (kappa_prior + K) * (y_mean - mu_prior) * ((y_mean - mu_prior).t()); mu_est = mu_posterior; if ((nu_posterior - ny - 1) > 0) @@ -131,10 +131,10 @@ void Bayesian_estimator::update_sequential(arma::vec data) Psi_est = Psi_posterior / (nu_posterior + ny + 1); } - mu_prior = mu_posterior; + mu_prior = mu_posterior; kappa_prior = kappa_posterior; - nu_prior = nu_posterior; - Psi_prior = Psi_posterior; + nu_prior = nu_posterior; + Psi_prior = Psi_posterior; } @@ -142,10 +142,9 @@ void Bayesian_estimator::update_sequential(arma::vec data) * Perform Bayesian noise estimation using a new set of normal-inverse-Wishart priors * and update the priors according to the computed posteriors */ -void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) +void Bayesian_estimator::update_sequential(const arma::vec& data, const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0) { - - int K = data.n_cols; + int K = data.n_cols; int ny = data.n_rows; arma::vec y_mean = arma::mean(data, 1); @@ -153,13 +152,13 @@ void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0, for (int kk = 0; kk < K; kk++) { - Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t()); + Psi_N = Psi_N + (data.col(kk) - y_mean) * ((data.col(kk) - y_mean).t()); } - arma::vec mu_posterior = (kappa_prior_0*mu_prior_0 + K*y_mean) / (kappa_prior_0 + K); - int kappa_posterior = kappa_prior_0 + K; - int nu_posterior = nu_prior_0 + K; - arma::mat Psi_posterior = Psi_prior_0 + Psi_N + (kappa_prior_0*K)/(kappa_prior_0 + K)*(y_mean - mu_prior_0)*((y_mean - mu_prior_0).t()); + arma::vec mu_posterior = (kappa_prior_0 * mu_prior_0 + K * y_mean) / (kappa_prior_0 + K); + int kappa_posterior = kappa_prior_0 + K; + int nu_posterior = nu_prior_0 + K; + arma::mat Psi_posterior = Psi_prior_0 + Psi_N + (kappa_prior_0 * K) / (kappa_prior_0 + K) * (y_mean - mu_prior_0) * ((y_mean - mu_prior_0).t()); mu_est = mu_posterior; if ((nu_posterior - ny - 1) > 0) @@ -171,10 +170,10 @@ void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0, Psi_est = Psi_posterior / (nu_posterior + ny + 1); } - mu_prior = mu_posterior; + mu_prior = mu_posterior; kappa_prior = kappa_posterior; - nu_prior = nu_posterior; - Psi_prior = Psi_posterior; + nu_prior = nu_posterior; + Psi_prior = Psi_posterior; } arma::mat Bayesian_estimator::get_mu_est() @@ -186,4 +185,3 @@ arma::mat Bayesian_estimator::get_Psi_est() { return Psi_est; } - diff --git a/src/algorithms/tracking/libs/bayesian_estimation.h b/src/algorithms/tracking/libs/bayesian_estimation.h index 44370cacb..606c97c77 100644 --- a/src/algorithms/tracking/libs/bayesian_estimation.h +++ b/src/algorithms/tracking/libs/bayesian_estimation.h @@ -53,22 +53,21 @@ * \mathbf{\mu}_{0}, \kappa_{0}, \nu_{0}, and \mathbf{\Psi}. * * [1] TODO: Ref1 - * + * */ class Bayesian_estimator { - public: Bayesian_estimator(); Bayesian_estimator(int ny); - Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); + Bayesian_estimator(const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0); ~Bayesian_estimator(); - void init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); + void init(const arma::mat& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0); - void update_sequential(arma::vec data); - void update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); + void update_sequential(const arma::vec& data); + void update_sequential(const arma::vec& data, const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0); arma::mat get_mu_est(); arma::mat get_Psi_est(); @@ -76,12 +75,11 @@ public: private: arma::vec mu_est; arma::mat Psi_est; - + arma::vec mu_prior; int kappa_prior; int nu_prior; arma::mat Psi_prior; - }; #endif From fb33bbc6c3e92dfc192f7ac6c9c484d2ffe38c82 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 21 Aug 2018 15:00:57 +0200 Subject: [PATCH 18/23] Add use of cstdint typedefs --- .../gps_l1_ca_kf_tracking_cc.cc | 114 ++++++++-------- .../gps_l1_ca_kf_tracking_cc.h | 122 +++++++++--------- 2 files changed, 117 insertions(+), 119 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index 0f6b3d9d5..138cf604f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -59,19 +59,19 @@ using google::LogMessage; gps_l1_ca_kf_tracking_cc_sptr gps_l1_ca_kf_make_tracking_cc( - unsigned int order, - long if_freq, - long fs_in, - unsigned int vector_length, + uint32_t order, + int64_t if_freq, + int64_t fs_in, + uint32_t vector_length, bool dump, std::string dump_filename, float dll_bw_hz, float early_late_space_chips, bool bce_run, - unsigned int bce_ptrans, - unsigned int bce_strans, - int bce_nu, - int bce_kappa) + uint32_t bce_ptrans, + uint32_t bce_strans, + int32_t bce_nu, + int32_t bce_kappa) { return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(order, if_freq, fs_in, vector_length, dump, dump_filename, dll_bw_hz, early_late_space_chips, @@ -90,20 +90,20 @@ void Gps_L1_Ca_Kf_Tracking_cc::forecast(int noutput_items, Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( - unsigned int order, - long if_freq, - long fs_in, - unsigned int vector_length, + uint32_t order, + int64_t if_freq, + int64_t fs_in, + uint32_t vector_length, bool dump, std::string dump_filename, float dll_bw_hz, float early_late_space_chips, bool bce_run, - unsigned int bce_ptrans, - unsigned int bce_strans, - int bce_nu, - int bce_kappa) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + uint32_t bce_ptrans, + uint32_t bce_strans, + int32_t bce_nu, + int32_t bce_kappa) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); @@ -132,7 +132,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( // correlator outputs (scalar) d_n_correlator_taps = 3; // Early, Prompt, and Late d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - for (int n = 0; n < d_n_correlator_taps; n++) + for (int32_t n = 0; n < d_n_correlator_taps; n++) { d_correlator_outs[n] = gr_complex(0, 0); } @@ -199,8 +199,8 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( //covariances (static) double sigma2_carrier_phase = GPS_TWO_PI / 4; - double sigma2_doppler = 450; - double sigma2_doppler_rate = pow(4.0 * GPS_TWO_PI, 2) / 12.0; + double sigma2_doppler = 450; + double sigma2_doppler_rate = pow(4.0 * GPS_TWO_PI, 2) / 12.0; kf_P_x_ini = arma::zeros(2, 2); kf_P_x_ini(0, 0) = sigma2_carrier_phase; @@ -252,16 +252,16 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( } // Bayesian covariance estimator initialization - kf_iter = 0; - bayes_run = bce_run; + kf_iter = 0; + bayes_run = bce_run; bayes_ptrans = bce_ptrans; bayes_strans = bce_strans; - bayes_kappa = bce_kappa; - bayes_nu = bce_nu; - kf_R_est = kf_R; + bayes_kappa = bce_kappa; + bayes_nu = bce_nu; + kf_R_est = kf_R; - bayes_estimator.init(arma::zeros(1,1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R)*(bayes_nu + 2)); + bayes_estimator.init(arma::zeros(1, 1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R) * (bayes_nu + 2)); } void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() @@ -278,12 +278,12 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() if (d_acquisition_gnss_synchro->Acq_doppler_step > 0) { kf_P_x_ini(1, 1) = pow(d_acq_carrier_doppler_step_hz / 3.0, 2); - bayes_estimator.init(arma::zeros(1,1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R)*(bayes_nu + 2)); + bayes_estimator.init(arma::zeros(1, 1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R) * (bayes_nu + 2)); } - long int acq_trk_diff_samples; + int64_t acq_trk_diff_samples; double acq_trk_diff_seconds; - acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect Fd = (C / (C + Vr)) * F @@ -325,7 +325,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() gps_l1_ca_code_gen_float(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); - for (int n = 0; n < d_n_correlator_taps; n++) + for (int32_t n = 0; n < d_n_correlator_taps; n++) { d_correlator_outs[n] = gr_complex(0, 0); } @@ -396,14 +396,14 @@ Gps_L1_Ca_Kf_Tracking_cc::~Gps_L1_Ca_Kf_Tracking_cc() } -int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() +int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile() { // READ DUMP FILE std::ifstream::pos_type size; - int number_of_double_vars = 1; - int number_of_float_vars = 19; - int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + - sizeof(float) * number_of_float_vars + sizeof(unsigned int); + int32_t number_of_double_vars = 1; + int32_t number_of_float_vars = 19; + int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(float) * number_of_float_vars + sizeof(uint32_t); std::ifstream dump_file; dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); try @@ -416,11 +416,11 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() return 1; } // count number of epochs and rewind - long int num_epoch = 0; + int64_t num_epoch = 0; if (dump_file.is_open()) { size = dump_file.tellg(); - num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); dump_file.seekg(0, std::ios::beg); } else @@ -434,7 +434,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() float *abs_VL = new float[num_epoch]; float *Prompt_I = new float[num_epoch]; float *Prompt_Q = new float[num_epoch]; - unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch]; + uint64_t *PRN_start_sample_count = new uint64_t[num_epoch]; float *acc_carrier_phase_rad = new float[num_epoch]; float *carrier_doppler_hz = new float[num_epoch]; float *carrier_dopplerrate_hz2 = new float[num_epoch]; @@ -448,13 +448,13 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() float *carrier_lock_test = new float[num_epoch]; float *aux1 = new float[num_epoch]; double *aux2 = new double[num_epoch]; - unsigned int *PRN = new unsigned int[num_epoch]; + uint32_t *PRN = new uint32_t[num_epoch]; try { if (dump_file.is_open()) { - for (long int i = 0; i < num_epoch; i++) + for (int64_t i = 0; i < num_epoch; i++) { dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); @@ -463,7 +463,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); + dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(uint64_t)); dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carrier_dopplerrate_hz2[i]), sizeof(float)); @@ -477,7 +477,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); + dump_file.read(reinterpret_cast(&PRN[i]), sizeof(uint32_t)); } } dump_file.close(); @@ -635,7 +635,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() } -void Gps_L1_Ca_Kf_Tracking_cc::set_channel(unsigned int channel) +void Gps_L1_Ca_Kf_Tracking_cc::set_channel(uint32_t channel) { gr::thread::scoped_lock l(d_setlock); d_channel = channel; @@ -691,9 +691,9 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus if (d_pull_in == true) { // Signal alignment (skip samples until the incoming signal is aligned with local replica) - unsigned long int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + uint64_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; double acq_trk_shif_correction_samples = static_cast(d_current_prn_length_samples) - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - int samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + int32_t samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); if (samples_offset < 0) { samples_offset = 0; @@ -720,7 +720,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus // Covariance estimation initialization reset kf_iter = 0; - bayes_estimator.init(arma::zeros(1,1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R)*(bayes_nu + 2)); + bayes_estimator.init(arma::zeros(1, 1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R) * (bayes_nu + 2)); consume_each(samples_offset); // shift input to perform alignment with local replica return 1; @@ -764,13 +764,13 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus } else { - kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix + kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix kf_R_est = kf_R; } // Kalman filter update step - kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain - kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation + kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain + kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation kf_P_x = (arma::eye(size(kf_P_x_pre)) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix // Store Kalman filter results @@ -855,7 +855,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus // ########### Output the tracking data to navigation and PVT ########## current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast(d_current_prn_length_samples); current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; @@ -864,16 +864,15 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus current_synchro_data.correlation_length_ms = 1; kf_iter++; - } else { - for (int n = 0; n < d_n_correlator_taps; n++) + for (int32_t n = 0; n < d_n_correlator_taps; n++) { d_correlator_outs[n] = gr_complex(0, 0); } - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast(d_current_prn_length_samples); current_synchro_data.System = {'G'}; current_synchro_data.correlation_length_ms = 1; } @@ -892,7 +891,6 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus float tmp_VL = 0.0; float tmp_float; double tmp_double; - unsigned long int tmp_long; prompt_I = d_correlator_outs[1].real(); prompt_Q = d_correlator_outs[1].imag(); tmp_E = std::abs(d_correlator_outs[0]); @@ -910,7 +908,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(uint64_t)); // accumulated carrier phase tmp_float = d_acc_carrier_phase_rad; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); @@ -941,11 +939,11 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + tmp_double = static_cast(d_sample_counter + static_cast(d_current_prn_length_samples)); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN - unsigned int prn_ = d_acquisition_gnss_synchro->PRN; - d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + uint32_t prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(uint32_t)); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index 718613820..64f22ee26 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -40,16 +40,16 @@ #ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H #define GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H -#include -#include -#include -#include #include "gnss_synchro.h" #include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_PLL_filter.h" -#include #include "cpu_multicorrelator_real_codes.h" #include "bayesian_estimation.h" +#include +#include +#include +#include +#include class Gps_L1_Ca_Kf_Tracking_cc; @@ -57,18 +57,18 @@ typedef boost::shared_ptr gps_l1_ca_kf_tracking_cc_sptr; gps_l1_ca_kf_tracking_cc_sptr -gps_l1_ca_kf_make_tracking_cc(unsigned int order, - long if_freq, - long fs_in, unsigned int vector_length, +gps_l1_ca_kf_make_tracking_cc(uint32_t order, + int64_t if_freq, + int64_t fs_in, uint32_t vector_length, bool dump, std::string dump_filename, float pll_bw_hz, float early_late_space_chips, bool bce_run, - unsigned int bce_ptrans, - unsigned int bce_strans, - int bce_nu, - int bce_kappa); + uint32_t bce_ptrans, + uint32_t bce_strans, + int32_t bce_nu, + int32_t bce_kappa); /*! @@ -79,7 +79,7 @@ class Gps_L1_Ca_Kf_Tracking_cc : public gr::block public: ~Gps_L1_Ca_Kf_Tracking_cc(); - void set_channel(unsigned int channel); + void set_channel(uint32_t channel); void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); void start_tracking(); @@ -90,42 +90,42 @@ public: private: friend gps_l1_ca_kf_tracking_cc_sptr - gps_l1_ca_kf_make_tracking_cc(unsigned int order, - long if_freq, - long fs_in, unsigned int vector_length, + gps_l1_ca_kf_make_tracking_cc(uint32_t order, + int64_t if_freq, + int64_t fs_in, uint32_t vector_length, bool dump, std::string dump_filename, float dll_bw_hz, float early_late_space_chips, bool bce_run, - unsigned int bce_ptrans, - unsigned int bce_strans, - int bce_nu, - int bce_kappa); + uint32_t bce_ptrans, + uint32_t bce_strans, + int32_t bce_nu, + int32_t bce_kappa); - Gps_L1_Ca_Kf_Tracking_cc(unsigned int order, - long if_freq, - long fs_in, unsigned int vector_length, + Gps_L1_Ca_Kf_Tracking_cc(uint32_t order, + int64_t if_freq, + int64_t fs_in, uint32_t vector_length, bool dump, std::string dump_filename, float dll_bw_hz, float early_late_space_chips, bool bce_run, - unsigned int bce_ptrans, - unsigned int bce_strans, - int bce_nu, - int bce_kappa); + uint32_t bce_ptrans, + uint32_t bce_strans, + int32_t bce_nu, + int32_t bce_kappa); // tracking configuration vars - unsigned int d_order; - unsigned int d_vector_length; + uint32_t d_order; + uint32_t d_vector_length; bool d_dump; Gnss_Synchro* d_acquisition_gnss_synchro; - unsigned int d_channel; + uint32_t d_channel; - long d_if_freq; - long d_fs_in; + int64_t d_if_freq; + int64_t d_fs_in; double d_early_late_spc_chips; @@ -135,42 +135,42 @@ private: double d_rem_carr_phase_rad; // Kalman filter variables - arma::mat kf_P_x_ini; //initial state error covariance matrix - arma::mat kf_P_x; //state error covariance matrix - arma::mat kf_P_x_pre; //Predicted state error covariance matrix - arma::mat kf_P_y; //innovation covariance matrix + arma::mat kf_P_x_ini; // initial state error covariance matrix + arma::mat kf_P_x; // state error covariance matrix + arma::mat kf_P_x_pre; // Predicted state error covariance matrix + arma::mat kf_P_y; // innovation covariance matrix - arma::mat kf_F; //state transition matrix - arma::mat kf_H; //system matrix - arma::mat kf_R; //measurement error covariance matrix - arma::mat kf_Q; //system error covariance matrix + arma::mat kf_F; // state transition matrix + arma::mat kf_H; // system matrix + arma::mat kf_R; // measurement error covariance matrix + arma::mat kf_Q; // system error covariance matrix - arma::colvec kf_x; //state vector - arma::colvec kf_x_pre; //predicted state vector - arma::colvec kf_y; //measurement vector - arma::mat kf_K; //Kalman gain matrix + arma::colvec kf_x; // state vector + arma::colvec kf_x_pre; // predicted state vector + arma::colvec kf_y; // measurement vector + arma::mat kf_K; // Kalman gain matrix // Bayesian estimator Bayesian_estimator bayes_estimator; - arma::mat kf_R_est; //measurement error covariance - unsigned int bayes_ptrans; - unsigned int bayes_strans; - int bayes_nu; - int bayes_kappa; + arma::mat kf_R_est; // measurement error covariance + uint32_t bayes_ptrans; + uint32_t bayes_strans; + int32_t bayes_nu; + int32_t bayes_kappa; bool bayes_run; - unsigned int kf_iter; + uint32_t kf_iter; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; - //Tracking_2nd_PLL_filter d_carrier_loop_filter; + // Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition double d_acq_carrier_doppler_step_hz; double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; // correlator - int d_n_correlator_taps; + int32_t d_n_correlator_taps; float* d_ca_code; float* d_local_code_shift_chips; gr_complex* d_correlator_outs; @@ -189,20 +189,20 @@ private: double code_error_chips; double code_error_filt_chips; - //PRN period in samples - int d_current_prn_length_samples; + // PRN period in samples + int32_t d_current_prn_length_samples; - //processing samples counters - unsigned long int d_sample_counter; - unsigned long int d_acq_sample_stamp; + // processing samples counters + uint64_t d_sample_counter; + uint64_t d_acq_sample_stamp; // CN0 estimation and lock detector - int d_cn0_estimation_counter; + int32_t d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; - int d_carrier_lock_fail_counter; + int32_t d_carrier_lock_fail_counter; // control vars bool d_enable_tracking; @@ -215,7 +215,7 @@ private: std::map systemName; std::string sys; - int save_matfile(); + int32_t save_matfile(); }; -#endif //GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H +#endif // GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H From ff90cf1a7e0a5e88f75efe0b0b05b21b9d380538 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 21 Aug 2018 15:01:35 +0200 Subject: [PATCH 19/23] Remove unused parameter --- src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc index 4c06d5580..38b3e3cad 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -61,7 +61,6 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( std::string dump_filename; std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; float dll_bw_hz; float early_late_space_chips; bool bce_run; From 0c26a95af4371ab24fd993568bf67ead78c97fdb Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 21 Aug 2018 15:20:48 +0200 Subject: [PATCH 20/23] Enhance const correctness, minor cosmetics --- .../tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc | 9 ++++----- src/algorithms/tracking/libs/bayesian_estimation.cc | 6 +++--- src/algorithms/tracking/libs/bayesian_estimation.h | 4 ++-- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index 138cf604f..740a11e84 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -84,7 +84,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::forecast(int noutput_items, { if (noutput_items != 0) { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + ninput_items_required[0] = static_cast(d_vector_length) * 2; // set the required available samples in each call } } @@ -122,7 +122,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( // Initialize tracking ========================================== d_code_loop_filter.set_DLL_BW(dll_bw_hz); - //--- DLL variables -------------------------------------------------------- + // --- DLL variables -------------------------------------------------------- d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) // Initialization of local code replica @@ -144,7 +144,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( multicorrelator_cpu.init(2 * d_current_prn_length_samples, d_n_correlator_taps); - //--- Perform initializations ------------------------------ + // --- Perform initializations ------------------------------ // define initial code frequency basis of NCO d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ; // define residual code phase (in chips) @@ -156,7 +156,6 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( // sample synchronization d_sample_counter = 0; - //d_sample_counter_seconds = 0; d_acq_sample_stamp = 0; d_enable_tracking = false; @@ -197,7 +196,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( double sigma2_phase_detector_cycles2; sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)); - //covariances (static) + // covariances (static) double sigma2_carrier_phase = GPS_TWO_PI / 4; double sigma2_doppler = 450; double sigma2_doppler_rate = pow(4.0 * GPS_TWO_PI, 2) / 12.0; diff --git a/src/algorithms/tracking/libs/bayesian_estimation.cc b/src/algorithms/tracking/libs/bayesian_estimation.cc index 536ca1803..30e7d45ec 100644 --- a/src/algorithms/tracking/libs/bayesian_estimation.cc +++ b/src/algorithms/tracking/libs/bayesian_estimation.cc @@ -38,7 +38,7 @@ */ #include "bayesian_estimation.h" -#include + Bayesian_estimator::Bayesian_estimator() { @@ -176,12 +176,12 @@ void Bayesian_estimator::update_sequential(const arma::vec& data, const arma::ve Psi_prior = Psi_posterior; } -arma::mat Bayesian_estimator::get_mu_est() +arma::mat Bayesian_estimator::get_mu_est() const { return mu_est; } -arma::mat Bayesian_estimator::get_Psi_est() +arma::mat Bayesian_estimator::get_Psi_est() const { return Psi_est; } diff --git a/src/algorithms/tracking/libs/bayesian_estimation.h b/src/algorithms/tracking/libs/bayesian_estimation.h index 606c97c77..002e7a75c 100644 --- a/src/algorithms/tracking/libs/bayesian_estimation.h +++ b/src/algorithms/tracking/libs/bayesian_estimation.h @@ -69,8 +69,8 @@ public: void update_sequential(const arma::vec& data); void update_sequential(const arma::vec& data, const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0); - arma::mat get_mu_est(); - arma::mat get_Psi_est(); + arma::mat get_mu_est() const; + arma::mat get_Psi_est() const; private: arma::vec mu_est; From 4cf10c221a8ba0f728a7fe101e3e497d381d9e03 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 21 Aug 2018 16:41:07 +0200 Subject: [PATCH 21/23] Update SBAS PRN numbers --- src/core/receiver/gnss_flowgraph.cc | 2 +- src/core/system_parameters/gnss_satellite.cc | 49 +++++++++++--------- 2 files changed, 29 insertions(+), 22 deletions(-) diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 3793709f7..a88ace37a 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1183,7 +1183,7 @@ void GNSSFlowgraph::set_signals_list() 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32}; - std::set available_sbas_prn = {120, 124, 126}; + std::set available_sbas_prn = {123, 131, 135, 136, 138}; std::set available_galileo_prn = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index 646aa2b69..dd8c8ffc8 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -184,26 +184,30 @@ void Gnss_Satellite::set_PRN(uint32_t PRN_) } else if (system.compare("SBAS") == 0) { - if (PRN_ == 122) + if (PRN_ == 120) { PRN = PRN_; - } // WAAS Inmarsat 3F4 (AOR-W) - else if (PRN_ == 134) + } // EGNOS Test Platform.Inmarsat 3-F2 (Atlantic Ocean Region-East) + else if (PRN_ == 123) { PRN = PRN_; - } // WAAS Inmarsat 3F3 (POR) - else if (PRN_ == 120) + } // EGNOS Operational Platform. Astra 5B + else if (PRN_ == 131) { PRN = PRN_; - } // EGNOS AOR-E Broadcast satellite http://www.egnos-pro.esa.int/index.html - else if (PRN_ == 124) + } // WAAS Eutelsat 117 West B + else if (PRN_ == 135) { PRN = PRN_; - } // EGNOS ESA ARTEMIS used for EGNOS Operations - else if (PRN_ == 126) + } // WAAS Galaxy 15 + else if (PRN_ == 136) { PRN = PRN_; - } // EGNOS IOR-W currently used by Industry to perform various tests on the system. + } // EGNOS Operational Platform. SES-5 (a.k.a. Sirius 5 or Astra 4B) + else if (PRN_ == 138) + { + PRN = PRN_; + } // WAAS Anik F1R else { DLOG(INFO) << "This PRN is not defined"; @@ -492,20 +496,23 @@ std::string Gnss_Satellite::what_block(const std::string& system_, uint32_t PRN_ { switch (PRN_) { - case 122: - block_ = std::string("WAAS"); // WAAS Inmarsat 3F4 (AOR-W) - break; - case 134: - block_ = std::string("WAAS"); // WAAS Inmarsat 3F3 (POR) - break; case 120: - block_ = std::string("EGNOS"); // EGNOS AOR-E Broadcast satellite http://www.egnos-pro.esa.int/index.html + block_ = std::string("EGNOS Test Platform"); // Inmarsat 3-F2 (Atlantic Ocean Region-East) break; - case 124: - block_ = std::string("EGNOS"); // EGNOS ESA ARTEMIS used for EGNOS Operations + case 123: + block_ = std::string("EGNOS"); // EGNOS Operational Platform. Astra 5B break; - case 126: - block_ = std::string("EGNOS"); // EGNOS IOR-W currently used by Industry to perform various tests on the system. + case 131: + block_ = std::string("WAAS"); // WAAS Eutelsat 117 West B + break; + case 135: + block_ = std::string("WAAS"); // WAAS Galaxy 15 + break; + case 136: + block_ = std::string("EGNOS"); // EGNOS Operational Platform. SES-5 (a.k.a. Sirius 5 or Astra 4B) + break; + case 138: + block_ = std::string("WAAS"); // WAAS Anik F1R break; default: block_ = std::string("Unknown"); From 1ee3e867f0535917c887ec17d41429f4ca112f34 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 21 Aug 2018 17:14:59 +0200 Subject: [PATCH 22/23] Add Gerald to the list of contributors --- AUTHORS | 1 + 1 file changed, 1 insertion(+) diff --git a/AUTHORS b/AUTHORS index e6508f5ce..6f98d2038 100644 --- a/AUTHORS +++ b/AUTHORS @@ -50,6 +50,7 @@ Daniel Fehr daniel.co@bluewin.ch Contributor David Pubill david.pubill@cttc.cat Contributor Fran Fabra fabra@ice.csic.es Contributor Gabriel Araujo gabriel.araujo.5000@gmail.com Contributor +Gerald LaMountain gerald@gece.neu.edu Contributor Leonardo Tonetto tonetto.dev@gmail.com Contributor Mara Branzanti mara.branzanti@gmail.com Contributor Marc Molina marc.molina.pena@gmail.com Contributor From 8184e0eb81999a9159807d4080a8afe2325ac0b2 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 21 Aug 2018 18:11:03 +0200 Subject: [PATCH 23/23] Remove unused includes, avoid pointer dereference --- .../tracking/bayesian_estimation_test.cc | 21 +++++-------------- 1 file changed, 5 insertions(+), 16 deletions(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc index b9f9c9356..f32e82f16 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc @@ -29,15 +29,9 @@ * ------------------------------------------------------------------------- */ -#include -#include #include -#include #include #include -#include -#include -#include #include "bayesian_estimation.h" #define BAYESIAN_TEST_N_TRIALS 100 @@ -46,16 +40,11 @@ TEST(BayesianEstimationPositivityTest, BayesianPositivityTest) { Bayesian_estimator bayes; - std::chrono::time_point start, end; - std::chrono::duration elapsed_seconds(0); - arma::vec bayes_mu = arma::zeros(1, 1); int bayes_nu = 0; int bayes_kappa = 0; arma::mat bayes_Psi = arma::ones(1, 1); - - arma::vec input = arma::zeros(1, 1); - arma::mat output = arma::ones(1, 1); + arma::vec input = arma::zeros(1, 1); //--- Perform initializations ------------------------------ @@ -67,14 +56,14 @@ TEST(BayesianEstimationPositivityTest, BayesianPositivityTest) { bayes.init(bayes_mu, bayes_kappa, bayes_nu, bayes_Psi); - for (int n = 0; n < BAYESIAN_TEST_ITER; n++) { - input(0) = (double)(normal_dist(e1)); + input(0) = static_cast(normal_dist(e1)); bayes.update_sequential(input); - output = bayes.get_Psi_est(); - ASSERT_EQ(output(0) > 0, true); + arma::mat output = bayes.get_Psi_est(); + double output0 = output(0, 0); + ASSERT_EQ(output0 > 0.0, true); } } }