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

Merge branch 'contrib' of https://github.com/odrisci/gnss-sdr into

Browse Source 
odrisci-contrib

# Conflicts:
#	src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
#	src/algorithms/tracking/libs/tracking_discriminators.cc
This commit is contained in:
Carles Fernandez
2015-11-30 10:18:09 +01:00
parent ed89b70241 26b18c19ee
commit 74d42250d8
21 changed files with 984 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
View File

@@ -210,10 +210,10 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking()
d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples = 0.0;
d_rem_carr_phase_rad = 0;
d_acc_carrier_phase_rad = 0;
d_rem_carr_phase_rad = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_acc_code_phase_secs = 0;
d_acc_code_phase_secs = 0.0;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_current_prn_length_samples = d_vector_length;
@@ -249,17 +249,17 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
code_phase_step_chips = d_code_freq_chips / (static_cast<double>(d_fs_in));
code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast<double>(d_fs_in));
rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in);
rem_code_phase_half_chips = d_rem_code_phase_samples * (2.0 * d_code_freq_chips / static_cast<double>(d_fs_in));
tcode_half_chips = - rem_code_phase_half_chips;
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
very_early_late_spc_samples = round(d_very_early_late_spc_chips / code_phase_step_chips);
early_late_spc_samples = std::round(d_early_late_spc_chips / code_phase_step_chips);
very_early_late_spc_samples = std::round(d_very_early_late_spc_chips / code_phase_step_chips);
epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples * 2;
for (int i = 0; i < epl_loop_length_samples; i++)
{
associated_chip_index = 2 + round(fmod(tcode_half_chips - 2 * d_very_early_late_spc_chips, code_length_half_chips));
associated_chip_index = 2 + std::round(std::fmod(tcode_half_chips - 2.0 * d_very_early_late_spc_chips, static_cast<double>(code_length_half_chips)));
d_very_early_code[i] = d_ca_code[associated_chip_index];
tcode_half_chips = tcode_half_chips + code_phase_step_half_chips;
}
@@ -273,7 +273,7 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
void galileo_e1_dll_pll_veml_tracking_cc::update_local_carrier()
{
float sin_f, cos_f;
float phase_step_rad = static_cast<float>(2 * GALILEO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
float phase_step_rad = static_cast<float>(2.0 * GALILEO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad);
@@ -310,11 +310,10 @@ galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc()
int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
double carr_error_hz = 0.0;
double carr_error_filt_hz = 0.0;
double code_error_chips = 0.0;
double code_error_filt_chips = 0.0;
double carr_error_hz = 0.0;
double carr_error_filt_hz = 0.0;
double code_error_chips = 0.0;
double code_error_filt_chips = 0.0;
if (d_enable_tracking == true)
{
@@ -327,8 +326,8 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
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);
acq_trk_shif_correction_samples = static_cast<double>(d_current_prn_length_samples) - static_cast<double>(std::fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_current_prn_length_samples)));
samples_offset = static_cast<int>(std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples));
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica
@@ -365,7 +364,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
// ################## PLL ##########################################################
// PLL discriminator
carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_TWO_PI);
carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<double>(GPS_TWO_PI);
// Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation
@@ -376,7 +375,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
//remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
// ################## DLL ##########################################################
// DLL discriminator
@@ -400,7 +399,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
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 samples
d_current_prn_length_samples = static_cast<int>(std::round(K_blk_samples)); //round to a discrete samples
//d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
@@ -457,7 +456,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
//compute remnant code phase samples AFTER the Tracking timestamp
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_current_prn_length_samples); //rounding error < 1 sample
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
@@ -472,10 +471,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
* \todo The stop timer has to be moved to the signal source!
*/
// stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream;
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
if (std::floor(d_sample_counter / d_fs_in) != d_last_seg)
{
d_last_seg = floor(d_sample_counter / d_fs_in);
d_last_seg = std::floor(d_sample_counter / d_fs_in);
if (d_channel == 0)
{
@@ -498,9 +496,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
*/
// stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream;
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
if (std::floor(d_sample_counter / d_fs_in) != d_last_seg)
{
d_last_seg = floor(d_sample_counter / d_fs_in);
d_last_seg = std::floor(d_sample_counter / d_fs_in);
if (d_channel == 0)
{

18
src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h
View File

@@ -126,8 +126,8 @@ private:
long d_if_freq;
long d_fs_in;
double d_early_late_spc_chips;
double d_very_early_late_spc_chips;
float d_early_late_spc_chips;
float d_very_early_late_spc_chips;
gr_complex* d_ca_code;
@@ -146,22 +146,22 @@ private:
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
double d_rem_carr_phase_rad;
float d_rem_carr_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
Tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
double d_acq_code_phase_samples;
double d_acq_carrier_doppler_hz;
float d_acq_code_phase_samples;
float d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// tracking vars
double d_code_freq_chips;
double d_carrier_doppler_hz;
float d_carrier_doppler_hz;
double d_acc_carrier_phase_rad;
double d_acc_code_phase_secs;
@@ -175,9 +175,9 @@ private:
// 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;
float d_carrier_lock_test;
float d_CN0_SNV_dB_Hz;
float d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars

4
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
View File

@@ -297,7 +297,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code()
void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_carrier()
{
float sin_f, cos_f;
float phase_step_rad = static_cast<float>(GPS_TWO_PI) * static_cast<float>(d_carrier_doppler_hz) / static_cast<float>(d_fs_in);
float phase_step_rad = static_cast<float>(GPS_TWO_PI) * ( d_if_freq + d_carrier_doppler_hz ) / static_cast<float>(d_fs_in);
int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad);
@@ -424,7 +424,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
//carrier phase accumulator for (K) doppler estimation
d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * ( d_if_freq + d_carrier_doppler_hz ) * GPS_L1_CA_CODE_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
// ################## DLL ##########################################################

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

@@ -40,6 +40,7 @@ set(TRACKING_LIB_SOURCES
tracking_2nd_PLL_filter.cc
tracking_discriminators.cc
tracking_FLL_PLL_filter.cc
tracking_loop_filter.cc
)
include_directories(

47
src/algorithms/tracking/libs/correlator.cc
View File

@@ -151,3 +151,50 @@ void Correlator::Carrier_wipeoff_and_EPL_volk_custom(int signal_length_samples,
volk_cw_epl_corr_u(input, carrier, E_code, P_code, L_code, E_out, P_out, L_out, signal_length_samples);
}
#endif
void Correlator::Carrier_rotate_and_EPL_volk(int signal_length_samples,
const gr_complex* input,
gr_complex *phase_as_complex,
gr_complex phase_inc_as_complex,
const gr_complex* E_code,
const gr_complex* P_code,
const gr_complex* L_code,
gr_complex* E_out,
gr_complex* P_out,
gr_complex* L_out )
{
gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
volk_32fc_s32fc_x2_rotator_32fc(bb_signal, input, phase_inc_as_complex, phase_as_complex, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples);
volk_free(bb_signal);
}
void Correlator::Carrier_rotate_and_VEPL_volk(int signal_length_samples,
const gr_complex* input,
gr_complex *phase_as_complex,
gr_complex phase_inc_as_complex,
const gr_complex* VE_code,
const gr_complex* E_code,
const gr_complex* P_code,
const gr_complex* L_code,
const gr_complex* VL_code,
gr_complex* VE_out,
gr_complex* E_out,
gr_complex* P_out,
gr_complex* L_out,
gr_complex* VL_out )
{
gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
volk_32fc_s32fc_x2_rotator_32fc(bb_signal, input, phase_inc_as_complex, phase_as_complex, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(VE_out, bb_signal, VE_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(VL_out, bb_signal, VL_code, signal_length_samples);
volk_free(bb_signal);
}

31
src/algorithms/tracking/libs/correlator.h
View File

@@ -56,13 +56,40 @@
class Correlator
{
public:
Correlator();
~Correlator();
void Carrier_wipeoff_and_EPL_generic(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out);
void Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out);
void Carrier_wipeoff_and_VEPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* VE_code, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* VL_code, gr_complex* VE_out, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* VL_out);
// void Carrier_wipeoff_and_EPL_volk_IQ(int prn_length_samples,int integration_time ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out);
void Carrier_wipeoff_and_EPL_volk_IQ(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out);
Correlator();
~Correlator();
void Carrier_rotate_and_EPL_volk(int signal_length_samples,
const gr_complex* input,
gr_complex *phase_as_complex,
gr_complex phase_inc_as_complex,
const gr_complex* E_code,
const gr_complex* P_code,
const gr_complex* L_code,
gr_complex* E_out,
gr_complex* P_out,
gr_complex* L_out );
void Carrier_rotate_and_VEPL_volk(int signal_length_samples,
const gr_complex* input,
gr_complex *phase_as_complex,
gr_complex phase_inc_as_complex,
const gr_complex* VE_code,
const gr_complex* E_code,
const gr_complex* P_code,
const gr_complex* L_code,
const gr_complex* VL_code,
gr_complex* VE_out,
gr_complex* E_out,
gr_complex* P_out,
gr_complex* L_out,
gr_complex* VL_out );
#if USING_VOLK_CW_EPL_CORR_CUSTOM
void Carrier_wipeoff_and_EPL_volk_custom(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out);
#endif

20
src/algorithms/tracking/libs/tracking_discriminators.cc
View File

@@ -91,7 +91,7 @@ double pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
/*
* DLL Noncoherent Early minus Late envelope normalized discriminator:
* \f{equation}
* error=\frac{E-L}{E+L},
* error=\frac{1}{2}\frac{E-L}{E+L},
* \f}
* where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and
* \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips].
@@ -101,7 +101,14 @@ double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1)
double P_early, P_late;
P_early = std::abs(early_s1);
P_late = std::abs(late_s1);
return 0.5*(P_early - P_late) / ((P_early + P_late));
if( P_early + P_late == 0.0 )
{
return 0.0;
}
else
{
return 0.5 * (P_early - P_late) / ((P_early + P_late));
}
}
/*
@@ -118,5 +125,12 @@ double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr
double P_early, P_late;
P_early = std::sqrt(std::norm(very_early_s1) + std::norm(early_s1));
P_late = std::sqrt(std::norm(very_late_s1) + std::norm(late_s1));
return (P_early - P_late) / ((P_early + P_late));
if( P_early + P_late == 0.0 )
{
return 0.0;
}
else
{
return (P_early - P_late) / ((P_early + P_late));
}
}

284
src/algorithms/tracking/libs/tracking_loop_filter.cc Normal file
View File

@@ -0,0 +1,284 @@
/*!
* \file tracking_loop_filter.cc
* \brief Generic 1st to 3rd order loop filter implementation
* \author Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
*
* Class implementing a generic 1st, 2nd or 3rd order loop filter. Based
* on the bilinear transform of the standard Weiner filter.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "tracking_loop_filter.h"
#include <cmath>
#include <glog/logging.h>
#define MAX_LOOP_ORDER 3
#define MAX_HISTORY_LENGTH 4
Tracking_loop_filter::Tracking_loop_filter( float update_interval,
float noise_bandwidth,
int loop_order,
bool include_last_integrator )
: d_loop_order( loop_order ),
d_current_index( 0 ),
d_include_last_integrator( include_last_integrator ),
d_noise_bandwidth( noise_bandwidth ),
d_update_interval( update_interval )
{
d_inputs.resize( MAX_HISTORY_LENGTH, 0.0 );
d_outputs.resize( MAX_HISTORY_LENGTH, 0.0 );
update_coefficients();
}
Tracking_loop_filter::Tracking_loop_filter()
: d_loop_order( 2 ),
d_current_index( 0 ),
d_include_last_integrator( false ),
d_noise_bandwidth( 15.0 ),
d_update_interval( 0.001 )
{
d_inputs.resize( MAX_HISTORY_LENGTH, 0.0 );
d_outputs.resize( MAX_HISTORY_LENGTH, 0.0 );
update_coefficients();
}
Tracking_loop_filter::~Tracking_loop_filter()
{
// Don't need to do anything here
}
float Tracking_loop_filter::apply( float current_input )
{
// Now apply the filter coefficients:
float result = 0;
// Hanlde the old outputs first:
for( unsigned int ii=0; ii < d_output_coefficients.size(); ++ii )
{
result += d_output_coefficients[ii] * d_outputs[ (d_current_index+ii)%MAX_HISTORY_LENGTH ];
}
// Now update the index to handle the inputs.
// DO NOT CHANGE THE ORDER OF THE ABOVE AND BELOW CODE
// SNIPPETS!!!!!!!
// Implementing a sort of circular buffer for the inputs and outputs
// the current input/output is at d_current_index, the nth previous
// input/output is at (d_current_index+n)%d_loop_order
d_current_index--;
if( d_current_index < 0 )
{
d_current_index += MAX_HISTORY_LENGTH;
}
d_inputs[d_current_index] = current_input;
for( unsigned int ii=0; ii < d_input_coefficients.size(); ++ii )
{
result += d_input_coefficients[ii] * d_inputs[ (d_current_index+ii)%MAX_HISTORY_LENGTH ];
}
d_outputs[d_current_index] = result;
return result;
}
void Tracking_loop_filter::update_coefficients( void )
{
// Analog gains:
float g1;
float g2;
float g3;
// Natural frequency
float wn;
float T = d_update_interval;
float zeta = 1/std::sqrt(2);
// The following is based on the bilinear transform approximation of
// the analog integrator. The loop format is from Kaplan & Hegarty
// Table 5.6. The basic concept is that the loop has a cascade of
// integrators:
// 1 for a 1st order loop
// 2 for a 2nd order loop
// 3 for a 3rd order loop
// The bilinear transform approximates 1/s as
// T/2(1 + z^-1)/(1-z^-1) in the z domain.
switch( d_loop_order )
{
case 1:
wn = d_noise_bandwidth*4.0;
g1 = wn;
if( d_include_last_integrator )
{
d_input_coefficients.resize(2);
d_input_coefficients[0] = g1*T/2.0;
d_input_coefficients[1] = g1*T/2.0;
d_output_coefficients.resize(1);
d_output_coefficients[0] = 1;
}
else
{
d_input_coefficients.resize(1);
d_input_coefficients[0] = g1;
d_output_coefficients.resize(0);
}
break;
case 2:
wn = d_noise_bandwidth * (8*zeta)/ (4*zeta*zeta + 1 );
g1 = wn*wn;
g2 = wn*2*zeta;
if( d_include_last_integrator )
{
d_input_coefficients.resize(3);
d_input_coefficients[0] = T/2*( g1*T/2 + g2 );
d_input_coefficients[1] = T*T/2*g1;
d_input_coefficients[2] = T/2*( g1*T/2 - g2 );
d_output_coefficients.resize(2);
d_output_coefficients[0] = 2;
d_output_coefficients[1] = -1;
}
else
{
d_input_coefficients.resize(2);
d_input_coefficients[0] = ( g1*T/2.0+g2 );
d_input_coefficients[1] = g1*T/2-g2;
d_output_coefficients.resize(1);
d_output_coefficients[0] = 1;
}
break;
case 3:
wn = d_noise_bandwidth / 0.7845; // From Kaplan
float a3 = 1.1;
float b3 = 2.4;
g1 = wn*wn*wn;
g2 = a3*wn*wn;
g3 = b3*wn;
if( d_include_last_integrator )
{
d_input_coefficients.resize(4);
d_input_coefficients[0] = T/2*( g3 + T/2*( g2 + T/2*g1 ) );
d_input_coefficients[1] = T/2*( -g3 + T/2*( g2 + 3*T/2*g1 ) );
d_input_coefficients[2] = T/2*( -g3 - T/2*( g2 - 3*T/2*g1 ) );
d_input_coefficients[3] = T/2*( g3 - T/2*( g2 - T/2*g1 ) );
d_output_coefficients.resize(3);
d_output_coefficients[0] = 3;
d_output_coefficients[1] = -3;
d_output_coefficients[2] = 1;
}
else
{
d_input_coefficients.resize(3);
d_input_coefficients[0] = g3 + T/2*( g2 + T/2*g1 );
d_input_coefficients[1] = g1*T*T/2 -2*g3;
d_input_coefficients[2] = g3 + T/2*( -g2 + T/2*g1 );
d_output_coefficients.resize(2);
d_output_coefficients[0] = 2;
d_output_coefficients[1] = -1;
}
break;
};
}
void Tracking_loop_filter::set_noise_bandwidth( float noise_bandwidth )
{
d_noise_bandwidth = noise_bandwidth;
update_coefficients();
}
float Tracking_loop_filter::get_noise_bandwidth( void ) const
{
return d_noise_bandwidth;
}
void Tracking_loop_filter::set_update_interval( float update_interval )
{
d_update_interval = update_interval;
update_coefficients();
}
float Tracking_loop_filter::get_update_interval( void ) const
{
return d_update_interval;
}
void Tracking_loop_filter::set_include_last_integrator( bool include_last_integrator )
{
d_include_last_integrator = include_last_integrator;
update_coefficients();
}
bool Tracking_loop_filter::get_include_last_integrator( void ) const
{
return d_include_last_integrator;
}
void Tracking_loop_filter::set_order( int loop_order )
{
if( loop_order < 1 || loop_order > MAX_LOOP_ORDER )
{
LOG(ERROR) << "Ignoring attempt to set loop order to " << loop_order
<< ". Maximum allowed order is: " << MAX_LOOP_ORDER
<< ". Not changing current value of " << d_loop_order;
return;
}
d_loop_order = loop_order;
update_coefficients();
}
int Tracking_loop_filter::get_order( void ) const
{
return d_loop_order;
}
void Tracking_loop_filter::initialize( float initial_output )
{
d_inputs.assign( MAX_HISTORY_LENGTH, 0.0 );
d_outputs.assign( MAX_HISTORY_LENGTH, initial_output );
d_current_index = MAX_HISTORY_LENGTH - 1;
}

98
src/algorithms/tracking/libs/tracking_loop_filter.h Normal file
View File

@@ -0,0 +1,98 @@
/*!
* \file tracking_loop_filter.h
* \brief Generic 1st to 3rd order loop filter implementation
* \author Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
*
* Class implementing a generic 1st, 2nd or 3rd order loop filter. Based
* on the bilinear transform of the standard Weiner filter.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_TRACKING_LOOP_FILTER_H_
#define GNSS_SDR_TRACKING_LOOP_FILTER_H_
#include <vector>
/*!
* \brief This class implements a generic 1st, 2nd or 3rd order loop filter
*
*/
class Tracking_loop_filter
{
private:
// Store the last inputs and outputs:
std::vector< float > d_inputs;
std::vector< float > d_outputs;
// Store the filter coefficients:
std::vector< float > d_input_coefficients;
std::vector< float > d_output_coefficients;
// The loop order:
int d_loop_order;
// The current index in the i/o arrays:
int d_current_index;
// Should the last integrator be included?
bool d_include_last_integrator;
// The noise bandwidth (in Hz)
// Note this is an approximation only valid when the product of this
// number and the update interval (T) is small.
float d_noise_bandwidth;
// Loop update interval
float d_update_interval;
// Compute the filter coefficients:
void update_coefficients(void);
public:
float get_noise_bandwidth(void) const;
float get_update_interval(void) const;
bool get_include_last_integrator(void) const;
int get_order(void) const;
void set_noise_bandwidth( float noise_bandwidth );
void set_update_interval( float update_interval );
void set_include_last_integrator( bool include_last_integrator );
void set_order( int loop_order );
void initialize(float initial_output = 0.0);
float apply(float current_input );
Tracking_loop_filter(float update_interval, float noise_bandwidth,
int loop_order = 2,
bool include_last_integrator = false );
Tracking_loop_filter();
~Tracking_loop_filter();
};
#endif