mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-01-28 18:04:51 +00:00
Added a generic tracking_loop_filter class
This implements a generic loop filter. Based on the analog PLL filters from Kaplan and Hegarty, with a bilinear (Tustin's) transform from s-plane to z-plane ( 1/s -> T/2 ( 1 + z^-1 )/( 1 - z^-1 ) ) Also added tests. Note the "truth" outputs were derived from an Octave implementation of the loop filter and Octave's builtin filter function
This commit is contained in:
Cillian O'Driscoll
parent
518deb501a
commit
26b18c19ee
@ -39,6 +39,7 @@ set(TRACKING_LIB_SOURCES
|
||||
tracking_2nd_PLL_filter.cc
|
||||
tracking_discriminators.cc
|
||||
tracking_FLL_PLL_filter.cc
|
||||
tracking_loop_filter.cc
|
||||
)
|
||||
|
||||
include_directories(
|
||||
|
||||
284
src/algorithms/tracking/libs/tracking_loop_filter.cc
Normal file
284
src/algorithms/tracking/libs/tracking_loop_filter.cc
Normal 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
98
src/algorithms/tracking/libs/tracking_loop_filter.h
Normal 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
|
||||
@ -335,8 +335,9 @@ endif(NOT ${GTEST_DIR_LOCAL})
|
||||
# add_test(acq_test acq_test)
|
||||
|
||||
add_executable(trk_test
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_dll_pll_veml_tracking_test.cc
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/arithmetic/tracking_loop_filter_test.cc
|
||||
)
|
||||
if(NOT ${ENABLE_PACKAGING})
|
||||
set_property(TARGET trk_test PROPERTY EXCLUDE_FROM_ALL TRUE)
|
||||
|
||||
234
src/tests/arithmetic/tracking_loop_filter_test.cc
Normal file
234
src/tests/arithmetic/tracking_loop_filter_test.cc
Normal file
@ -0,0 +1,234 @@
|
||||
/*!
|
||||
* \file tracking_loop_filter_test.cc
|
||||
* \brief This file implements tests for the general loop filter
|
||||
* \author Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
|
||||
*
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* 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 "tracking_2nd_PLL_filter.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
TEST(TrackingLoopFilterTest, FirstOrderLoop)
|
||||
{
|
||||
int loop_order = 1;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = false;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float g1 = noise_bandwidth*4.0;
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
|
||||
ASSERT_FLOAT_EQ( result, sample_data[i]*g1 );
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrackingLoopFilterTest, FirstOrderLoopWithLastIntegrator)
|
||||
{
|
||||
int loop_order = 1;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = true;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 0.01, 0.02, 0.02, 0.02 };
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float g1 = noise_bandwidth*4.0;
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
TEST(TrackingLoopFilterTest, SecondOrderLoop)
|
||||
{
|
||||
int loop_order = 2;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = false;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 13.37778, 0.0889, 0.0889, 0.0889 };
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrackingLoopFilterTest, SecondOrderLoopWithLastIntegrator)
|
||||
{
|
||||
int loop_order = 2;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = true;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 0.006689, 0.013422, 0.013511, 0.013600 };
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float g1 = noise_bandwidth*4.0;
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
|
||||
TEST(TrackingLoopFilterTest, ThirdOrderLoop)
|
||||
{
|
||||
int loop_order = 3;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = false;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 15.31877, 0.04494, 0.04520, 0.04546};
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrackingLoopFilterTest, ThirdOrderLoopWithLastIntegrator)
|
||||
{
|
||||
int loop_order = 3;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = true;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 0.007659, 0.015341, 0.015386, 0.015432};
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float g1 = noise_bandwidth*4.0;
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user