mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-30 23:03:05 +00:00
Code Issues Releases Wiki Activity

Add work on the Kalman filter

Browse Source
This commit is contained in:
Carles Fernandez
2018-04-10 10:52:19 +02:00
parent 6345b5dd15
commit 6e19c0c63d
5 changed files with 310 additions and 310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc
View File

@@ -1,18 +1,20 @@
/*! /*!
* \file gps_l1_ca_kf_tracking.cc * \file gps_l1_ca_kf_tracking.cc
* \brief Implementation of an adapter of a DLL+PLL tracking loop block * \brief Implementation of an adapter of a DLL + Kalman carrier
* for GPS L1 C/A to a TrackingInterface * tracking loop block for GPS L1 C/A signals
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com * \author Javier Arribas, 2018. jarribas(at)cttc.es
* Javier Arribas, 2011. jarribas(at)cttc.es * \author Jordi Vila-Valls 2018. jvila(at)cttc.es
* \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
* *
* Code DLL + carrier PLL according to the algorithms described in: * Reference:
* K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, * J. Vila-Valls, P. Closas, M. Navarro and C. Fernández-Prades,
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
* Approach, Birkhauser, 2007 * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
* Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -66,13 +68,11 @@ GpsL1CaKfTracking::GpsL1CaKfTracking(
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
f_if = configuration->property(role + ".if", 0); f_if = configuration->property(role + ".if", 0);
dump = configuration->property(role + ".dump", false); dump = configuration->property(role + ".dump", false);
//pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
//if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); 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); 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); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
std::string default_dump_filename = "./track_ch"; std::string default_dump_filename = "./track_ch";
dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! dump_filename = configuration->property(role + ".dump_filename", default_dump_filename);
vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
//################# MAKE TRACKING GNURadio object ################### //################# MAKE TRACKING GNURadio object ###################

33
src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h
View File

@@ -1,18 +1,20 @@
/*! /*!
* \file GPS_L1_CA_KF_Tracking.h * \file GPS_L1_CA_KF_Tracking.h
* \brief Interface of an adapter of a DLL+PLL tracking loop block * \brief Interface of an adapter of a DLL + Kalman carrier
* for GPS L1 C/A to a TrackingInterface * tracking loop block for GPS L1 C/A signals
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com * \author Javier Arribas, 2018. jarribas(at)cttc.es
* Javier Arribas, 2011. jarribas(at)cttc.es * \author Jordi Vila-Valls 2018. jvila(at)cttc.es
* \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
* *
* Code DLL + carrier PLL according to the algorithms described in: * Reference:
* K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, * J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades,
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
* Approach, Birkhauser, 2007 * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
* Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -38,10 +40,9 @@
#ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ #ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_
#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ #define GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_
#include <string>
#include "tracking_interface.h"
#include "gps_l1_ca_kf_tracking_cc.h" #include "gps_l1_ca_kf_tracking_cc.h"
#include "tracking_interface.h"
#include <string>
class ConfigurationInterface; class ConfigurationInterface;
@@ -52,9 +53,9 @@ class GpsL1CaKfTracking : public TrackingInterface
{ {
public: public:
GpsL1CaKfTracking(ConfigurationInterface* configuration, GpsL1CaKfTracking(ConfigurationInterface* configuration,
std::string role, std::string role,
unsigned int in_streams, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL1CaKfTracking(); virtual ~GpsL1CaKfTracking();
@@ -101,4 +102,4 @@ private:
unsigned int out_streams_; unsigned int out_streams_;
}; };
#endif // GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_ #endif // GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_

534
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc
View File

@@ -1,17 +1,20 @@
/*! /*!
* \file gps_l1_ca_kf_tracking_cc.cc * \file gps_l1_ca_kf_tracking_cc.cc
* \brief Implementation of a code DLL + carrier PLL tracking block * \brief Implementation of a processing block of a DLL + Kalman carrier
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com * tracking loop for GPS L1 C/A signals
* Javier Arribas, 2011. jarribas(at)cttc.es * \author Javier Arribas, 2018. jarribas(at)cttc.es
* \author Jordi Vila-Valls 2018. jvila(at)cttc.es
* \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
* *
* Code DLL + carrier PLL according to the algorithms described in: * Reference:
* [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, * J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades,
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
* Approach, Birkhauser, 2007 * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
* Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -188,14 +191,10 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
kf_R = arma::zeros(1, 1); kf_R = arma::zeros(1, 1);
kf_R(0, 0) = sigma2_phase_detector_cycles2; kf_R(0, 0) = sigma2_phase_detector_cycles2;
//arma::colvec G={pow(GPS_L1_CA_CODE_PERIOD,3)/6.0, pow(GPS_L1_CA_CODE_PERIOD,2)/2.0,GPS_L1_CA_CODE_PERIOD};
kf_Q = arma::zeros(2, 2); kf_Q = arma::zeros(2, 2);
kf_Q(0, 0) = 1e-12; kf_Q(0, 0) = 1e-14;
kf_Q(1, 1) = 1e-2; kf_Q(1, 1) = 1e-2;
// kf_Q=arma::diagmat(pow(GPS_L1_CA_CODE_PERIOD,6)*kf_Q);
//std::cout<<"kf_Q="<<kf_Q<<std::endl;
kf_F = arma::zeros(2, 2); kf_F = arma::zeros(2, 2);
kf_F(0, 0) = 1.0; kf_F(0, 0) = 1.0;
kf_F(0, 1) = GPS_TWO_PI * GPS_L1_CA_CODE_PERIOD; kf_F(0, 1) = GPS_TWO_PI * GPS_L1_CA_CODE_PERIOD;
@@ -224,8 +223,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking()
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp); //-d_vector_length; 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; 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); acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
// Doppler effect // Doppler effect Fd = (C / (C + Vr)) * F
// Fd=(C/(C+Vr))*F
double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ; 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 // new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds; double T_chip_mod_seconds;
@@ -333,255 +331,6 @@ Gps_L1_Ca_Kf_Tracking_cc::~Gps_L1_Ca_Kf_Tracking_cc()
} }
int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
double carr_phase_error_rad = 0.0;
double carr_phase_error_filt_rad = 0.0;
double code_error_chips = 0.0;
double code_error_filt_chips = 0.0;
// Block input data and block output stream pointers
const gr_complex *in = reinterpret_cast<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)
{
int samples_offset;
double acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset;
d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples
d_pull_in = false;
// take into account the carrier cycles accumulated in the pull in signal alignment
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.fs = d_fs_in;
current_synchro_data.correlation_length_ms = 1;
*out[0] = current_synchro_data;
//Kalman filter initialization reset
kf_P_x = kf_P_x_ini;
//Update Kalman states based on acquisition information
kf_x(0) = d_carrier_phase_step_rad * samples_offset;
kf_x(1) = current_synchro_data.Carrier_Doppler_hz;
consume_each(samples_offset); // shift input to perform alignment with local replica
return 1;
}
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// perform carrier wipe-off and compute Early, Prompt and Late correlation
multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in);
multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad,
d_carrier_phase_step_rad,
d_rem_code_phase_chips,
d_code_phase_step_chips,
d_current_prn_length_samples);
// ################## Kalman Carrier Tracking ######################################
//Kalman state prediction (time update)
kf_x_pre = kf_F * kf_x; //state prediction
kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q; //state error covariance prediction
// Update discriminator [rads/Ti]
carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output
//Kalman estimation (measuremant update)
double sigma2_phase_detector_cycles2;
double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0);
sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD));
kf_R(0, 0) = sigma2_phase_detector_cycles2;
kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix
kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain
kf_y(0) = carr_phase_error_rad; // measurement vector
kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation
kf_P_x = (arma::eye(2, 2) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix
d_rem_carr_phase_rad = kf_x(0); // set a new carrier Phase estimation to the NCO
d_carrier_doppler_hz = kf_x(1); // set a new carrier Doppler estimation to the NCO
carr_phase_error_filt_rad = d_rem_carr_phase_rad;
// ################## DLL ##########################################################
// New code Doppler frequency estimation based on carrier frequency estimation
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
// DLL discriminator
code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] early and late
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second]
double T_chip_seconds = 1.0 / static_cast<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 = round(K_blk_samples); // round to a discrete number of samples
//################### NCO COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// carrier phase accumulator
d_acc_carrier_phase_rad -= kf_x(0);
//################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
// remnant code phase [chips]
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample
d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast<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, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{
d_carrier_lock_fail_counter++;
}
else
{
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<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;
}
else
{
for (int n = 0; n < d_n_correlator_taps; n++)
{
d_correlator_outs[n] = gr_complex(0, 0);
}
current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples;
current_synchro_data.System = {'G'};
current_synchro_data.correlation_length_ms = 1;
}
// assign the GNU Radio block output data
current_synchro_data.fs = d_fs_in;
*out[0] = current_synchro_data;
if (d_dump)
{
// MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I;
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
float tmp_VE = 0.0;
float tmp_VL = 0.0;
double tmp_float;
double tmp_double;
unsigned long int tmp_long;
prompt_I = d_correlator_outs[1].real();
prompt_Q = d_correlator_outs[1].imag();
tmp_E = std::abs<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_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// PLL commands
tmp_float = 0.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = 0.0;
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
}
int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() int Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
{ {
// READ DUMP FILE // READ DUMP FILE
@@ -700,7 +449,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
if (reinterpret_cast<long *>(matfp) != NULL) if (reinterpret_cast<long *>(matfp) != NULL)
{ {
size_t dims[2] = {1, static_cast<size_t>(num_epoch)}; 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_E, 0); matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
@@ -716,7 +465,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
@@ -807,12 +556,13 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
void Gps_L1_Ca_Kf_Tracking_cc::set_channel(unsigned int channel) void Gps_L1_Ca_Kf_Tracking_cc::set_channel(unsigned int channel)
{ {
gr::thread::scoped_lock l(d_setlock);
d_channel = channel; d_channel = channel;
LOG(INFO) << "Tracking Channel set to " << d_channel; LOG(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_dump == true) if (d_dump)
{ {
if (d_dump_file.is_open() == false) if (!d_dump_file.is_open())
{ {
try try
{ {
@@ -835,3 +585,251 @@ void Gps_L1_Ca_Kf_Tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro)
{ {
d_acquisition_gnss_synchro = p_gnss_synchro; d_acquisition_gnss_synchro = p_gnss_synchro;
} }
int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
double carr_phase_error_rad = 0.0;
double code_error_chips = 0.0;
double code_error_filt_chips = 0.0;
// Block input data and block output stream pointers
const gr_complex *in = reinterpret_cast<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) = current_synchro_data.Carrier_Doppler_hz;
consume_each(samples_offset); // shift input to perform alignment with local replica
return 1;
}
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// Perform carrier wipe-off and compute Early, Prompt and Late correlation
multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in);
multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad,
d_carrier_phase_step_rad,
d_rem_code_phase_chips,
d_code_phase_step_chips,
d_current_prn_length_samples);
// ################## Kalman Carrier Tracking ######################################
// Kalman state prediction (time update)
kf_x_pre = kf_F * kf_x; //state prediction
kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q; //state error covariance prediction
// Update discriminator [rads/Ti]
carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output
// Kalman estimation (measurement update)
double sigma2_phase_detector_cycles2;
double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0);
sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD));
kf_R(0, 0) = sigma2_phase_detector_cycles2;
kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix
kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain
kf_y(0) = carr_phase_error_rad; // measurement vector
kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation
kf_P_x = (arma::eye(2, 2) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix
d_rem_carr_phase_rad = kf_x(0); // set a new carrier Phase estimation to the NCO
d_carrier_doppler_hz = kf_x(1); // set a new carrier Doppler estimation to the NCO
// ################## DLL ##########################################################
// New code Doppler frequency estimation based on carrier frequency estimation
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
// DLL discriminator
code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] early and late
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second]
double T_chip_seconds = 1.0 / static_cast<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, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{
d_carrier_lock_fail_counter++;
}
else
{
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false;
}
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<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;
}
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_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// PLL commands
tmp_float = static_cast<float>(carr_phase_error_rad * GPS_TWO_PI);
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
}

22
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h
View File

@@ -1,18 +1,20 @@
/*! /*!
* \file gps_l1_ca_dll_pll_tracking_cc.h * \file gps_l1_ca_kf_tracking_cc.cc
* \brief Interface of a code DLL + carrier PLL tracking block * \brief Interface of a processing block of a DLL + Kalman carrier
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com * tracking loop for GPS L1 C/A signals
* Javier Arribas, 2011. jarribas(at)cttc.es * \author Javier Arribas, 2018. jarribas(at)cttc.es
* Cillian O'Driscoll, 2017. cillian.odriscoll(at)gmail.com * \author Jordi Vila-Valls 2018. jvila(at)cttc.es
* \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
* *
* Code DLL + carrier PLL according to the algorithms described in: * Reference:
* K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, * J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades,
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, * "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
* Birkhauser, 2007 * Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
* Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver

7
src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc
View File

@@ -7,7 +7,7 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2012-2017 (see AUTHORS file for a list of contributors) * Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -123,7 +123,7 @@ public:
std::string p4; std::string p4;
std::string p5; std::string p5;
std::string implementation = "GPS_L1_CA_KF_Tracking"; // "GPS_L1_CA_KF_Tracking"; std::string implementation = "GPS_L1_CA_KF_Tracking";
const int baseband_sampling_freq = FLAGS_fs_gen_sps; const int baseband_sampling_freq = FLAGS_fs_gen_sps;
@@ -251,7 +251,6 @@ void GpsL1CAKfTrackingTest::check_results_doppler(arma::vec& true_time_s,
arma::uvec meas_time_s_valid = find(meas_time_s > 0); arma::uvec meas_time_s_valid = find(meas_time_s > 0);
meas_time_s = meas_time_s(meas_time_s_valid); meas_time_s = meas_time_s(meas_time_s_valid);
meas_value = meas_value(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); arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp);
// 2. RMSE // 2. RMSE
@@ -464,6 +463,7 @@ TEST_F(GpsL1CAKfTrackingTest, ValidationOfResults)
nepoch = trk_dump.num_epochs(); nepoch = trk_dump.num_epochs();
std::cout << "Measured observation epochs=" << nepoch << std::endl; std::cout << "Measured observation epochs=" << nepoch << std::endl;
//trk_dump.restart();
arma::vec trk_timestamp_s = arma::zeros(nepoch, 1); arma::vec trk_timestamp_s = arma::zeros(nepoch, 1);
arma::vec trk_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); arma::vec trk_acc_carrier_phase_cycles = arma::zeros(nepoch, 1);
@@ -482,7 +482,6 @@ TEST_F(GpsL1CAKfTrackingTest, ValidationOfResults)
trk_timestamp_s(epoch_counter) = static_cast<double>(trk_dump.PRN_start_sample_count) / static_cast<double>(baseband_sampling_freq); trk_timestamp_s(epoch_counter) = static_cast<double>(trk_dump.PRN_start_sample_count) / static_cast<double>(baseband_sampling_freq);
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI; 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; 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<double>(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast<double>(baseband_sampling_freq), 1.0e-3) / 1.0e-3); double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast<double>(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast<double>(baseband_sampling_freq), 1.0e-3) / 1.0e-3);
trk_prn_delay_chips(epoch_counter) = delay_chips; trk_prn_delay_chips(epoch_counter) = delay_chips;