%
% Macro to simulate a direct adaptive cancellation algorithm, for
% sinusoidal disturbances of unknown frequency with a fundamental and a
% 3rd harmonic, as described in:
% M. Bodson, J. Jensen, & S. Douglas, “Active Noise Control for Periodic 
% Disturbances,” IEEE Trans. on Control Systems Technology, vol. 9, no. 1, 
% pp. 200-205, 2001.
% (c) Marc Bodson, June 18, 2020
%
global kp ap d1 om1 d3 gi111 gi112 gi311 gi312 g11 g12 g31 g32 b a
%
% Plant and disturbance parameters
%
kp=100;ap=100;om1=100;d1=1;d3=-1;
%
% Controller parameters
%
pr1=kp*ap/(ap^2+om1^2);pi1=-kp*om1/(ap^2+om1^2);
pr3=kp*ap/(ap^2+(3*om1)^2);pi3=-kp*3*om1/(ap^2+(3*om1)^2);
p1=pr1^2+pi1^2;gi111=pr1/p1;gi112=pi1/p1;
p3=pr3^2+pi3^2;gi311=pr3/p3;gi312=pi3/p3;
g11=10;g12=400;g31=10;g32=10;b=30;a=5;
%
% States initialization
%
y0=0;al1d0=0;al3d0=0;th110=0.9;th120=90;
al10=0;x12f0=0;th310=0;th320=0;
th11nav0=th110;th12nav0=th120;dal1nav0=al10-al1d0;
x12fnav0=x12f0;th31nav0=th310;th32nav0=th320;
x0=[y0;al1d0;al3d0;th110;th120;al10;x12f0;th310;th320];
x0=[x0;th11nav0;th12nav0;dal1nav0;x12fnav0;th31nav0;th32nav0];
%
% Simulation
%
tmax=1;
[t,x]=ode45('dac13f',[0,tmax],x0);
%
% Plot the results
%
y=x(:,1);th11=x(:,4);th12=x(:,5);th31=x(:,8);th32=x(:,9);
th11nav=x(:,10);th12nav=x(:,11);th31nav=x(:,14);th32nav=x(:,15);
figure(1)
plot(t,y,'k')
xlabel('Time (seconds)')
ylabel('Plant Output')
figure(2)
plot(t,th11,'k',t,th11nav,'k--')
xlabel('Time (seconds)')
ylabel('Magnitude Estimate of Fundamental')
figure(3)
plot(t,th12,'k',t,th12nav,'k--')
xlabel('Time (seconds)')
ylabel('Frequency Estimate of Fundamental')
figure(4)
plot(t,th31,'k',t,th31nav,'k--')
xlabel('Time (seconds)')
ylabel('Magnitude Estimate of Third Harmonic')
figure(5)
plot(t,th32,'k',t,th32nav,'k--')
xlabel('Time (seconds)')
ylabel('Phase Estimate of Third Harmonic')

