Forums

Coupled form Goertzel

Started by Mimar September 17, 2013
Hello,

recently I have read very interesting article about Coupled-form 2nd-order
IIR resonators which was written by Rick Lyons. 

The princip of this form of resonator is clear for me, but I have got still
some problems if I try to associate it with Goertzel algorithm. I have
created short simulation script in Matlab, but I cannot find the mistake.
Output is terrible, there are a lot of swings insted straight amplitude
line.

%**************************************************************************
%
% Goerzel algorithm
%
%**************************************************************************

clc;
clear all;
close all;

%........const. data........................................

fvz = 2400; % sampling frequency
Ts = 1/fvz; %period of sampling
N = 48;     % buffer length
delka_sig = 2000; % number of iterations

f = 50;  % freq. of signal, no leakage effect is there


%........signal creating    ..............................

SNR =  30;   % SNR
AMP1 = 0.9;   % 1. harmonics amplitude
AMP2 = 0.4;  

treshold = 400;

NSAMP = AMP1/(10^(SNR/20)) % determine amplitude via SNR

phi = 0;

for n = 1 : 1 : delka_sig
    
    if n < treshold 
        AMP1 = 0.5;
    else
        AMP1 = 0.2;
    end
    
    phi = phi + 2*pi*f*Ts; % phase
    
    % test signal, harmonics
    harm1(n)  = AMP1 * sin(phi);
    harm3(n)  = 0.1 * sin(3*phi);
    harm5(n)  = 0.025 * sin(5*phi);  
    harm7(n)  = 0.02* sin(7*phi);  
    harm9(n)  = 0.018* sin(9*phi); 
    harm11(n) = 0.015 * sin(11*phi);

end
 
noise = NSAMP * randn(size(delka_sig));   % noise

% output signal, now clear, only 1. harm.
signal = harm1; % + harm3 + harm5 + harm7 + harm9 + harm11; % + noise;

% FIFO memory for Goertzel 
fifo = zeros(1, N); 

%..........................................................................

% Goertzel algorithm
r = 1; 
k = 1;  % first harmonics
pik_term = 2 * pi * k./N;  %pik_term = 2*pi*f/fvz;

const_cplx = cos(pik_term) - 1i*sin(pik_term);

xn_M = 0;   % sample z^-N

%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
% for coupled form
a_const = cos(pik_term);  % cos(2*pi*f/fvz)
b_const = sin(pik_term);   % sin(2*pi*f/fvz)

y_G = zeros(1, delka_sig);

% states
s0 = zeros(1, delka_sig); 
s1 = zeros(1, delka_sig);


% Goertzel go......
for n = 2 : 1 : delka_sig  
    
    fifo = [fifo(2:N), signal(n)];  % data in buffer
    input = signal(n) - xn_M;   % comb filter
    
    % coupled resonator
    s0(n) = input - b_const*s1(n-1) + s0(n-1)*a_const;
    s1(n) = s0(n)*b_const + a_const*s1(n-1);
    
    % forward branch
    y_G(n) = s1(n) - s1(n-1)*const_cplx;  
    
    xn_M = fifo(1);  

end

y_G = abs(y_G)./N;  % process output

%....... Graphs................................................

figure(1)
subplot(2,1,1);
plot(signal(1:1000), 'r', 'LineWidth', 1.2);
title('Input signal');
xlabel('n [-]');
ylabel('Amplitude [V]');
grid on


subplot(2,1,2)
hold on
stem(y_G(1:1000), 'b', 'LineWidth', 1.5);
title('1. harmon.');
xlabel('n [-]');
ylabel('Amplitude [V]');
grid on

----------------------------------------------------------

Variable s0 above is state after first delay, s1 after second delay.

Could somebody give me small advice how to get it to work?

Thank's a lot! 







    
    



    
	 

_____________________________		
Posted through www.DSPRelated.com
On Tuesday, September 17, 2013 4:20:48 AM UTC-4, Mimar wrote:
> Hello, > > > > recently I have read very interesting article about Coupled-form 2nd-order > > IIR resonators which was written by Rick Lyons. > > > > The princip of this form of resonator is clear for me, but I have got still > > some problems if I try to associate it with Goertzel algorithm. I have > > created short simulation script in Matlab, but I cannot find the mistake. > > Output is terrible, there are a lot of swings insted straight amplitude > > line. > > > > %************************************************************************** > > % > > % Goerzel algorithm > > % > > %************************************************************************** > > > > clc; > > clear all; > > close all; > > > > %........const. data........................................ > > > > fvz = 2400; % sampling frequency > > Ts = 1/fvz; %period of sampling > > N = 48; % buffer length > > delka_sig = 2000; % number of iterations > > > > f = 50; % freq. of signal, no leakage effect is there > > > > > > %........signal creating .............................. > > > > SNR = 30; % SNR > > AMP1 = 0.9; % 1. harmonics amplitude > > AMP2 = 0.4; > > > > treshold = 400; > > > > NSAMP = AMP1/(10^(SNR/20)) % determine amplitude via SNR > > > > phi = 0; > > > > for n = 1 : 1 : delka_sig > > > > if n < treshold > > AMP1 = 0.5; > > else > > AMP1 = 0.2; > > end > > > > phi = phi + 2*pi*f*Ts; % phase > > > > % test signal, harmonics > > harm1(n) = AMP1 * sin(phi); > > harm3(n) = 0.1 * sin(3*phi); > > harm5(n) = 0.025 * sin(5*phi); > > harm7(n) = 0.02* sin(7*phi); > > harm9(n) = 0.018* sin(9*phi); > > harm11(n) = 0.015 * sin(11*phi); > > > > end > > > > noise = NSAMP * randn(size(delka_sig)); % noise > > > > % output signal, now clear, only 1. harm. > > signal = harm1; % + harm3 + harm5 + harm7 + harm9 + harm11; % + noise; > > > > % FIFO memory for Goertzel > > fifo = zeros(1, N); > > > > %.......................................................................... > > > > % Goertzel algorithm > > r = 1; > > k = 1; % first harmonics > > pik_term = 2 * pi * k./N; %pik_term = 2*pi*f/fvz; > > > > const_cplx = cos(pik_term) - 1i*sin(pik_term); > > > > xn_M = 0; % sample z^-N > > > > %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! > > % for coupled form > > a_const = cos(pik_term); % cos(2*pi*f/fvz) > > b_const = sin(pik_term); % sin(2*pi*f/fvz) > > > > y_G = zeros(1, delka_sig); > > > > % states > > s0 = zeros(1, delka_sig); > > s1 = zeros(1, delka_sig); > > > > > > % Goertzel go...... > > for n = 2 : 1 : delka_sig > > > > fifo = [fifo(2:N), signal(n)]; % data in buffer > > input = signal(n) - xn_M; % comb filter > > > > % coupled resonator > > s0(n) = input - b_const*s1(n-1) + s0(n-1)*a_const; > > s1(n) = s0(n)*b_const + a_const*s1(n-1); > > > > % forward branch > > y_G(n) = s1(n) - s1(n-1)*const_cplx; > > > > xn_M = fifo(1); > > > > end > > > > y_G = abs(y_G)./N; % process output > > > > %....... Graphs................................................ > > > > figure(1) > > subplot(2,1,1); > > plot(signal(1:1000), 'r', 'LineWidth', 1.2); > > title('Input signal'); > > xlabel('n [-]'); > > ylabel('Amplitude [V]'); > > grid on > > > > > > subplot(2,1,2) > > hold on > > stem(y_G(1:1000), 'b', 'LineWidth', 1.5); > > title('1. harmon.'); > > xlabel('n [-]'); > > ylabel('Amplitude [V]'); > > grid on > > > > ---------------------------------------------------------- > > > > Variable s0 above is state after first delay, s1 after second delay. > > > > Could somebody give me small advice how to get it to work? > > > > Thank's a lot! > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > _____________________________ > > Posted through www.DSPRelated.com
I'm not sure to which of Rick's articles you are referring, but not well known is the fact that pretty much any 2nd order resonator may be driven in a Goertzel like manner to calculate a fourier coefficient. Some forms offer numerical advantages when compared to the classic Goertzel algo. Try analyzing a simple sinusoid where the result is known so you can debug your implementation. For details on the generalized Goertzel see: http://www.claysturner.com/dsp/digital_resonators.pdf The generalized Goertzel algo starts on slide 61 IHTH, Clay
On Tuesday, September 17, 2013 2:11:38 PM UTC-4, cl...@claysturner.com wrote:
> On Tuesday, September 17, 2013 4:20:48 AM UTC-4, Mimar wrote: > > > Hello, > > > > > > > > > > > > recently I have read very interesting article about Coupled-form 2nd-order > > > > > > IIR resonators which was written by Rick Lyons. > > > > > > > > > > > > The princip of this form of resonator is clear for me, but I have got still > > > > > > some problems if I try to associate it with Goertzel algorithm. I have > > > > > > created short simulation script in Matlab, but I cannot find the mistake. > > > > > > Output is terrible, there are a lot of swings insted straight amplitude > > > > > > line. > > > > > > > > > > > > %************************************************************************** > > > > > > % > > > > > > % Goerzel algorithm > > > > > > % > > > > > > %************************************************************************** > > > > > > > > > > > > clc; > > > > > > clear all; > > > > > > close all; > > > > > > > > > > > > %........const. data........................................ > > > > > > > > > > > > fvz = 2400; % sampling frequency > > > > > > Ts = 1/fvz; %period of sampling > > > > > > N = 48; % buffer length > > > > > > delka_sig = 2000; % number of iterations > > > > > > > > > > > > f = 50; % freq. of signal, no leakage effect is there > > > > > > > > > > > > > > > > > > %........signal creating .............................. > > > > > > > > > > > > SNR = 30; % SNR > > > > > > AMP1 = 0.9; % 1. harmonics amplitude > > > > > > AMP2 = 0.4; > > > > > > > > > > > > treshold = 400; > > > > > > > > > > > > NSAMP = AMP1/(10^(SNR/20)) % determine amplitude via SNR > > > > > > > > > > > > phi = 0; > > > > > > > > > > > > for n = 1 : 1 : delka_sig > > > > > > > > > > > > if n < treshold > > > > > > AMP1 = 0.5; > > > > > > else > > > > > > AMP1 = 0.2; > > > > > > end > > > > > > > > > > > > phi = phi + 2*pi*f*Ts; % phase > > > > > > > > > > > > % test signal, harmonics > > > > > > harm1(n) = AMP1 * sin(phi); > > > > > > harm3(n) = 0.1 * sin(3*phi); > > > > > > harm5(n) = 0.025 * sin(5*phi); > > > > > > harm7(n) = 0.02* sin(7*phi); > > > > > > harm9(n) = 0.018* sin(9*phi); > > > > > > harm11(n) = 0.015 * sin(11*phi); > > > > > > > > > > > > end > > > > > > > > > > > > noise = NSAMP * randn(size(delka_sig)); % noise > > > > > > > > > > > > % output signal, now clear, only 1. harm. > > > > > > signal = harm1; % + harm3 + harm5 + harm7 + harm9 + harm11; % + noise; > > > > > > > > > > > > % FIFO memory for Goertzel > > > > > > fifo = zeros(1, N); > > > > > > > > > > > > %.......................................................................... > > > > > > > > > > > > % Goertzel algorithm > > > > > > r = 1; > > > > > > k = 1; % first harmonics > > > > > > pik_term = 2 * pi * k./N; %pik_term = 2*pi*f/fvz; > > > > > > > > > > > > const_cplx = cos(pik_term) - 1i*sin(pik_term); > > > > > > > > > > > > xn_M = 0; % sample z^-N > > > > > > > > > > > > %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! > > > > > > % for coupled form > > > > > > a_const = cos(pik_term); % cos(2*pi*f/fvz) > > > > > > b_const = sin(pik_term); % sin(2*pi*f/fvz) > > > > > > > > > > > > y_G = zeros(1, delka_sig); > > > > > > > > > > > > % states > > > > > > s0 = zeros(1, delka_sig); > > > > > > s1 = zeros(1, delka_sig); > > > > > > > > > > > > > > > > > > % Goertzel go...... > > > > > > for n = 2 : 1 : delka_sig > > > > > > > > > > > > fifo = [fifo(2:N), signal(n)]; % data in buffer > > > > > > input = signal(n) - xn_M; % comb filter > > > > > > > > > > > > % coupled resonator > > > > > > s0(n) = input - b_const*s1(n-1) + s0(n-1)*a_const; > > > > > > s1(n) = s0(n)*b_const + a_const*s1(n-1); > > > > > > > > > > > > % forward branch > > > > > > y_G(n) = s1(n) - s1(n-1)*const_cplx; > > > > > > > > > > > > xn_M = fifo(1); > > > > > > > > > > > > end > > > > > > > > > > > > y_G = abs(y_G)./N; % process output > > > > > > > > > > > > %....... Graphs................................................ > > > > > > > > > > > > figure(1) > > > > > > subplot(2,1,1); > > > > > > plot(signal(1:1000), 'r', 'LineWidth', 1.2); > > > > > > title('Input signal'); > > > > > > xlabel('n [-]'); > > > > > > ylabel('Amplitude [V]'); > > > > > > grid on > > > > > > > > > > > > > > > > > > subplot(2,1,2) > > > > > > hold on > > > > > > stem(y_G(1:1000), 'b', 'LineWidth', 1.5); > > > > > > title('1. harmon.'); > > > > > > xlabel('n [-]'); > > > > > > ylabel('Amplitude [V]'); > > > > > > grid on > > > > > > > > > > > > ---------------------------------------------------------- > > > > > > > > > > > > Variable s0 above is state after first delay, s1 after second delay. > > > > > > > > > > > > Could somebody give me small advice how to get it to work? > > > > > > > > > > > > Thank's a lot! > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > _____________________________ > > > > > > Posted through www.DSPRelated.com > > > > I'm not sure to which of Rick's articles you are referring, but not well known is the fact that pretty much any 2nd order resonator may be driven in a Goertzel like manner to calculate a fourier coefficient. Some forms offer numerical advantages when compared to the classic Goertzel algo. > > > > Try analyzing a simple sinusoid where the result is known so you can debug your implementation. > > > > For details on the generalized Goertzel see: > > > > http://www.claysturner.com/dsp/digital_resonators.pdf > > > > The generalized Goertzel algo starts on slide 61 > > > > IHTH, > > Clay
I looked in my archives and found this code I wrote a few years ago that implements a Goertzel algo using a coupled quadrature oscillator. This is from a suite of Goertzels implemented with many different oscillator designs. The input vars are: N number of data points freq frequency bin number *data vector of N data points routine returns energy double Quadrature(int N,int freq,double *data) { double p,q,tmp,k,m,E; int n; k=sin(PI2*freq/N); m=sqrt(1-k*k); if (freq>=N/4) m = -m; p=0.0; q=0.0; for (n=0;n<N;n++) { tmp=m*p+k*q+data[n]; q=-k*p+m*q; p=tmp; } tmp=m*p+k*q; // do final iteration with 0 input q=-k*p+m*q; p=tmp; E=p*p+q*q; // quadrature energy return E; }
Just quickly glancing through your code, I suspect that you should change this line

  s1(n) = s0(n)*b_const + a_const*s1(n-1); 


To this

  s1(n) = s0(n-1)*b_const + a_const*s1(n-1); 

You can test by applying an impulse to the coupled resonator, without the (1 - z^-N) in front. If everything is working properly it should oscillate forever at your harm1 frequency, with sin and cos outputs at s0 and s1. 

When you get it working, you will find that, for input frequencies set to a "bin", the input to the coupled resonator goes to 0 after N cycles, and the output is sustained solely by the oscillator. This is what makes this algorithm both cool and scary at the same time (think numerical precision!)

Bob
    
   
>Just quickly glancing through your code, I suspect that you should change
t=
>his line > > s1(n) =3D s0(n)*b_const + a_const*s1(n-1);=20 > > >To this > > s1(n) =3D s0(n-1)*b_const + a_const*s1(n-1);=20 > >You can test by applying an impulse to the coupled resonator, without the
(=
>1 - z^-N) in front. If everything is working properly it should oscillate
f=
>orever at your harm1 frequency, with sin and cos outputs at s0 and s1.=20 > >When you get it working, you will find that, for input frequencies set to
a=
> "bin", the input to the coupled resonator goes to 0 after N cycles, and
th=
>e output is sustained solely by the oscillator. This is what makes this
alg=
>orithm both cool and scary at the same time (think numerical precision!) > >Bob > =20 > >Hello Bob,
I thank you a lot for your advice. I changed s1(n) line and I have to say it began to work properly. Super. _____________________________ Posted through www.DSPRelated.com
On Wed, 18 Sep 2013 04:41:18 -0700 (PDT), radams2000@gmail.com wrote:

>Just quickly glancing through your code, I suspect that you should change this line > > s1(n) = s0(n)*b_const + a_const*s1(n-1); > > >To this > > s1(n) = s0(n-1)*b_const + a_const*s1(n-1); > >You can test by applying an impulse to the coupled resonator, without the (1 - z^-N) in front. If everything is working properly it should oscillate forever at your harm1 frequency, with sin and cos outputs at s0 and s1. > >When you get it working, you will find that, for input frequencies set to a "bin", the input to the coupled resonator goes to 0 after N cycles, and the output is sustained solely by the oscillator. This is what makes this algorithm both cool and scary at the same time (think numerical precision!) > >Bob
Hello Bob, Thanks for helping Mimar. Also, you saved me from having to go through his Matlab code. [-Rick-]
No problem, happy to help when I can.