Filter: Examples: Add Transfer function check and MATLAB
This commit is contained in:
Iampete1
Andrew Tridgell
parent
4406570391
commit
f77635a5a6
206
libraries/Filter/examples/TransferFunctionCheck/FilterTest.m
Normal file
206
libraries/Filter/examples/TransferFunctionCheck/FilterTest.m
Normal file
@ -0,0 +1,206 @@
|
||||
close all
|
||||
clear
|
||||
clc
|
||||
|
||||
% File generated by the TransferFunctionCheck example
|
||||
filename = "test.csv";
|
||||
|
||||
% There are some anoying warnings for text parsing and matrix solve
|
||||
warning('off','MATLAB:rankDeficientMatrix');
|
||||
warning('off','MATLAB:table:ModifiedAndSavedVarnames');
|
||||
|
||||
data = readtable(filename);
|
||||
|
||||
lines = readlines(filename);
|
||||
data_start = find(startsWith(lines,"f(hz),"));
|
||||
if numel(data_start) ~= 1
|
||||
error("Could not parse type")
|
||||
end
|
||||
lines = lines(1:data_start-1);
|
||||
fprintf("%s\n",lines)
|
||||
|
||||
% Try and work out types
|
||||
filters = {};
|
||||
filter_index = find(startsWith(lines,"LowPassFilterFloat"));
|
||||
for i = 1:numel(filter_index)
|
||||
filters{end+1} = parse_low_pass(lines, filter_index(i)); %#ok<SAGROW>
|
||||
end
|
||||
filter_index = find(startsWith(lines,"LowPassFilter2pFloat"));
|
||||
for i = 1:numel(filter_index)
|
||||
filters{end+1} = parse_biquad(lines, filter_index(i)); %#ok<SAGROW>
|
||||
end
|
||||
filter_index = find(startsWith(lines,"NotchFilterFloat"));
|
||||
for i = 1:numel(filter_index)
|
||||
filters{end+1} = parse_notch(lines, filter_index(i)); %#ok<SAGROW>
|
||||
end
|
||||
|
||||
if numel(filters) == 0
|
||||
error("Failed to parse filters")
|
||||
end
|
||||
|
||||
% Make sure all have the same sample rate
|
||||
for i = 2:numel(filters)
|
||||
if filters{i}.sample_rate ~= filters{1}.sample_rate
|
||||
error("Sample rate mismatch")
|
||||
end
|
||||
end
|
||||
sample_freq = filters{1}.sample_rate;
|
||||
|
||||
% Parse time from headers
|
||||
for i = numel(data.Properties.VariableDescriptions):-1:2
|
||||
time(i-1) = sscanf(data.Properties.VariableDescriptions{i},'t = %f');
|
||||
end
|
||||
if any(isnan(time))
|
||||
error("Got NAN time")
|
||||
end
|
||||
|
||||
% Parse input frequency and output amplitude
|
||||
freq = data{:,1};
|
||||
if any(isnan(freq))
|
||||
error("Got NAN freq")
|
||||
end
|
||||
|
||||
output = data{:,2:end};
|
||||
if any(isnan(output))
|
||||
error("Got NAN time")
|
||||
end
|
||||
|
||||
% Brute force the amplitude and phase from the sweep
|
||||
% This method is currently used in the js filter tool
|
||||
|
||||
% Best fit to sin to get amplitude, phase and DC offset
|
||||
% https://math.stackexchange.com/questions/3926007/least-squares-regression-of-sine-wave
|
||||
% Expecting output to be of same frequency at input
|
||||
% Z = a*sin(t*kt + p) + O
|
||||
% A = a*cos(p)
|
||||
% B = a*sin(p)
|
||||
% S = sin(t*kt)
|
||||
% C = cos(t*kt)
|
||||
|
||||
num_sweep = numel(freq);
|
||||
|
||||
amplitude = zeros(num_sweep,1);
|
||||
phase = zeros(num_sweep,1);
|
||||
|
||||
% Only fit to last 100 points, better aproximation for steady state
|
||||
num_best_fit_points = 100;
|
||||
best_fit_index = false(1,numel(time));
|
||||
best_fit_index(end-num_best_fit_points+1:end) = true;
|
||||
for i = 1:num_sweep
|
||||
|
||||
X = [sin(time(best_fit_index) * 2*pi*freq(i));
|
||||
cos(time(best_fit_index) * 2*pi*freq(i));
|
||||
zeros(1,num_best_fit_points)];
|
||||
Y = output(i,best_fit_index);
|
||||
|
||||
% Z = a*sin(t*kt + p) + O
|
||||
Z = Y / X;
|
||||
|
||||
amplitude(i) = sqrt(Z(1)^2 + Z(2)^2);
|
||||
phase(i) = atan2d(Z(2), Z(1));
|
||||
% DC_offset = Z(3);
|
||||
|
||||
end
|
||||
|
||||
% Caculalte using transfer function
|
||||
Z = exp(1i*pi*(freq/(sample_freq*0.5)));
|
||||
|
||||
for i = numel(filters):-1:1
|
||||
% Transfer function of each filter
|
||||
H(:,i) = filters{i}.transfer_function(Z);
|
||||
end
|
||||
|
||||
% multiply all transfer functions together
|
||||
H_all = prod(H,2);
|
||||
|
||||
% Extract anmpitude and phase from transfer function
|
||||
calc_amplitude = abs(H_all);
|
||||
calc_phase = atan2d(imag(H_all),real(H_all));
|
||||
|
||||
% Caculate and print error size
|
||||
amp_error = abs(amplitude - calc_amplitude);
|
||||
fprintf('Amplitude error - max: %g mean: %g\n',max(amp_error), mean(amp_error))
|
||||
|
||||
phgase_error = abs(phase - calc_phase);
|
||||
fprintf('Phase error - max: %g mean: %g\n',max(phgase_error), mean(phgase_error))
|
||||
|
||||
% Bode plot
|
||||
figure
|
||||
tiledlayout(2,1)
|
||||
nexttile
|
||||
hold all
|
||||
plot(freq, amplitude)
|
||||
plot(freq, calc_amplitude)
|
||||
ylabel('magnitude')
|
||||
legend('Brute force','transfer function')
|
||||
|
||||
nexttile
|
||||
hold all
|
||||
plot(freq, phase)
|
||||
plot(freq, calc_phase)
|
||||
xlabel('Frequency (Hz)')
|
||||
ylabel('phase')
|
||||
|
||||
function ret = parse_low_pass(lines, start_index)
|
||||
if ~startsWith(lines(start_index),"LowPassFilterFloat")
|
||||
error("Expecting LowPassFilterFloat")
|
||||
end
|
||||
|
||||
% Next line gives sample rate and cutoff
|
||||
temp = sscanf(lines(start_index+1),'Sample rate: %f Hz, Cutoff: %f Hz');
|
||||
ret.sample_rate = temp(1);
|
||||
ret.target_freq = temp(2);
|
||||
|
||||
% Assume we know the form
|
||||
alpha = sscanf(lines(start_index+3),'a: %f');
|
||||
|
||||
ret.transfer_function = @(z) alpha./(1-(1-alpha)*z.^-1);
|
||||
|
||||
end
|
||||
|
||||
function ret = parse_biquad(lines, start_index)
|
||||
if ~startsWith(lines(start_index),"LowPassFilter2pFloat")
|
||||
error("Expecting LowPassFilter2pFloat")
|
||||
end
|
||||
|
||||
% Next line gives sample rate and cutoff
|
||||
temp = sscanf(lines(start_index+1),'Sample rate: %f Hz, Cutoff: %f Hz');
|
||||
ret.sample_rate = temp(1);
|
||||
ret.target_freq = temp(2);
|
||||
|
||||
% Assume we know the form
|
||||
coefficents = sscanf(lines(start_index+3),'a1: %f, a2: %f, b0: %f, b1: %f, b2: %f');
|
||||
|
||||
a1 = coefficents(1);
|
||||
a2 = coefficents(2);
|
||||
b0 = coefficents(3);
|
||||
b1 = coefficents(4);
|
||||
b2 = coefficents(5);
|
||||
|
||||
ret.transfer_function = @(z) (b0 + b1*z.^-1 + b2*z.^-2)./(1 + a1*z.^-1 + a2*z.^-2);
|
||||
|
||||
end
|
||||
|
||||
function ret = parse_notch(lines, start_index)
|
||||
if ~startsWith(lines(start_index),"NotchFilterFloat")
|
||||
error("Expecting NotchFilterFloat")
|
||||
end
|
||||
|
||||
% Next line gives sample rate and cutoff
|
||||
temp = sscanf(lines(start_index+1),'Sample rate: %f Hz, Center: %f Hz');
|
||||
ret.sample_rate = temp(1);
|
||||
ret.target_freq = temp(2);
|
||||
|
||||
% Assume we know the form
|
||||
coefficents = sscanf(lines(start_index+3),'a0: %f, a1: %f, a2: %f, b0: %f, b1: %f, b2: %f');
|
||||
|
||||
a0 = coefficents(1);
|
||||
a1 = coefficents(2);
|
||||
a2 = coefficents(3);
|
||||
b0 = coefficents(4);
|
||||
b1 = coefficents(5);
|
||||
b2 = coefficents(6);
|
||||
|
||||
ret.transfer_function = @(z) (b0 + b1*z.^-1 + b2*z.^-2)./(a0 + a1*z.^-1 + a2*z.^-2);
|
||||
|
||||
end
|
||||
@ -0,0 +1,201 @@
|
||||
// Example and MATLAB script for verifying transfer functions of filters as implemented
|
||||
|
||||
/* on Linux run with
|
||||
./waf configure --board linux
|
||||
./waf --targets examples/TransferFunctionCheck
|
||||
./build/linux/examples/TransferFunctionCheck
|
||||
*/
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <Filter/Filter.h>
|
||||
#include <Filter/LowPassFilter2p.h>
|
||||
#include <Filter/NotchFilter.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||||
|
||||
// Some helper classes to allow accessing protected variables, also useful for adding type specific prints
|
||||
|
||||
class LowPassHelper : public LowPassFilterFloat {
|
||||
public:
|
||||
using LowPassFilterFloat::LowPassFilterFloat;
|
||||
|
||||
void set_cutoff_frequency_override(float sample_freq, float cutoff_freq) {
|
||||
// Stash the sample rate so we can use it later
|
||||
_sample_freq = sample_freq;
|
||||
set_cutoff_frequency(sample_freq, cutoff_freq);
|
||||
}
|
||||
|
||||
// Were really cheating here and using the same method as the filter to get the coefficient
|
||||
// rather than pulling the coefficient directly
|
||||
void print_transfer_function() {
|
||||
hal.console->printf("LowPassFilterFloat\n");
|
||||
hal.console->printf("Sample rate: %.9f Hz, Cutoff: %.9f Hz\n", _sample_freq, get_cutoff_freq());
|
||||
hal.console->printf("Low pass filter in the form: H(z) = a/(1-(1-a)*z^-1)\n");
|
||||
hal.console->printf("a: %.9f\n", calc_lowpass_alpha_dt(1.0/_sample_freq, get_cutoff_freq()));
|
||||
}
|
||||
|
||||
private:
|
||||
float _sample_freq;
|
||||
};
|
||||
|
||||
class LowPass2pHelper : public LowPassFilter2pFloat {
|
||||
public:
|
||||
using LowPassFilter2pFloat::LowPassFilter2pFloat;
|
||||
|
||||
// Print transfer function and variables to console
|
||||
void print_transfer_function() {
|
||||
hal.console->printf("LowPassFilter2pFloat\n");
|
||||
hal.console->printf("Sample rate: %.9f Hz, Cutoff: %.9f Hz\n", get_sample_freq(), get_cutoff_freq());
|
||||
hal.console->printf("Biquad filter in the form: H(z) = (b0 + b1*z^-1 + b2*z^-2)/(1 + a1*z^-1 + a2*z^-2)\n");
|
||||
hal.console->printf("a1: %.9f, a2: %.9f, b0: %.9f, b1: %.9f, b2: %.9f\n", _params.a1, _params.a2, _params.b0, _params.b1, _params.b2);
|
||||
}
|
||||
};
|
||||
|
||||
class NotchHelper : public NotchFilterFloat {
|
||||
public:
|
||||
using NotchFilterFloat::NotchFilterFloat;
|
||||
|
||||
// Print transfer function and variables to console
|
||||
void print_transfer_function() {
|
||||
hal.console->printf("NotchFilterFloat\n");
|
||||
hal.console->printf("Sample rate: %.9f Hz, Center: %.9f Hz\n", _sample_freq_hz, _center_freq_hz);
|
||||
hal.console->printf("Notch filter in the form: H(z) = (b0 + b1*z^-1 + b2*z^-2)/(a0 + a1*z^-1 + a2*z^-2)\n");
|
||||
hal.console->printf("a0: %.9f, a1: %.9f, a2: %.9f, b0: %.9f, b1: %.9f, b2: %.9f\n", 1.0/a0_inv, a1, a2, b0, b1, b2);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
// create an instance each filter to test
|
||||
LowPassHelper lowpass;
|
||||
LowPass2pHelper biquad;
|
||||
NotchHelper notch;
|
||||
NotchHelper notch2;
|
||||
|
||||
enum class filter_type {
|
||||
LowPass,
|
||||
Biquad,
|
||||
Notch,
|
||||
Combination,
|
||||
} type;
|
||||
|
||||
void setup();
|
||||
void loop();
|
||||
void reset_all();
|
||||
float apply_to_filter_under_test(float input);
|
||||
void sweep(uint16_t num_samples, uint16_t max_freq, float sample_rate);
|
||||
|
||||
void loop() {};
|
||||
|
||||
// setup routine
|
||||
void setup()
|
||||
{
|
||||
hal.console->printf("Frequency sweep transfer function test\n");
|
||||
hal.console->printf("Sweeping a range of frequencies in the form sin(2*pi*t*f)\n");
|
||||
|
||||
// Set sample rate and target frequency
|
||||
const float sample_rate = 1000;
|
||||
const float target_freq = 50;
|
||||
|
||||
type = filter_type::Combination;
|
||||
|
||||
// Run 1000 time steps at each frequency
|
||||
const uint16_t num_samples = 1000;
|
||||
|
||||
// Run upto 150 hz
|
||||
const uint16_t max_freq = 150;
|
||||
|
||||
// Print transfer function of filter under test
|
||||
hal.console->printf("\n");
|
||||
switch (type) {
|
||||
case filter_type::LowPass:
|
||||
lowpass.set_cutoff_frequency_override(sample_rate, target_freq);
|
||||
lowpass.print_transfer_function();
|
||||
break;
|
||||
|
||||
case filter_type::Biquad:
|
||||
biquad.set_cutoff_frequency(sample_rate, target_freq);
|
||||
biquad.print_transfer_function();
|
||||
break;
|
||||
|
||||
case filter_type::Notch:
|
||||
notch.init(sample_rate, target_freq, target_freq*0.25, 40);
|
||||
notch.print_transfer_function();
|
||||
break;
|
||||
|
||||
case filter_type::Combination:
|
||||
// A combination of two notches and a biquad lowpass, representative of a typical setup
|
||||
notch.init(sample_rate, target_freq, target_freq*0.25, 40);
|
||||
notch2.init(sample_rate, target_freq*2.00, target_freq*0.25, 40);
|
||||
biquad.set_cutoff_frequency(sample_rate, target_freq*1.5);
|
||||
notch.print_transfer_function();
|
||||
hal.console->printf("\n");
|
||||
notch2.print_transfer_function();
|
||||
hal.console->printf("\n");
|
||||
biquad.print_transfer_function();
|
||||
break;
|
||||
}
|
||||
hal.console->printf("\n");
|
||||
|
||||
// Run sweep over given range
|
||||
sweep(num_samples, max_freq, sample_rate);
|
||||
|
||||
// Wait a while for print buffer to empty and exit
|
||||
hal.scheduler->delay(1000);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
void reset_all()
|
||||
{
|
||||
lowpass.reset(0.0);
|
||||
biquad.reset(0.0);
|
||||
notch.reset();
|
||||
notch2.reset();
|
||||
}
|
||||
|
||||
float apply_to_filter_under_test(float input)
|
||||
{
|
||||
switch (type) {
|
||||
case filter_type::LowPass:
|
||||
return lowpass.apply(input);
|
||||
|
||||
case filter_type::Biquad:
|
||||
return biquad.apply(input);
|
||||
|
||||
case filter_type::Notch:
|
||||
return notch.apply(input);
|
||||
|
||||
case filter_type::Combination: // Ordering does not matter
|
||||
return biquad.apply(notch.apply(notch2.apply(input)));
|
||||
|
||||
}
|
||||
return input;
|
||||
}
|
||||
|
||||
void sweep(uint16_t num_samples, uint16_t max_freq, float sample_rate)
|
||||
{
|
||||
// print header
|
||||
hal.console->printf("f(hz)");
|
||||
for (uint16_t i = 0; i < num_samples; i++) {
|
||||
hal.console->printf(", t = %.9f", i / sample_rate);
|
||||
}
|
||||
hal.console->printf("\n");
|
||||
|
||||
|
||||
for (uint16_t f = 1; f <= max_freq; f++) {
|
||||
// Print freq and reset filter
|
||||
hal.console->printf("%i", f);
|
||||
reset_all();
|
||||
|
||||
// Run over the given number of samples
|
||||
for (uint16_t i = 0; i < num_samples; i++) {
|
||||
const float t = i / sample_rate;
|
||||
const float input = sinf(M_2PI*t*f);
|
||||
const float output = apply_to_filter_under_test(input);
|
||||
hal.console->printf(", %+.9f", output);
|
||||
}
|
||||
hal.console->printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
7
libraries/Filter/examples/TransferFunctionCheck/wscript
Normal file
7
libraries/Filter/examples/TransferFunctionCheck/wscript
Normal file
@ -0,0 +1,7 @@
|
||||
#!/usr/bin/env python
|
||||
# encoding: utf-8
|
||||
|
||||
def build(bld):
|
||||
bld.ap_example(
|
||||
use='ap',
|
||||
)
|
||||
Loading…
Reference in New Issue
Block a user