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 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/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/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc index 56824a840..fe7b70878 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc @@ -280,7 +280,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init() d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_doppler_step = 0U; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; d_mag = 0.0; d_input_power = 0.0; const double GALILEO_TWO_PI = 6.283185307179600; @@ -328,7 +329,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_state(int state) { d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; @@ -376,7 +378,8 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items //restart acquisition variables d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; @@ -633,7 +636,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; - + d_gnss_synchro->Acq_doppler_step = d_doppler_step; // 5- Compute the test statistics and compare to the threshold d_test_statistics = d_mag / d_input_power; } diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc index ae5be3f95..318f6ad46 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc @@ -151,10 +151,10 @@ void galileo_pcps_8ms_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - + d_gnss_synchro->Acq_doppler_step = 0U; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; d_mag = 0.0; d_input_power = 0.0; const double GALILEO_TWO_PI = 6.283185307179600; @@ -188,7 +188,8 @@ void galileo_pcps_8ms_acquisition_cc::set_state(int state) { d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; @@ -219,7 +220,8 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items, //restart acquisition variables d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; @@ -328,6 +330,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items, d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; } // Record results to file if required diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index 66ef6b758..b1b6d699a 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -261,7 +261,7 @@ void pcps_acquisition::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - + d_gnss_synchro->Acq_doppler_step = 0U; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0ULL; @@ -334,6 +334,7 @@ void pcps_acquisition::set_state(int32_t state) d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_mag = 0.0; d_input_power = 0.0; d_test_statistics = 0.0; @@ -725,6 +726,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) d_gnss_synchro->Acq_delay_samples = static_cast(std::fmod(static_cast(indext), acq_parameters.samples_per_code)); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_gnss_synchro->Acq_samplestamp_samples = samp_count; + d_gnss_synchro->Acq_doppler_step = acq_parameters.doppler_step2; } lk.lock(); @@ -865,6 +867,7 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)), d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_mag = 0.0; d_input_power = 0.0; d_test_statistics = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc index 5b81e1929..a056436a5 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc @@ -180,10 +180,10 @@ void pcps_acquisition_fine_doppler_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - + d_gnss_synchro->Acq_doppler_step = 0U; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; d_state = 0; } @@ -295,6 +295,7 @@ double pcps_acquisition_fine_doppler_cc::compute_CAF() d_gnss_synchro->Acq_delay_samples = static_cast(index_time); d_gnss_synchro->Acq_doppler_hz = static_cast(index_doppler * d_doppler_step - d_config_doppler_max); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; return d_test_statistics; } @@ -461,7 +462,8 @@ void pcps_acquisition_fine_doppler_cc::set_state(int state) { d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_test_statistics = 0.0; d_active = true; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc index 2609670a7..31c28c319 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc @@ -150,10 +150,10 @@ void pcps_assisted_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - + d_gnss_synchro->Acq_doppler_step = 0U; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; d_input_power = 0.0; d_state = 0; @@ -279,6 +279,7 @@ double pcps_assisted_acquisition_cc::search_maximum() d_gnss_synchro->Acq_delay_samples = static_cast(index_time); d_gnss_synchro->Acq_doppler_hz = static_cast(index_doppler * d_doppler_step + d_doppler_min); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; // Record results to file if required if (d_dump) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc index bae39c693..34228b89b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc @@ -165,10 +165,10 @@ void pcps_cccwsr_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - + d_gnss_synchro->Acq_doppler_step = 0U; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; d_mag = 0.0; d_input_power = 0.0; @@ -203,7 +203,8 @@ void pcps_cccwsr_acquisition_cc::set_state(int state) { d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; @@ -234,7 +235,8 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items, //restart acquisition variables d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; @@ -354,6 +356,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items, d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; } // Record results to file if required diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc index 962dee76e..aa8bceffb 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc @@ -290,10 +290,10 @@ void pcps_opencl_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - + d_gnss_synchro->Acq_doppler_step = 0U; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; d_mag = 0.0; d_input_power = 0.0; @@ -450,6 +450,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk() d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_gnss_synchro->Acq_samplestamp_samples = samplestamp; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; // 5- Compute the test statistics and compare to the threshold //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; @@ -613,6 +614,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl() d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_gnss_synchro->Acq_samplestamp_samples = samplestamp; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; // 5- Compute the test statistics and compare to the threshold //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; @@ -676,7 +678,8 @@ void pcps_opencl_acquisition_cc::set_state(int state) { d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; @@ -708,7 +711,8 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items, //restart acquisition variables d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc index 1c9466db1..06a91dccb 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc @@ -199,7 +199,8 @@ void pcps_quicksync_acquisition_cc::init() //DLOG(INFO) << "START init"; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_mag = 0.0; d_input_power = 0.0; @@ -236,7 +237,8 @@ void pcps_quicksync_acquisition_cc::set_state(int state) { d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; @@ -279,7 +281,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items, //restart acquisition variables d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_well_count = 0; d_mag = 0.0; d_input_power = 0.0; @@ -456,6 +459,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items, d_gnss_synchro->Acq_delay_samples = static_cast(d_possible_delay[indext]); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; /* 5- Compute the test statistics and compare to the threshold d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/ d_test_statistics = d_mag / d_input_power; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc index 68694659e..77a714de4 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc @@ -166,10 +166,10 @@ void pcps_tong_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - + d_gnss_synchro->Acq_doppler_step = 0U; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; d_mag = 0.0; d_input_power = 0.0; @@ -211,7 +211,8 @@ void pcps_tong_acquisition_cc::set_state(int state) { d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_dwell_count = 0; d_tong_count = d_tong_init_val; d_mag = 0.0; @@ -250,7 +251,8 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items, //restart acquisition variables d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; + d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->Acq_doppler_step = 0U; d_dwell_count = 0; d_tong_count = d_tong_init_val; d_mag = 0.0; @@ -345,6 +347,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items, d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; } // Record results to file if required 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..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 @@ -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 @@ -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; 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/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index fe4b9292a..ae1754aa2 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -35,6 +35,7 @@ set(TRACKING_ADAPTER_SOURCES gps_l2_m_dll_pll_tracking.cc glonass_l1_ca_dll_pll_tracking.cc glonass_l1_ca_dll_pll_c_aid_tracking.cc + gps_l1_ca_kf_tracking.cc gps_l5_dll_pll_tracking.cc glonass_l2_ca_dll_pll_tracking.cc glonass_l2_ca_dll_pll_c_aid_tracking.cc @@ -49,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/adapters/gps_l1_ca_kf_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc new file mode 100644 index 000000000..38b3e3cad --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -0,0 +1,174 @@ +/*! + * \file gps_l1_ca_kf_tracking.cc + * \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 + * + * 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-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 "gps_l1_ca_kf_tracking.h" +#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) +{ + DLOG(INFO) << "role " << role; + //################# CONFIGURATION PARAMETERS ######################## + int order; + 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 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); + 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); + 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); + 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) + { + item_size_ = sizeof(gr_complex); + tracking_ = gps_l1_ca_kf_make_tracking_cc( + order, + f_if, + fs_in, + vector_length, + dump, + dump_filename, + dll_bw_hz, + early_late_space_chips, + bce_run, + bce_ptrans, + bce_strans, + bce_nu, + bce_kappa); + } + 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..a687b8a9e --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h @@ -0,0 +1,105 @@ +/*! + * \file GPS_L1_CA_KF_Tracking.h + * \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 + * + * 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-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_GPS_L1_CA_KF_TRACKING_H_ +#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ + +#include "gps_l1_ca_kf_tracking_cc.h" +#include "tracking_interface.h" +#include + +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 a0bd80004..b1a67d6bb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -34,6 +34,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 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 @@ -48,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/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..740a11e84 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -0,0 +1,956 @@ +/*! + * \file gps_l1_ca_kf_tracking_cc.cc + * \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 + * + * 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-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 "gps_l1_ca_kf_tracking_cc.h" +#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 +#include +#include +#include +#include + + +using google::LogMessage; + +gps_l1_ca_kf_tracking_cc_sptr +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, + 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, + bce_run, bce_ptrans, bce_strans, bce_nu, bce_kappa)); +} + + +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( + 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, + 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")); + 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; + 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); + + // --- 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 (int32_t 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; + // define residual carrier phase covariance + d_carr_phase_sigma2 = 0.0; + + // sample synchronization + d_sample_counter = 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[FLAGS_cn0_samples]; + d_carrier_lock_test = 1; + d_CN0_SNV_dB_Hz = 0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = FLAGS_carrier_lock_th; + + 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_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; + 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)); + + // Kalman filter initialization (receiver initialization) + + double CN_dB_Hz = 30; + 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; + 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; + kf_P_x_ini(1, 1) = sigma2_doppler; + + kf_R = arma::zeros(1, 1); + kf_R(0, 0) = sigma2_phase_detector_cycles2; + + kf_Q = arma::zeros(2, 2); + 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); + kf_F(0, 0) = 1.0; + kf_F(0, 1) = GPS_TWO_PI * GPS_L1_CA_CODE_PERIOD; + kf_F(1, 0) = 0.0; + kf_F(1, 1) = 1.0; + + kf_H = arma::zeros(1, 2); + kf_H(0, 0) = 1.0; + + 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) + { + 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(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.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; + 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.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() +{ + /* + * correct the code phase according to the delay between acq and trk + */ + 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(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)); + } + + 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; + 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_dopplerrate_hz2 = 0; + d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + + // 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); + + multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); + for (int32_t 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_carr_phase_sigma2 = 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(); + } + } + 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) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } +} + + +int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile() +{ + // READ DUMP FILE + std::ifstream::pos_type size; + 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 + { + 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 + int64_t 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_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]; + 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]; + 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]; + 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]; + uint32_t *PRN = new uint32_t[num_epoch]; + + try + { + if (dump_file.is_open()) + { + 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)); + 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(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)); + 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)); + 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(uint32_t)); + } + } + dump_file.close(); + } + 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; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + 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; + 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_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); + + 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("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); + + 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_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_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("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); + + 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_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); + + 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_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_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_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_SINGLE, MAT_T_SINGLE, 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_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + 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_noise_sigma2; + 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(uint32_t 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) + { + if (!d_dump_file.is_open()) + { + 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; +} + + +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 + d_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) + 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)); + int32_t 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) = d_carrier_doppler_hz; + if (kf_x.n_elem > 2) + { + 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; + } + + // ################# 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] + 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; + + 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 + 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; + } + d_carr_phase_sigma2 = kf_R_est(0, 0); + + // ################## 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, 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 + 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 + { + 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 + 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; + 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 + { + 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 + static_cast(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; + 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(uint64_t)); + // 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_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)); + // Kalman commands + 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)); + // 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 + static_cast(d_current_prn_length_samples)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // PRN + uint32_t prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(uint32_t)); + } + 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 new file mode 100644 index 000000000..64f22ee26 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -0,0 +1,221 @@ +/*! + * \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 + * + * 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-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_GPS_L1_CA_KF_TRACKING_CC_H +#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H + +#include "gnss_synchro.h" +#include "tracking_2nd_DLL_filter.h" +#include "tracking_2nd_PLL_filter.h" +#include "cpu_multicorrelator_real_codes.h" +#include "bayesian_estimation.h" +#include +#include +#include +#include +#include + +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(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, + uint32_t bce_ptrans, + uint32_t bce_strans, + int32_t bce_nu, + int32_t bce_kappa); + + +/*! + * \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(uint32_t 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(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, + uint32_t bce_ptrans, + uint32_t bce_strans, + int32_t bce_nu, + int32_t bce_kappa); + + 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, + uint32_t bce_ptrans, + uint32_t bce_strans, + int32_t bce_nu, + int32_t bce_kappa); + + // tracking configuration vars + uint32_t d_order; + uint32_t d_vector_length; + bool d_dump; + + Gnss_Synchro* d_acquisition_gnss_synchro; + uint32_t d_channel; + + int64_t d_if_freq; + int64_t 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::mat kf_K; // Kalman gain matrix + + // Bayesian estimator + Bayesian_estimator bayes_estimator; + 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; + 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; + + // acquisition + double d_acq_carrier_doppler_step_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; + // correlator + int32_t 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_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; + + // PRN period in samples + int32_t d_current_prn_length_samples; + + // processing samples counters + uint64_t d_sample_counter; + uint64_t d_acq_sample_stamp; + + // CN0 estimation and lock detector + 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; + int32_t 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; + + int32_t save_matfile(); +}; + +#endif // GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 593bfe55f..6ee4d4fce 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) @@ -56,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} diff --git a/src/algorithms/tracking/libs/bayesian_estimation.cc b/src/algorithms/tracking/libs/bayesian_estimation.cc new file mode 100644 index 000000000..30e7d45ec --- /dev/null +++ b/src/algorithms/tracking/libs/bayesian_estimation.cc @@ -0,0 +1,187 @@ +/*! + * \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" + + +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) +{ + 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(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; + 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(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; + 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 + */ +void Bayesian_estimator::update_sequential(const 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(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 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::mat Bayesian_estimator::get_mu_est() const +{ + return mu_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 new file mode 100644 index 000000000..002e7a75c --- /dev/null +++ b/src/algorithms/tracking/libs/bayesian_estimation.h @@ -0,0 +1,85 @@ +/*! + * \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(const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0); + ~Bayesian_estimator(); + + 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(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() const; + arma::mat get_Psi_est() const; + +private: + arma::vec mu_est; + arma::mat Psi_est; + + arma::vec mu_prior; + int kappa_prior; + int nu_prior; + arma::mat Psi_prior; +}; + +#endif diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 37625af4d..7ef051d26 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -104,6 +104,7 @@ #include "sbas_l1_telemetry_decoder.h" #include "hybrid_observables.h" #include "rtklib_pvt.h" +#include "gps_l1_ca_kf_tracking.h" #if RAW_UDP #include "custom_udp_signal_source.h" @@ -1501,6 +1502,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, @@ -1876,6 +1883,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, 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/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/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"); diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 6db984477..9209c30b4 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -53,6 +53,7 @@ public: double Acq_delay_samples; //!< Set by Acquisition processing block double Acq_doppler_hz; //!< Set by Acquisition processing block uint64_t Acq_samplestamp_samples; //!< Set by Acquisition processing block + uint32_t Acq_doppler_step; //!< Set by Acquisition processing block bool Flag_valid_acquisition; //!< Set by Acquisition processing block // Tracking @@ -96,6 +97,7 @@ public: ar& Acq_delay_samples; ar& Acq_doppler_hz; ar& Acq_samplestamp_samples; + ar& Acq_doppler_step; ar& Flag_valid_acquisition; // Tracking ar& fs; 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. + * + * ------------------------------------------------------------------------- + */ + +#include +#include +#include +#include "bayesian_estimation.h" + +#define BAYESIAN_TEST_N_TRIALS 100 +#define BAYESIAN_TEST_ITER 10000 + +TEST(BayesianEstimationPositivityTest, BayesianPositivityTest) +{ + Bayesian_estimator bayes; + 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); + + //--- 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) = static_cast(normal_dist(e1)); + bayes.update_sequential(input); + + arma::mat output = bayes.get_Psi_est(); + double output0 = output(0, 0); + ASSERT_EQ(output0 > 0.0, 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..65e820921 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc @@ -0,0 +1,568 @@ +/*! + * \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-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 +#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"; + + 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; + //trk_dump.restart(); + + 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; + } + } + } +} 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