diff --git a/matlab/sync/cfo_estimate_cp.m b/matlab/sync/cfo_estimate_cp.m
index 0d3a4d723..ebc1f0735 100644
--- a/matlab/sync/cfo_estimate_cp.m
+++ b/matlab/sync/cfo_estimate_cp.m
@@ -1,4 +1,4 @@
-function cfo = cfo_estimate_cp(input_slot, Nsyms, sym_len, cp0_len, cp_len)
+function [cfo, r] = cfo_estimate_cp(input_slot, Nsyms, sym_len, cp0_len, cp_len)
 
 % Start correlating from the end. Nsyms is the number of symbols to
 % correlate starting from the end. 
diff --git a/matlab/sync/cp_corr.m b/matlab/sync/cp_corr.m
index 90c00bcb7..c97a2f82b 100644
--- a/matlab/sync/cp_corr.m
+++ b/matlab/sync/cp_corr.m
@@ -1,4 +1,4 @@
-function [ lambda ] = cp_corr( x, theta, N, L, rho)
+function [ lambda ] = cp_corr( x, theta, N, L)
 
 l0=sum(x(theta:theta+L-1).*conj(x(theta+N:theta+L+N-1)));
 l1=0;
diff --git a/matlab/tests/sync_test.m b/matlab/tests/sync_test.m
index 733b3523b..029ac76a1 100644
--- a/matlab/tests/sync_test.m
+++ b/matlab/tests/sync_test.m
@@ -1,12 +1,11 @@
 clear
 enb = lteTestModel('1.1','1.4MHz');
-Ntrials = 1; 
-SNR_values = 100;%linspace(0,10,6);
+Ntrials = 100; 
+SNR_values = linspace(-8,0,6);
 flen=1920;
 fft_size=128;
 
-tx_offset  = 0;%randi(50,1,Ntrials);
-cfo_offset = 0;%2*rand(1,Ntrials)-1;
+tx_offset  = randi(50,1,Ntrials);
 
 tx_signal = lteTestModelTool(enb); 
 pss_idx = flen/2-fft_size+1:flen/2;
@@ -36,7 +35,7 @@ addpath('../sync/')
 t = (0:length(tx_signal)-1).'/fft_size;
 
 L=16;
-M=3;
+M=4;
 diff=zeros(length(SNR_values),M);
 for snr_idx=1:length(SNR_values)
     fprintf('SNR=%.1f dB\n', SNR_values(snr_idx));
@@ -44,7 +43,6 @@ for snr_idx=1:length(SNR_values)
     rx_offset = zeros(M,length(tx_offset));
     for i=1:Ntrials
         [rx_signal, chinfo] = lteFadingChannel(cfg,tx_signal);
-       % rx_signal = rx_signal.*exp(-1i*cfo_offset(i)*2*pi.*t);
         %rx_signal = tx_signal; 
         SNR = 10^(SNRdB/10);    % Linear SNR  
         rx = [zeros(tx_offset(i),1); rx_signal];
@@ -60,22 +58,27 @@ for snr_idx=1:length(SNR_values)
         % srsLTE in tracking mode
         track_offset=2+rx_offset(2,i)+flen/2-fft_size-L/2;
         [rx_offset(3,i),corr_lt_track] = srslte_pss(enb,rx(track_offset:end),L);
-        rx_offset(3,i) = rx_offset(2,i) + (rx_offset(3,i) - fft_size - L/2);        
+        rx_offset(3,i) = rx_offset(2,i) + (rx_offset(3,i) - fft_size - L/2) + 1;        
+        
+        % CP based
+        [rx_offset(4,i), corr_lt_cp] = srslte_cp_synch(enb, rx);
+        
     end
-    diff(snr_idx,:)=mean(abs(rx_offset-repmat(tx_offset,M,1)),2);
+    diff(snr_idx,:)=mean(abs(rx_offset-repmat(tx_offset,M,1) - chinfo.ChannelFilterDelay),2);
 end
 
 if (Ntrials == 1)
     disp(diff)
     %plot(1:flen,abs(corr_res(1:flen)),1:flen,abs(corr_lt(1:flen)));
-    t=940:1000;
-    plot(t,abs(corr_lt(t)));
+    t=1:fft_size;
+    plot(t,abs(corr_lt_cp(t)));
+    %plot(lambda)
     %plot(1:L,abs(corr_lt_track),[L/2, L/2], [0 max(abs(corr_lt_track))])
     grid on
     
 else
     plot(SNR_values,diff);
-    legend('Matlab','srs find','srs tracking 16','srs tracking 64')
+    legend('Matlab','srs find','srs tracking','srs cp')
     grid on
     xlabel('SNR (dB)')
     ylabel('Avg time offset')