clc;
clear all;

Chip_rate = 6.3*10^6;          % 擴(kuò)頻碼速率(chips/s)
Spreadcode_len = 63;           % 擴(kuò)頻碼長(zhǎng)度(chips)
Data_len = 400;                % 數(shù)據(jù)長(zhǎng)度(symbols)
Freq_carrier = 4*10^9;        % 射頻載波頻率(Hz)
Freq_sample = 6.3*10^6;        % 接收機(jī)采樣頻率(Hz)
Symbol_rate = 1*10^5;          % 符號(hào)速率(sym/s)
SNR = 0;                     % 信噪比(dB)
load GoldSequence.mat;
PNcode = 2*GoldSequence(1:Data_len,1:63)-1;       % 偽碼長(zhǎng)度：數(shù)據(jù)長(zhǎng)度*63
n_chip = 63;

% JPL高動(dòng)態(tài)模型
g = 9.8;                       % 重力加速度(m/s^2)
c = 3*10^8;                    % 光速(m/s)
ts = 1/3150;                    % JPL的采樣時(shí)間(s),所設(shè)置的值與擴(kuò)碼序列的長(zhǎng)度有關(guān)
V0 = -500;                     % 初始速度(m/s)
Theta0 = 2*pi*randn(1);        % 初始相位(rad)

Time = [3 0.5 2 0.5 2];        % JPL模型的各狀態(tài)持續(xù)時(shí)間
State_length = round(Time/ts); % 各狀態(tài)對(duì)應(yīng)的采樣時(shí)間長(zhǎng)度
% 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;                                                      % 由速度計(jì)算出的多普勒頻偏(Hz)
Freq_Doppler_der1 = Freq_carrier / c * Acceleration;                                             % 由加速度計(jì)算出的頻偏一次變化率(Hz/s)
Freq_Doppler_der2 = Freq_carrier / c * Jerk;                                                     % 由加加速度計(jì)算出的頻偏二次變化率(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; %由多普勒頻偏引起的相位變化

%================== 數(shù)據(jù)序列 ===================%
Data_sequence = randi(2,1,Data_len)-1;         % 數(shù)據(jù)序列

%================== 信道編碼 ===================%
% trellis = poly2trellis([5,4],[23 35 0;0 5 13]);
% Data_sequence_conv = convenc(Data_sequence,trellis);   % 經(jīng)卷積編碼后為240bit

%================== BPSK調(diào)制 ===================%
Data_sequence_mod = BPSKMod(Data_sequence);       % 經(jīng)BPSK調(diào)制后仍為240bit

%================== 擴(kuò)頻調(diào)制 ===================%
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
%================== 環(huán)路參數(shù)設(shè)置 ===================%
k = 0.5;
r = 3;
Bl = 300;   % 環(huán)路帶寬，根據(jù)跟蹤門(mén)限設(shè)置
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