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

Merge branch 'kf' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez
2018-08-21 11:50:39 +02:00
parent 5d86e68b60 f5c67d0c7d
commit a833abbb8a
29 changed files with 3124 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc
View File

@@ -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<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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;
}

11
src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc
View File

@@ -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<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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

5
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@@ -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<double>(std::fmod(static_cast<float>(indext), acq_parameters.samples_per_code));
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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;

8
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc
View File

@@ -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<double>(index_time);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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;

5
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc
View File

@@ -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<double>(index_time);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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)

11
src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc
View File

@@ -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<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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

12
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc
View File

@@ -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<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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;

10
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc
View File

@@ -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<double>(d_possible_delay[indext]);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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;

11
src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc
View File

@@ -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<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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

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

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

175
src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc Normal file
View File

@@ -0,0 +1,175 @@
/*!
* \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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "gps_l1_ca_kf_tracking.h"
#include "gnss_sdr_flags.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
#include <glog/logging.h>
using google::LogMessage;
GpsL1CaKfTracking::GpsL1CaKfTracking(
ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
{
DLOG(INFO) << "role " << role;
//################# CONFIGURATION PARAMETERS ########################
int 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 pll_bw_hz;
float dll_bw_hz;
float early_late_space_chips;
bool bce_run;
unsigned int bce_ptrans;
unsigned int bce_strans;
int bce_nu;
int bce_kappa;
item_type = configuration->property(role + ".item_type", default_item_type);
order = configuration->property(role + ".order", 2);
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<float>(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_;
}

105
src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#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 <string>
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_

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

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

959
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc Normal file
View File

@@ -0,0 +1,959 @@
/*!
* \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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#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 <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <matio.h>
#include <cmath>
#include <iostream>
#include <memory>
#include <sstream>
using google::LogMessage;
gps_l1_ca_kf_tracking_cc_sptr
gps_l1_ca_kf_make_tracking_cc(
unsigned int order,
long if_freq,
long fs_in,
unsigned int vector_length,
bool dump,
std::string dump_filename,
float dll_bw_hz,
float early_late_space_chips,
bool bce_run,
unsigned int bce_ptrans,
unsigned int bce_strans,
int bce_nu,
int bce_kappa)
{
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<int>(d_vector_length) * 2; //set the required available samples in each call
}
}
Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
unsigned int order,
long if_freq,
long fs_in,
unsigned int vector_length,
bool dump,
std::string dump_filename,
float dll_bw_hz,
float early_late_space_chips,
bool bce_run,
unsigned int bce_ptrans,
unsigned int bce_strans,
int bce_nu,
int bce_kappa) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
// 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<int>(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<float *>(volk_gnsssdr_malloc(static_cast<int>(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<gr_complex *>(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
for (int n = 0; n < d_n_correlator_taps; n++)
{
d_correlator_outs[n] = gr_complex(0, 0);
}
d_local_code_shift_chips = static_cast<float *>(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_sample_counter_seconds = 0;
d_acq_sample_stamp = 0;
d_enable_tracking = false;
d_pull_in = false;
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[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<double>(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<double>(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));
}
long int acq_trk_diff_samples;
double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(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<float>(acq_trk_diff_samples) / static_cast<float>(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<double>(d_code_freq_chips) / static_cast<double>(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<double>(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<double>(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<double>(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<double>(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<int>(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
for (int n = 0; n < d_n_correlator_taps; n++)
{
d_correlator_outs[n] = gr_complex(0, 0);
}
d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples = 0;
d_rem_carr_phase_rad = 0.0;
d_rem_code_phase_chips = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_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();
}
}
int Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
{
// READ DUMP FILE
std::ifstream::pos_type size;
int number_of_double_vars = 1;
int number_of_float_vars = 19;
int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(unsigned int);
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
{
dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem opening dump file:" << e.what() << std::endl;
return 1;
}
// count number of epochs and rewind
long int num_epoch = 0;
if (dump_file.is_open())
{
size = dump_file.tellg();
num_epoch = static_cast<long int>(size) / static_cast<long int>(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];
unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch];
float *acc_carrier_phase_rad = new float[num_epoch];
float *carrier_doppler_hz = new float[num_epoch];
float *carrier_dopplerrate_hz2 = new float[num_epoch];
float *code_freq_chips = new float[num_epoch];
float *carr_error_hz = new float[num_epoch];
float *carr_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];
unsigned int *PRN = new unsigned int[num_epoch];
try
{
if (dump_file.is_open())
{
for (long int i = 0; i < num_epoch; i++)
{
dump_file.read(reinterpret_cast<char *>(&abs_VE[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&abs_E[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&abs_P[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&abs_L[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&abs_VL[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&Prompt_I[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&Prompt_Q[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&PRN_start_sample_count[i]), sizeof(unsigned long int));
dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carrier_dopplerrate_hz2[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&code_freq_chips[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carr_error_hz[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carr_noise_sigma2[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&code_error_chips[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&code_error_filt_chips[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&CN0_SNV_dB_Hz[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carrier_lock_test[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&aux1[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&aux2[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&PRN[i]), sizeof(unsigned int));
}
}
dump_file.close();
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_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<long *>(matfp) != NULL)
{
size_t dims[2] = {1, static_cast<size_t>(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(unsigned int channel)
{
gr::thread::scoped_lock l(d_setlock);
d_channel = channel;
LOG(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump)
{
if (!d_dump_file.is_open())
{
try
{
d_dump_filename.append(boost::lexical_cast<std::string>(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<const gr_complex *>(input_items[0]);
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]);
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data = Gnss_Synchro();
if (d_enable_tracking == true)
{
// Fill the acquisition data
current_synchro_data = *d_acquisition_gnss_synchro;
// Receiver signal alignment
if (d_pull_in == true)
{
// Signal alignment (skip samples until the incoming signal is aligned with local replica)
unsigned long int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
double acq_trk_shif_correction_samples = static_cast<double>(d_current_prn_length_samples) - std::fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_current_prn_length_samples));
int samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
if (samples_offset < 0)
{
samples_offset = 0;
}
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_acq_code_phase_samples;
d_sample_counter += samples_offset; // count for the processed samples
d_pull_in = false;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.fs = d_fs_in;
current_synchro_data.correlation_length_ms = 1;
*out[0] = current_synchro_data;
// Kalman filter initialization reset
kf_P_x = kf_P_x_ini;
// Update Kalman states based on acquisition information
kf_x(0) = d_carrier_phase_step_rad * samples_offset;
kf_x(1) = 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<double>(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<double>(d_fs_in);
double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
d_current_prn_length_samples = static_cast<int>(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<double>(d_fs_in);
// carrier phase accumulator
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(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<double>(d_fs_in);
// remnant code phase [chips]
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(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<double>(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<double>((d_correlator_outs[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples;
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms = 1;
kf_iter++;
}
else
{
for (int n = 0; n < d_n_correlator_taps; n++)
{
d_correlator_outs[n] = gr_complex(0, 0);
}
current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples;
current_synchro_data.System = {'G'};
current_synchro_data.correlation_length_ms = 1;
}
// assign the GNU Radio block output data
current_synchro_data.fs = d_fs_in;
*out[0] = current_synchro_data;
if (d_dump)
{
// MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I;
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
float tmp_VE = 0.0;
float tmp_VL = 0.0;
float tmp_float;
double tmp_double;
unsigned long int tmp_long;
prompt_I = d_correlator_outs[1].real();
prompt_Q = d_correlator_outs[1].imag();
tmp_E = std::abs<float>(d_correlator_outs[0]);
tmp_P = std::abs<float>(d_correlator_outs[1]);
tmp_L = std::abs<float>(d_correlator_outs[2]);
try
{
// EPR
d_dump_file.write(reinterpret_cast<char *>(&tmp_VE), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&tmp_E), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&tmp_P), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&tmp_L), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&tmp_VL), sizeof(float));
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write(reinterpret_cast<char *>(&prompt_I), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&prompt_Q), sizeof(float));
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase
tmp_float = d_acc_carrier_phase_rad;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// carrier and code frequency
tmp_float = d_carrier_doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_carrier_dopplerrate_hz2;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// Kalman commands
tmp_float = static_cast<float>(d_carr_phase_error_rad * GPS_TWO_PI);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = static_cast<float>(d_carr_phase_sigma2);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = static_cast<float>(d_rem_carr_phase_rad * GPS_TWO_PI);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// DLL commands
tmp_float = code_error_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = code_error_filt_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// CN0 and carrier lock test
tmp_float = d_CN0_SNV_dB_Hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_carrier_lock_test;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// PRN
unsigned int prn_ = d_acquisition_gnss_synchro->PRN;
d_dump_file.write(reinterpret_cast<char *>(&prn_), sizeof(unsigned int));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing trk dump file " << e.what();
}
}
consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates
d_sample_counter += d_current_prn_length_samples; // count for the processed samples
return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false
}

221
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H
#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H
#include <fstream>
#include <map>
#include <string>
#include <gnuradio/block.h>
#include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h"
#include <armadillo>
#include "cpu_multicorrelator_real_codes.h"
#include "bayesian_estimation.h"
class Gps_L1_Ca_Kf_Tracking_cc;
typedef boost::shared_ptr<Gps_L1_Ca_Kf_Tracking_cc>
gps_l1_ca_kf_tracking_cc_sptr;
gps_l1_ca_kf_tracking_cc_sptr
gps_l1_ca_kf_make_tracking_cc(unsigned int order,
long if_freq,
long fs_in, unsigned int vector_length,
bool dump,
std::string dump_filename,
float pll_bw_hz,
float early_late_space_chips,
bool bce_run,
unsigned int bce_ptrans,
unsigned int bce_strans,
int bce_nu,
int bce_kappa);
/*!
* \brief This class implements a DLL + PLL tracking loop block
*/
class Gps_L1_Ca_Kf_Tracking_cc : public gr::block
{
public:
~Gps_L1_Ca_Kf_Tracking_cc();
void set_channel(unsigned int channel);
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
void start_tracking();
int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star& input_items, gr_vector_void_star& output_items);
void forecast(int noutput_items, gr_vector_int& ninput_items_required);
private:
friend gps_l1_ca_kf_tracking_cc_sptr
gps_l1_ca_kf_make_tracking_cc(unsigned int order,
long if_freq,
long fs_in, unsigned int vector_length,
bool dump,
std::string dump_filename,
float dll_bw_hz,
float early_late_space_chips,
bool bce_run,
unsigned int bce_ptrans,
unsigned int bce_strans,
int bce_nu,
int bce_kappa);
Gps_L1_Ca_Kf_Tracking_cc(unsigned int order,
long if_freq,
long fs_in, unsigned int vector_length,
bool dump,
std::string dump_filename,
float dll_bw_hz,
float early_late_space_chips,
bool bce_run,
unsigned int bce_ptrans,
unsigned int bce_strans,
int bce_nu,
int bce_kappa);
// tracking configuration vars
unsigned int d_order;
unsigned int d_vector_length;
bool d_dump;
Gnss_Synchro* d_acquisition_gnss_synchro;
unsigned int d_channel;
long d_if_freq;
long d_fs_in;
double d_early_late_spc_chips;
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
double d_rem_code_phase_chips;
double d_rem_carr_phase_rad;
// Kalman filter variables
arma::mat kf_P_x_ini; //initial state error covariance matrix
arma::mat kf_P_x; //state error covariance matrix
arma::mat kf_P_x_pre; //Predicted state error covariance matrix
arma::mat kf_P_y; //innovation covariance matrix
arma::mat kf_F; //state transition matrix
arma::mat kf_H; //system matrix
arma::mat kf_R; //measurement error covariance matrix
arma::mat kf_Q; //system error covariance matrix
arma::colvec kf_x; //state vector
arma::colvec kf_x_pre; //predicted state vector
arma::colvec kf_y; //measurement vector
arma::mat kf_K; //Kalman gain matrix
// Bayesian estimator
Bayesian_estimator bayes_estimator;
arma::mat kf_R_est; //measurement error covariance
unsigned int bayes_ptrans;
unsigned int bayes_strans;
int bayes_nu;
int bayes_kappa;
bool bayes_run;
unsigned int 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
int d_n_correlator_taps;
float* d_ca_code;
float* d_local_code_shift_chips;
gr_complex* d_correlator_outs;
cpu_multicorrelator_real_codes multicorrelator_cpu;
// tracking vars
double d_code_freq_chips;
double d_code_phase_step_chips;
double d_carrier_doppler_hz;
double d_carrier_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
int d_current_prn_length_samples;
//processing samples counters
unsigned long int d_sample_counter;
unsigned long int d_acq_sample_stamp;
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars
bool d_enable_tracking;
bool d_pull_in;
// file dump
std::string d_dump_filename;
std::ofstream d_dump_file;
std::map<std::string, std::string> systemName;
std::string sys;
int save_matfile();
};
#endif //GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H

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

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

189
src/algorithms/tracking/libs/bayesian_estimation.cc Normal file
View File

@@ -0,0 +1,189 @@
/*!
* \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 <ul>
* <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu
* <li> Jordi Vila-Valls 2018. jvila(at)cttc.es
* </ul>
* -------------------------------------------------------------------------
*
* 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 <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "bayesian_estimation.h"
#include <armadillo>
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(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0)
{
mu_prior = mu_prior_0;
kappa_prior = kappa_prior_0;
nu_prior = nu_prior_0;
Psi_prior = Psi_prior_0;
mu_est = mu_prior;
Psi_est = Psi_prior;
}
Bayesian_estimator::~Bayesian_estimator()
{
}
void Bayesian_estimator::init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0)
{
mu_prior = mu_prior_0;
kappa_prior = kappa_prior_0;
nu_prior = nu_prior_0;
Psi_prior = Psi_prior_0;
mu_est = mu_prior;
Psi_est = Psi_prior;
}
/*
* Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in
* the class structure, and update the priors according to the computed posteriors
*/
void Bayesian_estimator::update_sequential(arma::vec data)
{
int K = data.n_cols;
int ny = data.n_rows;
if (mu_prior.is_empty())
{
mu_prior = arma::zeros(ny,1);
}
if (Psi_prior.is_empty())
{
Psi_prior = arma::zeros(ny,ny);
}
arma::vec y_mean = arma::mean(data, 1);
arma::mat Psi_N = arma::zeros(ny, ny);
for (int kk = 0; kk < K; kk++)
{
Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t());
}
arma::vec mu_posterior = (kappa_prior*mu_prior + K*y_mean) / (kappa_prior + K);
int kappa_posterior = kappa_prior + K;
int nu_posterior = nu_prior + K;
arma::mat Psi_posterior = Psi_prior + Psi_N + (kappa_prior*K)/(kappa_prior + K)*(y_mean - mu_prior)*((y_mean - mu_prior).t());
mu_est = mu_posterior;
if ((nu_posterior - ny - 1) > 0)
{
Psi_est = Psi_posterior / (nu_posterior - ny - 1);
}
else
{
Psi_est = Psi_posterior / (nu_posterior + ny + 1);
}
mu_prior = mu_posterior;
kappa_prior = kappa_posterior;
nu_prior = nu_posterior;
Psi_prior = Psi_posterior;
}
/*
* Perform Bayesian noise estimation using a new set of normal-inverse-Wishart priors
* and update the priors according to the computed posteriors
*/
void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0)
{
int K = data.n_cols;
int ny = data.n_rows;
arma::vec y_mean = arma::mean(data, 1);
arma::mat Psi_N = arma::zeros(ny, ny);
for (int kk = 0; kk < K; kk++)
{
Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t());
}
arma::vec mu_posterior = (kappa_prior_0*mu_prior_0 + K*y_mean) / (kappa_prior_0 + K);
int kappa_posterior = kappa_prior_0 + K;
int nu_posterior = nu_prior_0 + K;
arma::mat Psi_posterior = Psi_prior_0 + Psi_N + (kappa_prior_0*K)/(kappa_prior_0 + K)*(y_mean - mu_prior_0)*((y_mean - mu_prior_0).t());
mu_est = mu_posterior;
if ((nu_posterior - ny - 1) > 0)
{
Psi_est = Psi_posterior / (nu_posterior - ny - 1);
}
else
{
Psi_est = Psi_posterior / (nu_posterior + ny + 1);
}
mu_prior = mu_posterior;
kappa_prior = kappa_posterior;
nu_prior = nu_posterior;
Psi_prior = Psi_posterior;
}
arma::mat Bayesian_estimator::get_mu_est()
{
return mu_est;
}
arma::mat Bayesian_estimator::get_Psi_est()
{
return Psi_est;
}

87
src/algorithms/tracking/libs/bayesian_estimation.h Normal file
View File

@@ -0,0 +1,87 @@
/*!
* \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 <ul>
* <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu
* <li> Jordi Vila-Valls 2018. jvila(at)cttc.es
* </ul>
* -------------------------------------------------------------------------
*
* 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 <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_BAYESIAN_ESTIMATION_H_
#define GNSS_SDR_BAYESIAN_ESTIMATION_H_
#include <gnuradio/gr_complex.h>
#include <armadillo>
/*! \brief Bayesian_estimator is an estimator of noise characteristics (i.e. mean, covariance)
*
* Bayesian_estimator is an estimator which performs estimation of noise characteristics from
* a sequence of identically and independently distributed (IID) samples of a stationary
* stochastic process by way of Bayesian inference using conjugate priors. The posterior
* distribution is assumed to be Gaussian with mean \mathbf{\mu} and covariance \hat{\mathbf{C}},
* which has a conjugate prior given by a normal-inverse-Wishart distribution with paramemters
* \mathbf{\mu}_{0}, \kappa_{0}, \nu_{0}, and \mathbf{\Psi}.
*
* [1] TODO: Ref1
*
*/
class Bayesian_estimator
{
public:
Bayesian_estimator();
Bayesian_estimator(int ny);
Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0);
~Bayesian_estimator();
void init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0);
void update_sequential(arma::vec data);
void update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0);
arma::mat get_mu_est();
arma::mat get_Psi_est();
private:
arma::vec mu_est;
arma::mat Psi_est;
arma::vec mu_prior;
int kappa_prior;
int nu_prior;
arma::mat Psi_prior;
};
#endif