pll3_4.zip_GPS跟踪环路_matlab 锁相环_pll3_三阶环路_跟踪环路

%CPAFC，跟踪JPL动态 clear all; close all; clc; g = 9.8; ts = 1/500; jerk = [zeros(1,3/ts) 0*g*ones(1,0.5/ts) zeros(1,2/ts) -0*g*ones(1,0.5/ts) zeros(1,2/ts)]; accel = [-25*g cumsum(jerk(1:end-1))*ts-25*g]; vol = [0 cumsum(accel(1:end-1)*ts+jerk(1:end-1)*ts^2/2)]; fc = 1.575e9; c = 3e8; f0 = fc/c*vol; f1 = fc/c*accel; f2 = fc/c*jerk; theta0 = 2*pi*rand(1); theta = theta0+cumsum(f0)*2*pi*ts+cumsum(f1).*2*pi*ts^2/2+cumsum(f2).*2*pi*ts^3/6; Z = [cos(theta); sin(theta)]; num_all = length(theta); % ts = 1/9400; % num_all = 8000; % f1_ini = 2.5e3; % f0_ini = 10; % f2 = zeros(1,num_all); % f1 = [f1_ini cumsum(f2(1:end-1))*ts+f1_ini]; % f0 = [f0_ini cumsum(f1(1:end-1))*ts+f0_ini]; % theta0 = 2*pi*rand(1); % theta = theta0+cumsum(f0)*2*pi*ts+cumsum(f1).*2*pi*ts^2/2+cumsum(f2).*2*pi*ts^3/6; % Z = [cos(theta); sin(theta)]; C_No=25;%-1+10*log10(9400); noise_power = 1/ts/(10^(C_No/10))/2; Z = Z+sqrt(noise_power)*randn(size(Z)); sc = 0.707; BL = 5; Wn = 2*BL/(sc+1/4/sc); k0 = 1; k1 = 1; C1 = 8*sc*Wn*ts/(k0*k1*(4+4*sc*Wn*ts+(Wn*ts)^2)); C2 = (4*(Wn*ts)^2)/(k0*k1*(4+4*sc*Wn*ts+(Wn*ts)^2)); flt_out = zeros(1); acc_out = zeros(1); f_err = zeros(1); nco_out = zeros(1); rec_nco = zeros(1); i_in = Z(1,1); q_in = Z(2,1); for num=2:num_all i_in(num) = Z(1,num)*cos(rec_nco(num-1))+Z(2,num)*sin(rec_nco(num-1)); q_in(num) = Z(2,num)*cos(rec_nco(num-1))-Z(1,num)*sin(rec_nco(num-1)); f_err(num) = (i_in(num-1)*q_in(num)-q_in(num-1)*i_in(num))/2/pi/ts; acc_out(num) = f_err(num)*C2+acc_out(num-1); flt_out(num) = acc_out(num)+f_err(num)*C1; nco_out(num) = nco_out(num-1)+flt_out(num); rec_nco(num) = rec_nco(num-1)+2*pi*ts*nco_out(num); end fre_err = f0(2:end-0)-nco_out(1:end-1); figure(1) plot(fre_err) figure(2) plot(nco_out) rms_f = std(fre_err(1000:end)) mean_f = mean(fre_err(1000:end)); figure(3) plot(abs(fre_err(1000:end)))