lock_phase.m

mimo2x2天线选择系统的全系统matlab程序,先前的是dsp程序.

function [delta, theta] = lock_phase(K0, initial_phase, initial_delta, pilot_seq)
%% function [delta, ] = lock_phase(K0, initial_phase, initial_delta, pilot_seq)

%%%% Input
%  K0: nominal frequency (ie 2*Pi*Fc/Fs)
%  pilot_seq: pilot sequence (sinusoid)

%%%% Return 
%  delta: estimate of frequency offset (ie 2*Pi*F_offset/Fs)
%  theta: next phase

% DPLL parameters
alpha = .8;
loop_gain  = .15;

% Order of the FIR low pass filter
filter_order = 20;

% Cut off frequency at the center between Fc and 2*Fc
cut_off = 1.5*K0/(2*pi);

lpf = fir1(filter_order, cut_off);

data_len = length(pilot_seq);
in_seq = pilot_seq;
theta = initial_phase;


loop_filter_output = zeros(1, data_len + 1);
loop_filter_output(1) = initial_delta;

Ich = zeros(1, filter_order-1 + data_len); %%% insert filter_order-1 zeros before actual data
Qch = zeros(1, filter_order-1 + data_len);

out_seq = zeros(1, data_len);

pos = filter_order; 

for k = 1:data_len
    %    [cosval, sinval] = lookup_cos_sin(cos_tab, theta);
 
    Ich(pos) = cos(theta)*in_seq(k);
    Qch(pos) = sin(theta)*in_seq(k);
    %   Ich(pos) = cosval*in_seq(k);
    %   Qch(pos) = sinval*in_seq(k);    
    
    %%% Low Pass Filter
    I_filter_output = 0;
    Q_filter_output = 0;
    
    for i = 1:filter_order
        I_filter_output = I_filter_output + lpf(i)*Ich(pos-i+1);
        Q_filter_output = Q_filter_output + lpf(i)*Qch(pos-i+1);
    end
    
    phase_diff = -(Q_filter_output/I_filter_output);
    
    %%% Loop Filter
    loop_filter_output(k+1) = loop_filter_output(k)*alpha + (1-alpha)*phase_diff;

    loop_filter_output(k+1) = loop_filter_output(k+1)*loop_gain;
    
    out_seq(k) = loop_filter_output(k+1);    

    %%%% NCO
    theta = theta + K0 + loop_filter_output(k+1);
    %     if theta >= 2*pi
    %         theta = theta - 2*pi;
    %     elseif theta<0
    %         theta = theta + 2*pi;
    %     end
    twopi = 2*pi;
        
    while (theta>=twopi) 
        theta = theta - twopi;
    end
    while (theta<0) 
            theta = theta + twopi;
    end
    
    pos = pos + 1;
end


delta  = mean(loop_filter_output(data_len-256:data_len));
plot(out_seq);

%std(loop_filter_output(data_len-100:data_len))
%delta = loop_filter_output(data_len-31:data_len);