[ÇóÖú] Èý½×PLL·ÂÕæ¸ß¶¯Ì¬ÆµÆ«µÄÇé¿öϳöÏÖÎÊÌ⣬ÇëÇó´ó¼ÒÖ¸µã£¡

clc;
clear all;

Chip_rate = 6.3*10^6;          % À©ÆµÂëËÙÂÊ(chips/s)
Spreadcode_len = 63;           % À©ÆµÂ볤¶È(chips)
Data_len = 400;                % Êý¾Ý³¤¶È(symbols)
Freq_carrier = 4*10^9;        % ÉäƵÔز¨ÆµÂÊ(Hz)
Freq_sample = 6.3*10^6;        % ½ÓÊÕ»ú²ÉÑùƵÂÊ(Hz)
Symbol_rate = 1*10^5;          % ·ûºÅËÙÂÊ(sym/s)
SNR = 0;                     % ÐÅÔë±È(dB)
load GoldSequence.mat;
PNcode = 2*GoldSequence(1:Data_len,1:63)-1;       % αÂ볤¶È£ºÊý¾Ý³¤¶È*63
n_chip = 63;

% JPL¸ß¶¯Ì¬Ä£ÐÍ
g = 9.8;                       % ÖØÁ¦¼ÓËÙ¶È(m/s^2)
c = 3*10^8;                    % ¹âËÙ(m/s)
ts = 1/3150;                    % JPLµÄ²ÉÑùʱ¼ä(s),ËùÉèÖõÄÖµÓëÀ©ÂëÐòÁеij¤¶ÈÓйØ
V0 = -500;                     % ³õʼËÙ¶È(m/s)
Theta0 = 2*pi*randn(1);        % ³õʼÏàλ(rad)

Time = [3 0.5 2 0.5 2];        % JPLÄ£Ð͵ĸ÷״̬³ÖÐøʱ¼ä
State_length = round(Time/ts); % ¸÷״̬¶ÔÓ¦µÄ²ÉÑùʱ¼ä³¤¶È
% Total_length = sum(State_length);
% Total_time = 0:1:Total_length-1;
% Jerk = [zeros(1,3/ts) 100*g*ones(1,0.5/ts) zeros(1,2/ts) -100*g*ones(1,0.5/ts) zeros(1,2/ts)];   % ¶þ´Î¼ÓËÙ¶È(m/s^3)
Jerk = [zeros(1,State_length(1)) 100*g*ones(1,State_length(2)) zeros(1,State_length(3)) -100*g*ones(1,State_length(4)) zeros(1,State_length(5))];   % ¶þ´Î¼ÓËÙ¶È(m/s^3)
Acceleration = [-25*g cumsum(Jerk(1:end-1)*ts)-25*g];                                            % Ò»´Î¼ÓËÙ¶È(m/s^2)
Velocity = [V0 cumsum(Acceleration(1:end-1)*ts + Jerk(1:end-1)*ts^2/2)+V0];                         % ·ÉÐÐËÙ¶È(m/s)
Freq_Doppler = Freq_carrier / c * Velocity;                                                      % ÓÉËٶȼÆËã³öµÄ¶àÆÕÀÕƵƫ(Hz)
Freq_Doppler_der1 = Freq_carrier / c * Acceleration;                                             % ÓɼÓËٶȼÆËã³öµÄƵƫһ´Î±ä»¯ÂÊ(Hz/s)
Freq_Doppler_der2 = Freq_carrier / c * Jerk;                                                     % ÓɼӼÓËٶȼÆËã³öµÄƵƫ¶þ´Î±ä»¯ÂÊ(Hz/s^2)
Theta = Theta0 + 2*pi*cumsum(Freq_Doppler)*ts + 2*pi*cumsum(Freq_Doppler_der1)*ts^2/2 + 2*pi*cumsum(Freq_Doppler_der2)*ts^3/6; %ÓɶàÆÕÀÕƵƫÒýÆðµÄÏàλ±ä»¯

%================== Êý¾ÝÐòÁÐ ===================%
Data_sequence = randi(2,1,Data_len)-1;         % Êý¾ÝÐòÁÐ

%================== ÐŵÀ±àÂë ===================%
% trellis = poly2trellis([5,4],[23 35 0;0 5 13]);
% Data_sequence_conv = convenc(Data_sequence,trellis);   % ¾­¾í»ý±àÂëºóΪ240bit

%================== BPSKµ÷ÖÆ ===================%
Data_sequence_mod = BPSKMod(Data_sequence);       % ¾­BPSKµ÷ÖƺóÈÔΪ240bit

%================== À©Æµµ÷ÖÆ ===================%
Spread_sequence = Data_sequence_mod(1) * PNcode(1,:);
for i=2:1:Data_len
    Spread_sequence = cat(2,Spread_sequence,Data_sequence_mod(i) * PNcode(i,:));
end

%================== ¼ÓƵƫºÍÔëÉù ===================%
Spread_sequence_offset = Spread_sequence .* exp(1i*Theta);
Spread_sequence_noise = Noise_Generate(Spread_sequence_offset,SNR);

PNcode1 = reshape(PNcode.',1,[]);
Signal_despread = Spread_sequence_noise.*PNcode1;            
% for k=1:Data_len
%     Signal_pro(k)=sum(Signal_despread((k-1)*n_chip+1:k*n_chip))/63;
% end
%================== »·Â·²ÎÊýÉèÖà ===================%
k = 0.5;
r = 3;
Bl = 300;   % »·Â·´ø¿í£¬¸ù¾Ý¸ú×ÙÃÅÏÞÉèÖÃ
updatetime = 1e-3;
d = 4*Bl*updatetime*(r-k)/(r*(r-k+1));
C1 = r*d/updatetime;
C2 = r*d^2/updatetime;
C3 = r*k*d^3/updatetime;
Simulation_len = length(Signal_despread);

acc1 = zeros(1,1);
acc2 = zeros(1,1);
temp = zeros(1,1);
Theta_err = zeros(1,Simulation_len);
Nco_out = zeros(1,Simulation_len);
Flt_out = zeros(1,Simulation_len);

for i=2:Simulation_len
   
    Signal_PLL(i) = Signal_despread(i)*exp(-1i*mod(Nco_out(i-1),2*pi));
    I_PLL(i) = real(Signal_PLL(i));
    Q_PLL(i) = imag(Signal_PLL(i));
    Theta_err(i)=sign(I_PLL(i))*Q_PLL(i)/abs(Signal_PLL(i));
%     Theta_err(i) = atan(Q_PLL(i)/I_PLL(i));
%     Theta_err(i) = angle(Signal_despread(i) * exp(-1i*Nco_out(i-1)));
    acc1 = acc1 + C3*Theta_err(i);
    temp = acc1 + C2*Theta_err(i);
    acc2 = acc2 + temp;
    Flt_out(i) = acc2 + C1*Theta_err(i);
   
    Nco_out(i) = Nco_out(i-1) + Flt_out(i)*updatetime;
   
end