[求助] 三阶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),所设置的值与扩码序列的长度有关
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