%sdcrt: squared difference carrier recovery test

clear all

%create pulse shaped received signal

Ts=1/10000;time=2; t=Ts:Ts:time;
M=50; N=length(t)/M;
m=pam(N,4,5);                             % 4-level signal of length N
mup=zeros(1,N*M); mup(1:M:end)=m;         % oversample by M
ps=hamming(M);                            % blip pulse of width M
s=filter(ps,1,mup);                       % convolve pulse shape with data
g=2/(sum(s.*s)/length(s));
f0=1000; phoff=-1;                        % carrier freq. and phase
c=cos(2*pi*f0*t+phoff);                   % construct carrier
r=s.*c;                                   % received signal

figure(1)
plotspec(s,Ts)
figure(2)
plotspec(r,Ts)

%send received signal through square and BPF

q=r.^2;                                % square nonlinearity
fl=500;                                % BPF length
yo=4*f0*Ts;
ff=[0 yo-0.01 yo-0.005 yo+0.005 yo+0.01 1];     % BPF center frequency at 2f0
fa=[0 0 1 1 0 0];                     
h=remez(fl,ff,fa);                     % BPF design via remez
r2bpf=filter(h,1,q);                   % filter to give bpf(r^2) 

figure(3)
plotspec(r2bpf,Ts)

%calculating cost function over data set
theta=-3:.1:3;
lent=length(t);
for ind=1:length(theta)               % for each possible theta
  x=cos(4*pi*f0*t+2*theta(ind));            % find x with this theta
  jsd(ind)=0.5*(g*r2bpf-x)*(g*r2bpf-x)'/lent;     % sd cost for this theta
end

figure(4)
plot(theta,jsd)                  % plot J(theta) vs theta
xlabel('Phase Estimates \theta')
ylabel('Cost Jsd(\theta)' )


%simulating sd and double sd  
fc=f0;musd=0.001;musd2=0.0002;
%fc=1.001*f0;musd=0.005;musd2=0.0003;
g=2/(sum(s.*s)/length(s));
thsd=zeros(1,length(t)); thsd(1)=0;  
thsd2=zeros(1,length(t)); thsd2(1)=0;  
for k=1:length(t)-1                       % algorithm update
  thsd(k+1)=thsd(k)-musd*(g*r2bpf(k)-cos(4*pi*fc*t(k)+2*thsd(k)))...
                        *sin(4*pi*fc*t(k)+2*thsd(k));
  thsd2(k+1)=thsd2(k)-musd2*(g*r2bpf(k)-cos(4*pi*fc*t(k)...
                        +2*thsd(k)+2*thsd2(k)))...
                        *sin(4*pi*fc*t(k)+2*thsd(k)+2*thsd2(k));
end
figure(5)
plot(t,thsd)
title('Phase Tracking via Squared Difference')
xlabel('time'); ylabel('phase offset')
figure(6)
plot(t,thsd2)
title('Phase Tracking via Double Squared Difference')
xlabel('time'); ylabel('phase offset')