原创 carrier track in matlab

%================= by G.S @ 607 08.05.08 ====================
clear all;
close all;
clc;
%==================================================
pi=3.1416;                      % PI
MHz=1000000;
KHz=1000;                       % kilo

%========freq vars====================
fs=4*KHz;                      % freq of sample

fc_base=0*KHz;                 % 基准频率
fc_local=0*KHz;                % now freq of local carrier
%=========global vars==================
N=fs*0.001;                           % data Length once in cycle: 1 ms

mod_track=10;
mod_rotate=10;

p_track=0;
p_rotate=0;

%==========loop filter parameters===========
K0=2^(-3);                 
K1=2^(-7);                     

%==========track var==================
d_cic_track=0;
Ri_track=zeros(1,2);              
Rq_track=zeros(1,2);
delta_phase=zeros(1,2);         % input of input of the loop filter
LF_out=zeros(1,2);              % output of the loop filter

fc_local_test=[];

%=========phase var==============
phase_local=0;
ROT=exp(-j*(phase_local));

d_cic_rotate=0;
Ri_rotate=0;              
Rq_rotate=0;

x_phy=zeros(1,2);
y_phy=0;

x_phy_test=[];

%========= NCO init============
n=0:N-1;
NCO=exp(-j*(2*pi*fc_local*n/fs));
last_phase_carrier = mod(2*pi*fc_local*(N-1)/fs,2*pi);
first_phase_carrier = last_phase_carrier + 2*pi*fc_local/fs;

%============data init============
delta_f=30;
snr=-6;
time=3;
[signal_noise,fc_in] = signal_gen(delta_f,snr,time);
%========== global switch

track_en=1;         %是否跟踪
rotate_en=1;        %是否纠相
%================================
cycle_close=length(signal_noise)/N;
for cycle="1:cycle"_close
    %=== 跟踪150ms后，频率锁定 =====================
    if(cycle*N/fs*1000 >= 500)% && track_en == 1)
        track_en = 0;
        fc_local=30+20*rand(1)-10;
    end

    %======================= get data =========================
    din = signal_noise( (cycle-1)*N+1 : cycle*N );
   
    %======================= ddc, rotate =========================
    dout_ddc = din.*NCO;

    dout_rot = dout_ddc.*ROT;
   
    %============================ freq_track 

    if(track_en==1)  
        p_track = p_track + 1;
        d_cic_track = d_cic_track + sum(dout_ddc);   

        %========================track========================
        if( p_track==mod_track )
            p_track = 0;  
            Ri_track(2) = real(d_cic_track);
            Rq_track(2) = imag(d_cic_track);
           
            %==============叉积鉴频=======
            delta_phase(2) = atan2(Ri_track(1)*Rq_track(2)-Ri_track(2)*Rq_track(1), Ri_track(1)*Ri_track(2)+Rq_track(1)*Rq_track(2))*fs/(2*pi*(N*mod_track));
            %==============环路滤波器===============================      
            LF_out(2) = (K0+K1)*delta_phase(2) - K0*delta_phase(1) + LF_out(1);
            %==============update vars=============================
            LF_out(1) = LF_out(2);
            delta_phase(1) = delta_phase(2);
            Ri_track(1) = Ri_track(2);     
            Rq_track(1) = Rq_track(2);
            d_cic_track = 0;
            %==============freq of output of the NCO===============
            fc_local = fc_local + LF_out(2);
            NCO="exp"(-j*(2*pi*fc_local*n/fs + first_phase_carrier));
            last_phase_carrier = mod(2*pi*fc_local*(N-1)/fs + first_phase_carrier, 2*pi);
            first_phase_carrier = last_phase_carrier + 2*pi*fc_local/fs;
        else
            NCO="exp"(-j*(2*pi*fc_local*n/fs + first_phase_carrier));
            last_phase_carrier = mod(2*pi*fc_local*(N-1)/fs + first_phase_carrier, 2*pi);
            first_phase_carrier = last_phase_carrier + 2*pi*fc_local/fs;
        end
    else %=== ( track_en==0 )
        NCO="exp"(-j*(2*pi*fc_local*n/fs + first_phase_carrier));
        last_phase_carrier = mod(2*pi*fc_local*(N-1)/fs + first_phase_carrier, 2*pi);
        first_phase_carrier = last_phase_carrier + 2*pi*fc_local/fs;
    end 

    if(rotate_en==1)
        p_rotate = p_rotate + 1;
        d_cic_rotate = d_cic_rotate + sum(dout_rot);
        if( p_rotate==mod_rotate )
            p_rotate = 0;
            Ri_rotate = real(d_cic_rotate);              
            Rq_rotate = imag(d_cic_rotate);
            d_cic_rotate = 0;
            %=============================
            x_phy(2) = atan2(Rq_rotate, Ri_rotate); 
            y_phy = (x_phy(1) + x_phy(2))/8;
            x_phy(1) = x_phy(2);

            phase_local = phase_local + y_phy;
            ROT = exp(-j*(phase_local));
        end
    end
    %================record NCO freq==========================               
    fc_local_test = [fc_local_test fc_local];
    x_phy_test = [x_phy_test x_phy(2)];
end

t=(1:length(fc_local_test))*N/fs*1000;
figure;
subplot(3,1,1);
plot(t,fc_local_test-fc_base,'.-b');grid on;
subplot(3,1,2);
plot(fc_in(1:4:end)-fc_local_test,'.-r');grid on;
subplot(3,1,3);
plot(t,x_phy_test*180/pi,'.-r');grid on;