2015-10-24 08:42:04 +00:00
|
|
|
/*!
|
|
|
|
* \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
|
2019-03-19 19:16:59 +00:00
|
|
|
* on the bilinear transform of the standard Wiener filter.
|
2015-10-24 08:42:04 +00:00
|
|
|
*
|
|
|
|
* -------------------------------------------------------------------------
|
|
|
|
*
|
2018-05-13 20:49:11 +00:00
|
|
|
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
2015-10-24 08:42:04 +00:00
|
|
|
*
|
|
|
|
* 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
|
2018-05-13 20:49:11 +00:00
|
|
|
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
2015-10-24 08:42:04 +00:00
|
|
|
*
|
|
|
|
* -------------------------------------------------------------------------
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
#include "tracking_loop_filter.h"
|
|
|
|
#include <glog/logging.h>
|
2018-12-09 21:00:09 +00:00
|
|
|
#include <cmath>
|
2015-10-24 08:42:04 +00:00
|
|
|
|
2019-03-19 19:16:59 +00:00
|
|
|
const int MAX_LOOP_ORDER = 3;
|
|
|
|
const int MAX_LOOP_HISTORY_LENGTH = 4;
|
2015-10-24 08:42:04 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
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)
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
2018-03-12 14:16:39 +00:00
|
|
|
d_inputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0);
|
|
|
|
d_outputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0);
|
2015-10-24 08:42:04 +00:00
|
|
|
update_coefficients();
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2015-10-24 08:42:04 +00:00
|
|
|
Tracking_loop_filter::Tracking_loop_filter()
|
2018-03-03 01:03:39 +00:00
|
|
|
: d_loop_order(2),
|
|
|
|
d_current_index(0),
|
|
|
|
d_include_last_integrator(false),
|
|
|
|
d_noise_bandwidth(15.0),
|
|
|
|
d_update_interval(0.001)
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
2018-03-12 14:16:39 +00:00
|
|
|
d_inputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0);
|
|
|
|
d_outputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0);
|
2015-10-24 08:42:04 +00:00
|
|
|
update_coefficients();
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
float Tracking_loop_filter::apply(float current_input)
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
|
|
|
// Now apply the filter coefficients:
|
2018-03-12 14:16:39 +00:00
|
|
|
float result = 0.0;
|
2015-10-24 08:42:04 +00:00
|
|
|
|
2019-03-19 19:16:59 +00:00
|
|
|
// Handle the old outputs first:
|
2018-03-03 01:03:39 +00:00
|
|
|
for (unsigned int ii = 0; ii < d_output_coefficients.size(); ++ii)
|
|
|
|
{
|
2018-03-12 14:16:39 +00:00
|
|
|
result += d_output_coefficients[ii] * d_outputs[(d_current_index + ii) % MAX_LOOP_HISTORY_LENGTH];
|
2018-03-03 01:03:39 +00:00
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
|
|
|
|
// 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--;
|
2018-03-03 01:03:39 +00:00
|
|
|
if (d_current_index < 0)
|
|
|
|
{
|
2018-03-12 14:16:39 +00:00
|
|
|
d_current_index += MAX_LOOP_HISTORY_LENGTH;
|
2018-03-03 01:03:39 +00:00
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
|
|
|
|
d_inputs[d_current_index] = current_input;
|
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
for (unsigned int ii = 0; ii < d_input_coefficients.size(); ++ii)
|
|
|
|
{
|
2018-03-12 14:16:39 +00:00
|
|
|
result += d_input_coefficients[ii] * d_inputs[(d_current_index + ii) % MAX_LOOP_HISTORY_LENGTH];
|
2018-03-03 01:03:39 +00:00
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
|
|
|
|
d_outputs[d_current_index] = result;
|
|
|
|
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
void Tracking_loop_filter::update_coefficients(void)
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
|
|
|
// Analog gains:
|
|
|
|
float g1;
|
|
|
|
float g2;
|
|
|
|
float g3;
|
|
|
|
|
|
|
|
// Natural frequency
|
|
|
|
float wn;
|
|
|
|
float T = d_update_interval;
|
|
|
|
|
2018-03-12 14:16:39 +00:00
|
|
|
float zeta = 1.0 / std::sqrt(2.0);
|
2015-10-24 08:42:04 +00:00
|
|
|
|
|
|
|
// 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.
|
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
switch (d_loop_order)
|
|
|
|
{
|
2015-10-24 08:42:04 +00:00
|
|
|
case 1:
|
2018-03-03 01:03:39 +00:00
|
|
|
wn = d_noise_bandwidth * 4.0;
|
2015-10-24 08:42:04 +00:00
|
|
|
g1 = wn;
|
2018-03-03 01:03:39 +00:00
|
|
|
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);
|
2018-03-12 14:16:39 +00:00
|
|
|
d_output_coefficients[0] = 1.0;
|
2018-03-03 01:03:39 +00:00
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
else
|
2018-03-03 01:03:39 +00:00
|
|
|
{
|
|
|
|
d_input_coefficients.resize(1);
|
|
|
|
d_input_coefficients[0] = g1;
|
2015-10-24 08:42:04 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
d_output_coefficients.resize(0);
|
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
break;
|
|
|
|
case 2:
|
2018-03-12 14:16:39 +00:00
|
|
|
wn = d_noise_bandwidth * (8.0 * zeta) / (4.0 * zeta * zeta + 1.0);
|
2018-03-03 01:03:39 +00:00
|
|
|
g1 = wn * wn;
|
2018-03-12 14:16:39 +00:00
|
|
|
g2 = wn * 2.0 * zeta;
|
2018-03-03 01:03:39 +00:00
|
|
|
if (d_include_last_integrator)
|
|
|
|
{
|
|
|
|
d_input_coefficients.resize(3);
|
2018-03-12 14:16:39 +00:00
|
|
|
d_input_coefficients[0] = T / 2.0 * (g1 * T / 2.0 + g2);
|
|
|
|
d_input_coefficients[1] = T * T / 2.0 * g1;
|
|
|
|
d_input_coefficients[2] = T / 2.0 * (g1 * T / 2.0 - g2);
|
2018-03-03 01:03:39 +00:00
|
|
|
|
|
|
|
d_output_coefficients.resize(2);
|
2018-03-12 14:16:39 +00:00
|
|
|
d_output_coefficients[0] = 2.0;
|
|
|
|
d_output_coefficients[1] = -1.0;
|
2018-03-03 01:03:39 +00:00
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
else
|
2018-03-03 01:03:39 +00:00
|
|
|
{
|
|
|
|
d_input_coefficients.resize(2);
|
|
|
|
d_input_coefficients[0] = (g1 * T / 2.0 + g2);
|
2018-03-12 14:16:39 +00:00
|
|
|
d_input_coefficients[1] = g1 * T / 2.0 - g2;
|
2018-03-03 01:03:39 +00:00
|
|
|
|
|
|
|
d_output_coefficients.resize(1);
|
2018-03-12 14:16:39 +00:00
|
|
|
d_output_coefficients[0] = 1.0;
|
2018-03-03 01:03:39 +00:00
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
break;
|
|
|
|
case 3:
|
2018-03-03 01:03:39 +00:00
|
|
|
wn = d_noise_bandwidth / 0.7845; // From Kaplan
|
2015-10-24 08:42:04 +00:00
|
|
|
float a3 = 1.1;
|
|
|
|
float b3 = 2.4;
|
2018-03-03 01:03:39 +00:00
|
|
|
g1 = wn * wn * wn;
|
|
|
|
g2 = a3 * wn * wn;
|
|
|
|
g3 = b3 * wn;
|
|
|
|
|
|
|
|
if (d_include_last_integrator)
|
|
|
|
{
|
|
|
|
d_input_coefficients.resize(4);
|
2018-03-12 14:16:39 +00:00
|
|
|
d_input_coefficients[0] = T / 2.0 * (g3 + T / 2.0 * (g2 + T / 2.0 * g1));
|
|
|
|
d_input_coefficients[1] = T / 2.0 * (-g3 + T / 2.0 * (g2 + 3.0 * T / 2.0 * g1));
|
|
|
|
d_input_coefficients[2] = T / 2.0 * (-g3 - T / 2.0 * (g2 - 3.0 * T / 2.0 * g1));
|
|
|
|
d_input_coefficients[3] = T / 2.0 * (g3 - T / 2.0 * (g2 - T / 2.0 * g1));
|
2018-03-03 01:03:39 +00:00
|
|
|
|
|
|
|
d_output_coefficients.resize(3);
|
2018-03-12 14:16:39 +00:00
|
|
|
d_output_coefficients[0] = 3.0;
|
|
|
|
d_output_coefficients[1] = -3.0;
|
|
|
|
d_output_coefficients[2] = 1.0;
|
2018-03-03 01:03:39 +00:00
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
else
|
2018-03-03 01:03:39 +00:00
|
|
|
{
|
|
|
|
d_input_coefficients.resize(3);
|
2018-03-12 14:16:39 +00:00
|
|
|
d_input_coefficients[0] = g3 + T / 2.0 * (g2 + T / 2.0 * g1);
|
|
|
|
d_input_coefficients[1] = g1 * T * T / 2.0 - 2.0 * g3;
|
|
|
|
d_input_coefficients[2] = g3 + T / 2.0 * (-g2 + T / 2.0 * g1);
|
2015-10-24 08:42:04 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
d_output_coefficients.resize(2);
|
2018-03-12 14:16:39 +00:00
|
|
|
d_output_coefficients[0] = 2.0;
|
|
|
|
d_output_coefficients[1] = -1.0;
|
2018-03-03 01:03:39 +00:00
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
break;
|
2018-03-03 01:03:39 +00:00
|
|
|
};
|
2015-10-24 08:42:04 +00:00
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
void Tracking_loop_filter::set_noise_bandwidth(float noise_bandwidth)
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
|
|
|
d_noise_bandwidth = noise_bandwidth;
|
|
|
|
update_coefficients();
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
float Tracking_loop_filter::get_noise_bandwidth(void) const
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
|
|
|
return d_noise_bandwidth;
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
void Tracking_loop_filter::set_update_interval(float update_interval)
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
|
|
|
d_update_interval = update_interval;
|
|
|
|
update_coefficients();
|
|
|
|
}
|
|
|
|
|
2019-07-07 19:44:58 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
float Tracking_loop_filter::get_update_interval(void) const
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
|
|
|
return d_update_interval;
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
void Tracking_loop_filter::set_include_last_integrator(bool include_last_integrator)
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
|
|
|
d_include_last_integrator = include_last_integrator;
|
|
|
|
update_coefficients();
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
bool Tracking_loop_filter::get_include_last_integrator(void) const
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
|
|
|
return d_include_last_integrator;
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
void Tracking_loop_filter::set_order(int loop_order)
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
2018-03-12 14:16:39 +00:00
|
|
|
if (loop_order < 1 or loop_order > MAX_LOOP_ORDER)
|
2018-03-03 01:03:39 +00:00
|
|
|
{
|
2019-03-19 19:16:59 +00:00
|
|
|
LOG(WARNING) << "Ignoring attempt to set loop order to " << loop_order
|
|
|
|
<< ". Maximum allowed order is: " << MAX_LOOP_ORDER
|
|
|
|
<< ". Not changing current value of " << d_loop_order;
|
2018-03-03 01:03:39 +00:00
|
|
|
return;
|
|
|
|
}
|
2015-10-24 08:42:04 +00:00
|
|
|
|
|
|
|
d_loop_order = loop_order;
|
|
|
|
update_coefficients();
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
int Tracking_loop_filter::get_order(void) const
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
|
|
|
return d_loop_order;
|
|
|
|
}
|
|
|
|
|
2018-12-08 17:49:31 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
void Tracking_loop_filter::initialize(float initial_output)
|
2015-10-24 08:42:04 +00:00
|
|
|
{
|
2018-03-12 14:16:39 +00:00
|
|
|
d_inputs.assign(MAX_LOOP_HISTORY_LENGTH, 0.0);
|
|
|
|
d_outputs.assign(MAX_LOOP_HISTORY_LENGTH, initial_output);
|
|
|
|
d_current_index = MAX_LOOP_HISTORY_LENGTH - 1;
|
2015-10-24 08:42:04 +00:00
|
|
|
}
|