Matlab 北斗B1C信号的捕获、跟踪和定位全过程

function [data_correlation, p_correlation, pll_lock, adjust_time, CN0, carrier_freq_new, code_freq_new, carrier_phase_new, code_phase_new] = B1C_tracking(pn_phase, carrier_offset, prn_no, N, sim_length, sp_BOC11, sd_BOC11) %导频分量跟踪 global settings fcarr = settings.fcarr; fcode = settings.fcode; Fs = settings.Fs; delta_t = 1/Fs;%采样间隔 %%%%%%%%%%%%环路参数 %环路积分时间 global dll_T global fll_T global pll_T global PllLock_T dll_T = 1e-3; fll_T = 1e-3; pll_T = 1e-3; PllLock_T = 1e-3; dll_bw = 10; dll_kexi = 0.707; dll_bw_fine = 10; dll_kexi_fine = 0.707; fll_bw = 10; fll_kexi = 0.707; pll_bw = 18; pll_bw_fine = 10; pll_lock_threshold_fine = 0.7;%环路锁定门限 K_step = 40;%%%%%%%%%%%从FLL转向PLL的时间ms down_no = 1; narrow1_th = 0.85;%%%%%%%%%%窄相关时超前滞后间隔小 噪声相关性大 跟踪精度高 能去除噪声抑制多径 narrow1_word = 0.25; narrow2_th = 0.95; narrow2_word = 0.1; code_word = Fs/(2*fcode); %%%%%%%%%%初始值 accum_p = 0; step_cnt = 0; dll_y = 0; dll_x = 0; fll_y = 0; fll_x = 0; pll_z = 0; pll_y = 0; ACC_I = 0; ACC_Q = 0; %读取数据 data_type = settings.data_type; data_unit = settings.data_unit; filename = settings.filename; num_drop = settings.num_drop; j = sqrt(-1); fid = fopen(filename); fseek(fid,num_drop*data_unit,'bof'); data = fread(fid, 2*(11*N + pn_phase),data_type)'; signal = data(1:2:end)+j*data(2:2:end); adjust_signal = signal(end-N+1:end); %待处理信号 %生成测距码 fcarr = fcarr+carrier_offset; carrier_freq_new = fcarr; code_freq_new = 2*fcode; %初始化 carrier_phase_new = 0; code_phase_new = 0; CN0 = zeros(1,sim_length); p_correlation = zeros(1,sim_length); data_correlation = zeros(1,sim_length); adjust_time = zeros(1,sim_length); fll_freq(1:10) = 0; carr_phase_reg = zeros(1,sim_length); code_phase_reg = zeros(1,sim_length); fcarr_reg = zeros(1,sim_length); fcode_reg = zeros(1,sim_length); dllx_reg = zeros(1,sim_length); fllx_reg = zeros(1,sim_length); pllx_reg = zeros(1,sim_length); pll_lock = zeros(1,sim_length); complete_count = ceil(sim_length/1000); waitbar_handle = waitbar(0, sprintf('正在跟踪PRN %d 卫星信号。请耐心等待...', prn_no)); d_word = 0.5*code_word; % 此处可以调整步长,宽相关、窄相关 半个码片 % = = = = = = = = = = = = = 跟踪处理 = = = = = = = = = = = = = = = = = = = = = = = = = = = = i = 2; while i <= sim_length %sim_length单位是ms demo_sig = adjust_signal(1:N); t = 1:N; %因为相位多了90度 所以数据电文*数据测距码*BOC11+导频测距码*BOC11成了虚部 导频测距码*BOC61成了实部 rev_sig = demo_sig.*exp( -j*2*pi*carrier_freq_new*delta_t.*t - j*carrier_phase_new -j*pi/2);%相位多了90度 carrier_phase_new = mod((carrier_phase_new + 2*pi*carrier_freq_new*10^-3), 2*pi); carr_phase_reg(i) = carrier_phase_new; accum_I_last = real(accum_p); accum_Q_last = imag(accum_p); code_phase_new = code_phase_new - dll_x*10^-3 * code_word; %%dll_x是频率*1ms时间*采样点数 换算到码相位 code_phase_reg(i) = code_phase_new; [b1c_ee] = B1C_code_sample(sp_BOC11, 10*N/down_no, code_phase_new-d_word, Fs/down_no, code_freq_new); [b1c_e] = B1C_code_sample(sp_BOC11, 10*N/down_no, code_phase_new-0.5*d_word, Fs/down_no, code_freq_new); [b1c_p] = B1C_code_sample(sp_BOC11,10*N/down_no, code_phase_new, Fs/down_no, code_freq_new);%导频测距码主码 [b1c_l] = B1C_code_sample(sp_BOC11, 10*N/down_no, code_phase_new+0.5*d_word, Fs/down_no, code_freq_new); [b1c_ll] = B1C_code_sample(sp_BOC11,10*N/down_no,code_phase_new+d_word, Fs/down_no, code_freq_new); [data_p] = B1C_code_sample(sd_BOC11, 10*N/down_no, code_phase_new, Fs/down_no, code_freq_new);%数据测距码主码 loc = mod((i-2)*N+1,10*N) : mod((i-2)*N+1,10*N)+N-1; temp_accum = rev_sig*[b1c_ee(loc)', b1c_e(loc)', b1c_p(loc)', b1c_l(loc)', b1c_ll(loc)', data_p(loc)']; % coef = xcorr(rev_sig, b1c_p(loc)'); % disp( find(coef == max(coef))); % accum_ee = temp_accum(1); accum_e = temp_accum(2); accum_p = temp_accum(3); accum_l = temp_accum(4); % accum_ll = temp_accum(5); accum_data_p = temp_accum(6); % (j*（1/2*数据电文*数据测距码*BOC11+sqrt(1/11)导频测距码*BOC61） + sqrt(29/44)导频测距码*BOC11)*导频测距码主码*BOC11 % = j*0 + sqrt(29/44)导频测距码子码(理论上） 因为数据测距码*导频测距码相关为0 ,BOC61和BOC11相互正交 % 实部是导频测距码子码 p_correlation(i) = real(accum_p); %导频子码 % (j*数据电文*数据测距码*BOC11+导频测距码*BOC61 + 导频测距码*BOC11)*数据测距码主码*BOC11 % = j*数据电文 + 0(理论上） 因为数据测距码*导频测距码相关为0 % 虚部是数据电文 data_correlation(i) = imag(accum_data_p);%数据电文 if pll_lock(i-1) < 0.9 [dll_x,dll_y] = B1C_DLL(accum_e, accum_l, dll_bw, dll_kexi, dll_y); dllx_reg(i) = dll_x; else [dll_x,dll_y] = B1C_DLL(accum_e, accum_l, dll_bw_fine, dll_kexi_fine, dll_y); dllx_reg(i) = dll_x; end code_freq_new = 2*fcode-dll_x; fcode_reg(i) = code_freq_new; adjust_time(i) = code_phase_new / (code_word/down_no) * 1/ code_freq_new; %%%%%%%%将码相位调整值换算为时间 [CN0(i), pll_lock(i), ACC_I, ACC_Q] = lock_detector(real(accum_p), imag(accum_p), ACC_I, ACC_Q); % disp(pll_lock(i)); if step_cnt <= K_step [fll_x, fll_y] = B1C_FLL(accum_I_last, accum_Q_last, accum_p, fll_bw, fll_kexi, fll_x, fll_y); carrier_freq_new = fcarr+ fll_x; fll_freq(2:10) = fll_freq(1:9); fll_freq(1) = fll_x; freq_fll2pll = mean(fll_freq);%%%fll辅助 step_cnt = step_cnt+1; fcarr_reg(i) = carrier_freq_new; fllx_reg(i) = fll_x; else if pll_lock(i) < pll_lock_threshold_fine [pll_x,pll_y,pll_z] = B1C_PLL(accum_p, pll_bw, pll_y, pll_z); else [pll_x,pll_y,pll_z] = B1C_PLL(accum_p, pll_bw_fine, pll_y, pll_z); end carrier_freq_new = fcarr + freq_fll2pll + pll_x ; fcarr_reg(i) = carrier_freq_new; pllx_reg(i) = pll_x; end if pll_lock(i) > narrow2_th d_word = narrow2_word*code_word/down_no; % 采用窄相关方式，提高跟踪精度 elseif pll_lock(i) > narrow1_th d_word = narrow1_word*code_word/down_no; % 采用窄相关方式，提高跟踪精度 else d_word = 0.5*code_word/down_no; end if mod(i,complete_count) == 0 msg = sprintf('PRN %d 已跟踪完成 %3.1f%% \n 请耐心等待...', prn_no, 100*i/sim_length); waitbar(i/sim_length, waitbar_handle, msg); end data = fread(fid, 2*N, data_type)'; adjust_signal = data(1:2:end) + j*data(2:2:end); if length(adjust_signal) < N disp('已跟踪完PRN %d 卫星信号',prn_no); break; else i = i+1; end if i == 500 plot(pll_lock(1:i)); end end CN0(1) = CN0(2); fclose(fid); close(waitbar_handle); end