function ModelDataAllAlg
% Model data, all three algorithms, the code reproduces Fig 1.
% Notice: in this example all frequencies are angular frequencies

[s,dt,npt]=GenerateMultiCompDataFrMod; % generate test data
time=(1:npt)*dt; HTampl=abs(hilbert(s)); HTphase=angle(hilbert(s));
figure(1);

%%%%% panel (a) the signal and its Hilbert amplitude %%%%%%%%%%%
subplot(4,1,1); plot(time,s,time,HTampl);
ylabel('$s,a_H$','Interpreter','Latex','FontSize',20);
set(gca,'Xticklabel',[]); ylim([-2,3]);
title('phases');

%%%%% panel (b): phase locking approach %%%%%%%%%%%%%%%%%
LBphase=LockingBasedPhase(s,dt,npt);
phidif=PhaseDifference(HTphase,LBphase);
subplot(4,1,2); plot(time,phidif);
ylabel('$\varphi_H-\varphi_L$','Interpreter','Latex','FontSize',20);
set(gca,'Xticklabel',[]); ylim([-pi pi]);

%%%%% panel (c): nonresonant oscillator %%%%%%%%%%%%%%%%%
[Dphase,NRDampl]=NonResonantOscillator(s,dt,npt);
phidif=PhaseDifference(HTphase,Dphase);
subplot(4,1,3); plot(time,phidif);
ylabel('$\varphi_H-\varphi_N$','Interpreter','Latex','FontSize',20);
set(gca,'Xticklabel',[]); ylim([-pi pi]);

%%%%% panel (d): resonant oscillator %%%%%%%%%%%%%%%%%
[Dphase,RDampl]=ResonantOscillator(s,dt,npt);
phidif=PhaseDifference(HTphase,Dphase);
subplot(4,1,4); plot(time,phidif); ylim([-pi pi]);
ylabel('$\varphi_H-\varphi_R$','Interpreter','Latex','FontSize',20);
xlabel('time');

%%%%%%%%% amplitudes %%%%%%%%%%%%
figure(2);
subplot(2,1,1); plot(time,s,time,NRDampl);
ylabel('$s,a_N$','Interpreter','Latex','FontSize',20);
set(gca,'Xticklabel',[]); title('amplitudes'); ylim([-2,4]);
subplot(2,1,2); plot(time,s,time,RDampl);

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Dphase,Dampl]=ResonantOscillator(s,dt,npt)
Dphase=zeros(1,npt); % reserve space for the "device" phase
Dampl=Dphase; % reserve space for the "device" amplitude
om0=1.1; % initial guess for frequency (10% large than the true value)
% parameters of the measurement "device"
alpha=0.3*om0; mu=500; gam=alpha/2;
% precomputed coefficients for the device
[C1,C2,C3,enuDel,ealDel,eta]=ExSolCoefs(om0,dt,gam); % for the oscillator
edelmu=exp(-dt/mu); % for the integrator
% parameters of adaptation algorithm
update_factor=5; % frequency correction 20 times per period
npbuf=round(2*pi/om0/dt); % number of points for frequency estimation
updatepoint=2*npbuf;
update_step=round(npbuf/update_factor);
% precomputed quantities for linear fit for frequency adaptation
tbuf=(1:npbuf)*dt; Sx=sum(tbuf); denom=npbuf*sum(tbuf.^2)-Sx*Sx;
% Initialization
x=0; y=0; z=0; ypp=0; yp=0;
for k=3:npt % starting the "real-time" estimation
[x,y]=OneStep(x,y,gam,eta,enuDel,ealDel,C1,C2,C3,s(k-2),s(k-1),s(k));
z=OneStepInt(z,edelmu,mu,dt,ypp,yp,y);
ypp=yp; yp=y; v=mu*z*om0;
Dphase(k)=atan2(v,y); % new phase value
Dampl(k)=alpha*sqrt(y*y+v*v); % new amplitude value
if(k>updatepoint) % frequency adaptation
buffer=unwrap(Dphase(k+1-npbuf:k)); % buffer for frequency estimation
om0=(npbuf*sum(tbuf.*buffer)-Sx*sum(buffer))/denom; % new frequency estimate
[C1,C2,C3,enuDel,ealDel,eta]=ExSolCoefs(om0,dt,gam);
updatepoint=updatepoint+update_step; % point for the next frequency correction
end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Dphase,Dampl]=NonResonantOscillator(s,dt,npt)
Dphase=zeros(1,npt); % reserve space for the "device" phase
Dampl=Dphase; % reserve space for the "device" amplitude
nu=1.1; % initial guess for frequency (10% large than the true value)
% parameters of the non-resonant "device"
om0=5*nu; om0_2=om0*om0; % oscillator frequency
alpha_a=6; gama=alpha_a/2; % damping for the "amplitude device"
alpha_p=0.2; gamp=alpha_p/2; % damping for the "phase device"
factor=sqrt((om0_2-nu*nu)^2+(alpha_a*nu)^2);
% precomputed coefficients for oscillators
[C1a,C2a,C3a,enuDela,ealDela,etaa]=ExSolCoefs(om0,dt,gama); % for the "amplitude device"
[C1p,C2p,C3p,enuDelp,ealDelp,etap]=ExSolCoefs(om0,dt,gamp); % for the "phase device"
% parameters of adaptation algorithm
update_factor=5; % frequency correction 20 times per period
npbuf=round(2*pi/nu/dt); % number of points for frequency estimation
updatepoint=2*npbuf;
update_step=round(npbuf/update_factor);
% precomputed quantities for linear fit for frequency adaptation
tbuf=(1:npbuf)*dt; Sx=sum(tbuf); denom=npbuf*sum(tbuf.^2)-Sx*Sx;
% Initialization
x=0; y=0; u=0; v=0;
for k=3:npt % starting the "real-time" estimation
% the next two lines provide the amplitude for s(k);
% comment them out for speed, if you do not need the amplitude
[x,y]=OneStep(x,y,gama,etaa,enuDela,ealDela,C1a,C2a,C3a,s(k-2),s(k-1),s(k));
z=y/nu; Dampl(k)=factor*sqrt(z*z+x*x); % new amplitude value
% the next two lines provide the phase for s(k);
% comment them out for speed, if you need only the amplitude
[u,v]=OneStep(u,v,gamp,etap,enuDelp,ealDelp,C1p,C2p,C3p,s(k-2),s(k-1),s(k));
z=v/nu; Dphase(k)=atan2(-z,u); % new phase value
if(k>updatepoint) % frequency adaptation
buffer=unwrap(Dphase(k+1-npbuf:k)); % buffer for frequency estimation
nu=(npbuf*sum(tbuf.*buffer)-Sx*sum(buffer))/denom; % new frequency estimate
updatepoint=updatepoint+update_step; % point for the next frequency correction
end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function LBphase=LockingBasedPhase(s,dt,npt)
LBphase=zeros(1,npt); % reserve space for the device phase
% parameters of the measurement "device"
epsilon=0.8; K=1;
nRKsteps=5; % number of Runge-Kutta steps per dt
% parameters of adaptation algorithm
nuest=1.1; % initial guess for frequency (10% large than the true value)
update_factor=20; % frequency correction 20 times per period
npbuf=round(2*pi/nuest/dt); % number of points for frequency estimation
updatepoint=2*npbuf;
update_step=round(npbuf/update_factor);
% precomputed quantities for linear fit for frequency adaptation
tbuf=(1:npbuf)*dt; Sx=sum(tbuf); denom=npbuf*sum(tbuf.^2)-Sx*Sx;
for k=3:npt % starting the "real-time" estimation
LBphase(k)=rk(LBphase(k-1),dt,nRKsteps,nuest,epsilon,s(k),s(k-1),s(k));
if(k>updatepoint) % frequency adaptation
buffer=LBphase(k+1-npbuf:k); % buffer for frequency estimation
nu_quasi=(npbuf*sum(tbuf.*buffer)-Sx*sum(buffer))/denom; %slope
nuest=nuest+K*(nu_quasi-nuest);
updatepoint=updatepoint+update_step; % point for the next frequency correction
end
end
end
% %%%%%%%%%%%%%%%%%%%%%%%% RK-solver, adapted %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function phi=rk(phi,dt,nsteps,omega,epsilon,sp,s,snew) % x is time,
h=dt/nsteps;
b=(snew-sp)/dt/2; c=(sp-2*s+snew)/dt^2;
hh=h/2.; h6=h/6.; % y is phi (dependent variable),
t=0; % initial time
for i=1:nsteps % h is the step Delta,
th=t+hh; % nsteps is the number of steps
dy0=omega - epsilon*(s+b*t+c*t*t)*sin(phi);
phit=phi+hh*dy0;
dyt=omega - epsilon*(s+b*th+c*th*th)*sin(phit);
phit=phi+hh*dyt;
dym=omega - epsilon*(s+b*th+c*th*th)*sin(phit);
phit=phi+h*dym;
dym=dym+dyt;
t=t+h;
dyt=omega - epsilon*(s+b*t+c*t*t)*sin(phit);
phi=phi+h6*(dy0+2*dym+dyt);
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x,xd]=OneStep(x,xd,alpha,eta,eetaDel,ealDel,C1,C2,C3,sprev,s,snew)
A=x-1i*(xd+alpha*x)/eta;
A=A-1i*(C1*sprev+C2*s+C3*snew)/eta;
d=A*eetaDel;
y=real(d); yd=1i*eta*(d-conj(d))/2;
x=y/ealDel; xd=(yd-alpha*y)/ealDel;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [C1,C2,C3,eetadel,ealdel,eta]=ExSolCoefs(om0,del,alpha)
% del is the time step dt
% alpha is the half of the dampling: x''+2*alpha*x'...
eta2=om0^2-alpha^2; eta=sqrt(eta2);
eetadel=exp(1i*eta*del); a=1/eetadel; ealdel=exp(alpha*del);
I1=1i*(a-1)/eta; I2=(a*(1+1i*eta*del)-1)/eta2;
I3=(a*(del*eta*(2+1i*del*eta)-2*1i)+2*1i)/eta2/eta;
C1=(I3-I2*del)/2/del^2/ealdel; C2=I1-I3/del^2;
C3=ealdel*(I2*del+I3)/2/del^2;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function z=OneStepInt(z,edelmu,mu,dt,ypp,yp,y)
dt2=dt*dt;
a=yp; b=(y-ypp)/dt/2; c=(ypp-2*yp+y)/dt2/2;
d=-a+b*mu-2*c*mu^2; C0=z+d;
z=C0*edelmu-d+b*dt-2*c*mu*dt+c*dt2;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function phidif=PhaseDifference(phi1,phi2)
cosphidif=cos(phi1).*cos(phi2)+sin(phi1).*sin(phi2);
sinphidif=sin(phi1).*cos(phi2)-cos(phi1).*sin(phi2);
phidif=atan2(sinphidif,cosphidif);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [s,dt,npt]=GenerateMultiCompDataFrMod
npt=100000;
dt=1/100; t=(1:npt)*dt;
omega1=sqrt(2)/30;
omega2=sqrt(5)/60;
amp=1+0.95*cos(omega1*t);
p=t+5*sin(omega2*t);
s=amp.*(cos(p)+0.2*cos(2*p+pi/6)+0.1*cos(3*p+pi/3));
end