Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Filter: added test for attenuation adjustment

Browse Source
This commit is contained in:
Andrew Tridgell 2024-01-29 17:12:42 +11:00
parent 8b9fe4d21d
commit 2286f2ce27
4 changed files with 202 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/Filter/tests/plot_harmonictest2.gnu Normal file
View File

@ -0,0 +1,11 @@
#!/usr/bin/gnuplot -persist
set y2tics 0,10
set ytics nomirror
set style data linespoints
set key autotitle
set datafile separator ","
set key autotitle columnhead
set xlabel "Freq(Hz)"
set ylabel "Attenuation(dB)"
#set ylabel2 "PhaseLag(deg)"
plot "harmonicnotch_test2.csv" using 1:2 axis x1y1, "harmonicnotch_test2.csv" using 1:3 axis x1y2

12
libraries/Filter/tests/plot_harmonictest3.gnu Normal file
View File

@ -0,0 +1,12 @@
#!/usr/bin/gnuplot -persist
set y2tics 0,10
set ytics nomirror
set style data linespoints
set key autotitle
set datafile separator ","
set key autotitle columnhead
set xlabel "Freq(Hz)"
set ylabel "Attenuation(dB)"
#set ylabel2 "PhaseLag(deg)"
plot "harmonicnotch_test3.csv" using 1:2 axis x1y1, "harmonicnotch_test3.csv" using 1:3 axis x1y2, "harmonicnotch_test3.csv" using 1:4 axis x1y1, "harmonicnotch_test3.csv" using 1:5 axis x1y2

12
libraries/Filter/tests/plot_harmonictest4.gnu Normal file
View File

@ -0,0 +1,12 @@
#!/usr/bin/gnuplot -persist
set y2tics 0,10
set ytics nomirror
set style data linespoints
set key autotitle
set datafile separator ","
set key autotitle columnhead
set xlabel "Freq(Hz)"
set ylabel "Attenuation(dB)"
set key left bottom
plot "harmonicnotch_test4.csv" using 1:2, "harmonicnotch_test4.csv" using 1:3, "harmonicnotch_test4.csv" using 1:4, "harmonicnotch_test4.csv" using 1:5, "harmonicnotch_test4.csv" using 1:6, "harmonicnotch_test4.csv" using 1:7, "harmonicnotch_test4.csv" using 1:8

172
libraries/Filter/tests/test_notchfilter.cpp
View File

@ -6,6 +6,11 @@
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
static double ratio_to_dB(double ratio)
{
return 10*log(ratio);
}
/*
test if a reset of a notch filter results in no glitch in the following samples
with constant input
@ -74,7 +79,6 @@ TEST(NotchFilterTest, HarmonicNotchTest)
{
const uint8_t num_test_freq = 150;
const uint8_t harmonics = 15;
const uint8_t num_harmonics = __builtin_popcount(harmonics);
const float base_freq = 46;
const float bandwidth = base_freq/2;
const float attenuation_dB = 60;
@ -86,7 +90,6 @@ TEST(NotchFilterTest, HarmonicNotchTest)
const float test_amplitude = 0.7;
const double dt = 1.0 / rate_hz;
bool double_notch = true;
HarmonicNotchFilter<float> filters[num_test_freq][chained_filters] {};
struct {
double in;
@ -105,13 +108,14 @@ TEST(NotchFilterTest, HarmonicNotchTest)
for (uint8_t i=0; i<num_test_freq; i++) {
for (uint8_t c=0; c<chained_filters; c++) {
auto &f = filters[i][c];
f.allocate_filters(num_harmonics, harmonics, double_notch?2:1);
HarmonicNotchFilterParams notch_params {};
notch_params.set_options(uint16_t(HarmonicNotchFilterParams::Options::TreatLowAsMin) |
uint16_t(HarmonicNotchFilterParams::Options::DoubleNotch));
notch_params.set_attenuation(attenuation_dB);
notch_params.set_bandwidth_hz(bandwidth);
notch_params.set_center_freq_hz(base_freq);
notch_params.set_freq_min_ratio(1.0);
notch_params.set_options(uint16_t(HarmonicNotchFilterParams::Options::TreatLowAsMin));
f.allocate_filters(1, harmonics, notch_params.num_composite_notches());
f.init(rate_hz, notch_params);
f.update(base_freq);
}
@ -149,7 +153,7 @@ TEST(NotchFilterTest, HarmonicNotchTest)
for (uint8_t i=0; i<num_test_freq; i++) {
const float freq = i+1;
const float lag_degrees = integrals[i].get_lag_degrees(freq);
fprintf(f, "%.1f,%f,%f\n", freq, integrals[i].out/integrals[i].in, lag_degrees);
fprintf(f, "%.3f,%f,%f\n", freq, integrals[i].out/integrals[i].in, lag_degrees);
}
fclose(f);
printf("Wrote %s\n", csv_file);
@ -163,4 +167,162 @@ TEST(NotchFilterTest, HarmonicNotchTest)
EXPECT_NEAR(integrals[9].get_lag_degrees(10), 112.23, 0.5);
}
/*
calculate attenuation and phase lag for a single harmonic notch filter
*/
static void test_one_filter(float base_freq, float attenuation_dB,
float bandwidth, float test_freq, float source_freq,
uint16_t harmonics, uint16_t options,
float &phase_lag, float &out_attenuation_dB)
{
const uint16_t rate_hz = 2000;
const uint32_t samples = 50000;
const float test_amplitude = 1.0;
const double dt = 1.0 / rate_hz;
HarmonicNotchFilter<float> filter {};
struct {
double last_in;
double last_out;
double v_max;
uint32_t last_crossing;
uint32_t total_lag_samples;
uint32_t lag_count;
float get_lag_degrees(const float freq) const {
const float lag_avg = total_lag_samples/float(lag_count);
return (360.0 * lag_avg * freq) / rate_hz;
}
} integral {};
auto &f = filter;
HarmonicNotchFilterParams notch_params {};
notch_params.set_options(options);
notch_params.set_attenuation(attenuation_dB);
notch_params.set_bandwidth_hz(bandwidth);
notch_params.set_center_freq_hz(base_freq);
notch_params.set_freq_min_ratio(1.0);
f.allocate_filters(1, harmonics, notch_params.num_composite_notches());
f.init(rate_hz, notch_params);
f.update(source_freq);
for (uint32_t s=0; s<samples; s++) {
const double t = s * dt;
const double sample = sin(test_freq * t * 2 * M_PI) * test_amplitude;
float v = sample;
v = f.apply(v);
if (s >= samples/10) {
integral.v_max = MAX(integral.v_max, v);
}
if (sample >= 0 && integral.last_in < 0) {
integral.last_crossing = s;
}
if (v >= 0 && integral.last_out < 0 && integral.last_crossing != 0) {
integral.total_lag_samples += s - integral.last_crossing;
integral.lag_count++;
}
integral.last_in = sample;
integral.last_out = v;
f.update(source_freq);
}
phase_lag = integral.get_lag_degrees(test_freq);
out_attenuation_dB = ratio_to_dB(integral.v_max/test_amplitude);
}
/*
test the test_one_filter function
*/
TEST(NotchFilterTest, HarmonicNotchTest2)
{
const float min_freq = 1.0;
const float max_freq = 200;
const uint16_t steps = 2000;
const char *csv_file = "harmonicnotch_test2.csv";
FILE *f = fopen(csv_file, "w");
fprintf(f, "Freq(Hz),Attenuation(dB),Lag(deg)\n");
for (uint16_t i=0; i<steps; i++) {
float attenuation_dB, phase_lag;
const float test_freq = min_freq + i*(max_freq-min_freq)/steps;
test_one_filter(50, 30, 25, test_freq, 50, 3, uint16_t(HarmonicNotchFilterParams::Options::TripleNotch), phase_lag, attenuation_dB);
fprintf(f, "%.3f,%f,%f\n", test_freq, attenuation_dB, phase_lag);
}
fclose(f);
}
/*
test behaviour with TreatLowAsMin, we expect attenuation to decrease
as we get close to zero source frequency and phase lag to approach
zero
*/
TEST(NotchFilterTest, HarmonicNotchTest3)
{
const float min_freq = 1.0;
const float max_freq = 200;
const uint16_t steps = 1000;
const char *csv_file = "harmonicnotch_test3.csv";
FILE *f = fopen(csv_file, "w");
fprintf(f, "Freq(Hz),Attenuation1(dB),Lag1(deg),Attenuation2(dB),Lag2(deg)\n");
const float source_freq = 35;
for (uint16_t i=0; i<steps; i++) {
float attenuation_dB1, phase_lag1;
float attenuation_dB2, phase_lag2;
const float test_freq = min_freq + i*(max_freq-min_freq)/steps;
// first with TreatLowAsMin
test_one_filter(50, 30, 25, test_freq, source_freq, 1, uint16_t(HarmonicNotchFilterParams::Options::TreatLowAsMin),
phase_lag1,
attenuation_dB1);
// and without
test_one_filter(50, 30, 25, test_freq, source_freq, 1, 0,
phase_lag2,
attenuation_dB2);
fprintf(f, "%.3f,%f,%f,%f,%f\n", test_freq, attenuation_dB1, phase_lag1, attenuation_dB2, phase_lag2);
// the phase lag with the attenuation adjustment should not be
// more than without for frequencies below min freq
if (test_freq < 50) {
EXPECT_LE(phase_lag2, phase_lag1);
}
}
fclose(f);
}
/*
show the progress of a multi-harmonic notch as the source frequency drops below the min frequency
*/
TEST(NotchFilterTest, HarmonicNotchTest4)
{
const float min_freq = 1.0;
const float max_freq = 250;
const uint16_t steps = 1000;
const char *csv_file = "harmonicnotch_test4.csv";
FILE *f = fopen(csv_file, "w");
fprintf(f, "Freq(Hz),60Hz(dB),50Hz(dB),40Hz(dB),30Hz(dB),20Hz(dB),10Hz(dB),0Hz(dB)\n");
for (uint16_t i=0; i<steps; i++) {
float phase_lag;
const float source_freq[7] { 60, 50, 40, 30, 20, 10, 0 };
float attenuations[7];
const float test_freq = min_freq + i*(max_freq-min_freq)/steps;
for (uint8_t F = 0; F < ARRAY_SIZE(source_freq); F++) {
test_one_filter(50, 30, 25, test_freq, source_freq[F], 15, 0,
phase_lag,
attenuations[F]);
}
fprintf(f, "%.3f,%f,%f,%f,%f,%f,%f,%f\n",
test_freq,
attenuations[0], attenuations[1], attenuations[2],
attenuations[3], attenuations[4], attenuations[5],
attenuations[6]);
}
fclose(f);
}
AP_GTEST_MAIN()