TOA的MATLAB定位算法代码

%=============================================================================% %= Asynchronous UWB position location system =% %= Programmed by Jasurbek Khodjaev =% %= Yeungnam University Mobile Communication Lab. =% %= MCL 2006 =% %=============================================================================% clear all; clc; %--------------- Initialization ----------------------------------------- %------------------------------------------- % Speed of Light light_speed = 3e8; % Coordinates of APs AP = [0 0; 0 20; 20 0]; % in meters % Number of Access Points (AP) num_ap = length(AP); % Tag's initial coordinate Tag = [14 12]; % Pulse shape pulse_order = 1; % 0-Gaussian pulse, 1-First derivative of Gaussian pulse, 2 - Second derivative; % Number of bits num_bits = 1000; % Pulse repetition interval, PRI pri = 200e-9; % The SNR range (in dB) % EbNo = 10000; fs = 20e9; %sample rate-10 times the highest frequency in GHz ts = 1/fs; %sample period t = [(-1.5E-9-ts):ts:(1.5E-9-ts)]; %vector with sample instants t1 = .5E-9; %pulse width(0.5 nanoseconds) %------------------------------------------------------------------------- %----------------- Transmitter ------------------------------------------- A =1;%positive value gives negative going monopulse;neg value gives %positive going monopulse [y] = monocycle(fs, ts, t, t1, A, pulse_order); % Generate Gaussian pulse n_pulse_pri = round(pri/ts); % Sampling of PRI sig = zeros(1,n_pulse_pri); sig(1:length(y)) = y; % One pulse in one PRI % Distance calculation between each AP and the Tag, IDEAL case for ii = 1:num_ap dist_ap_tag(ii) = dist_t(AP(ii,:), Tag); % Time from each AP to Tag time_ap_tag(ii) = dist_ap_tag(ii)/light_speed; end % Distance from AP1 to other APs % AP1 is always master AP for ii = 2:num_ap dist_ap_ap(ii-1) = dist_t(AP(1,:), AP(ii,:)); time_ap_ap(ii-1) = dist_ap_ap(ii-1)/light_speed; end shift_const = 40; kkk = 0; for EbNo = -40:5:-30 kkk = kkk + 1; for nloop = 1:1 %++++++++++++++++ TRANSMISSION +++++++++++++++++++++++++++++++ for jj = 1:num_bits % From AP1 to TAG del_sample_ap_tag = round(time_ap_tag(1)/ts); xx = zeros(1,del_sample_ap_tag); % Delayed signals from AP1 to Tag del_sig_ap1_tag(jj,:) = [xx sig(1:end-length(xx))]; h = uwb_channel(dist_ap_tag(1)); conv_data = conv(del_sig_ap1_tag(jj,:), h); ap1_tag_chan(jj, :) = conv_data(1:length(sig)); % % kk = 0; % % for ll = 1:length(h) % % add_chnl = conv(del_sig_ap1_tag(jj,:), h(ll)); % % added(ll,:) = [zeros(1,shift_const*kk) add_chnl(1:end - shift_const*kk)]; % % kk = kk + 1; % % end % % ap1_tag_chan(jj, :) = sum(added); % From AP1 to AP2 del_sample_ap_tag = round(time_ap_ap(1)/ts); xx = zeros(1,del_sample_ap_tag); % Delayed signals from AP1 to AP2 del_sig_ap2_tag(jj,:) = [xx sig(1:end-length(xx))]; h = uwb_channel(dist_ap_ap(1)); conv_data = conv(del_sig_ap2_tag(jj,:), h); ap1_ap2_chan(jj, :) = conv_data(1:length(sig)); % % kk = 0; % % for ll = 1:length(h) % % add_chnl = conv(del_sig_ap2_tag(jj,:), h(ll)); % % added(ll,:) = [zeros(1,shift_const*kk) add_chnl(1:end - shift_const*kk)]; % % kk = kk + 1; % % end % % ap1_ap2_chan(jj, :) = sum(added); % From AP1 to AP3 del_sample_ap_tag = round(time_ap_ap(2)/ts); xx = zeros(1,del_sample_ap_tag); % Delayed signals from AP1 to AP3 del_sig_ap3_tag(jj,:) = [xx sig(1:end-length(xx))]; h = uwb_channel(dist_ap_ap(2)); conv_data = conv(del_sig_ap3_tag(jj,:), h); ap1_ap3_chan(jj, :) = conv_data(1:length(sig)); % % kk = 0; % % for ll = 1:length(h) % % add_chnl = conv(del_sig_ap2_tag(jj,:), h(ll)); % % added(ll,:) = [zeros(1,shift_const*kk) add_chnl(1:end - shift_const*kk)]; % % kk = kk + 1; % % end % % ap1_ap3_chan(jj, :) = sum(added); end EbNo %------------------------------------------------------- % Additive White Gaussian Noise (AWGN) Channel --------- noise_var = 0.5 * 10^(-EbNo/10); for jj = 1:num_bits ap1_tag_chan_wgn(jj,:) = ap1_tag_chan(jj,:)/std(ap1_tag_chan(jj,:)) + randn(1,length(ap1_tag_chan(jj,:))) .* sqrt(noise_var); ap1_ap2_chan_wgn(jj,:) = ap1_ap2_chan(jj,:)/std(ap1_ap2_chan(jj,:)) + randn(1,length(ap1_ap2_chan(jj,:))) .* sqrt(noise_var); ap1_ap3_chan_wgn(jj,:) = ap1_ap3_chan(jj,:)/std(ap1_ap3_chan(jj,:)) + randn(1,length(ap1_ap3_chan(jj,:))) .* sqrt(noise_var); end %------------------------------------------------------- %------------------- TAG receiver ------------------------------ % Correlator received_signl_ap1 = sum(ap1_tag_chan_wgn)/num_bits; xc = xcorr(y, received_signl_ap1); [a,delay1]=max(xc); TOA_tag = (length(sig) - delay1) * ts; for jj = 1:num_bits % From TAG to AP1 del_sample_ap_tag = round(time_ap_tag(1)/ts); xx = zeros(1,del_sample_ap_tag); % Delayed signals from AP1 to Tag del_sig_ap1_tag(jj,:) = [xx sig(1:end-length(xx))]; h = uwb_channel(dist_ap_tag(1)); conv_data = conv(del_sig_ap1_tag(jj,:), h); tag_ap1_chan(jj, :) = conv_data(1:length(sig)); % % kk = 0; % % for ll = 1:length(h) % % add_chnl = conv(del_sig_ap1_tag(jj,:), h(ll)); % % added(ll,:) = [zeros(1,shift_const*kk) add_chnl(1:end - shift_const*kk)]; % % kk = kk + 1; % % end % % tag_ap1_chan(jj, :) = sum(added); % From TAG to AP2 del_sample_ap_tag = round(time_ap_tag(2)/ts); xx = zeros(1,del_sample_ap_tag); % Delayed signals from AP1 to Tag del_sig_ap2_tag(jj,:) = [xx sig(1:end-length(xx))]; h = uwb_channel(dist_ap_tag(2)); conv_data = conv(del_sig_ap2_tag(jj,:), h); tag_ap2_chan(jj, :) = conv_data(1:length(sig)); % % kk = 0; % % for ll = 1:length(h) % % add_chnl = conv(del_sig_ap2_tag(jj,:), h(ll)); % % added(ll,:) = [zeros(1,shift_const*kk) add_chnl(1:end - shift_const*kk)]; % % kk = kk + 1; % % end % % tag_ap2_chan(jj, :) = sum(added); % From TAG to AP3 del_sample_ap_tag = round(time_ap_tag(3)/ts); xx = zeros(1,del_sample_ap_tag); % Delayed signals from AP1 to Tag del_sig_ap3_tag(jj,:) = [xx sig(1:end-length(xx))]; h = uwb_channel(dist_ap_tag(3)); conv_data = conv(del_sig_ap3_tag(jj,:), h); tag_ap3_chan(jj, :) = conv_data(1:length(sig)); % % kk = 0; % % for ll = 1:length(h) % % add_chnl = conv(del_sig_ap3_tag(jj,:), h(ll)); % % added(ll,:) = [zeros(1,shift_const*kk) add_chnl(1:end - shift_const*kk)]; % % kk = kk + 1; % % end % % tag_ap3_chan(jj, :) = sum(added); end %------------------------------------------------------- % Additive White Gaussian Noise (AWGN) Channel --------- noise_var = 0.5 * 10^(-EbNo/10); for jj = 1:num_bits tag_ap1_chan_wgn(jj,:) = tag_ap1_chan(jj,:)/std(tag_ap1_chan(jj,:)) + randn(1,length(tag_ap1_chan(jj,:))) .* sqrt(noise_var); tag_ap2_chan_wgn(jj,:) = tag_ap2_chan(jj,:)/std(tag_ap2_chan(jj,:)) + randn(1,length(tag_ap2_chan(jj,:))) .* sqrt(noise_var); tag_ap3_chan_wgn(jj,:) = tag_ap3_chan(jj,:)/std(tag_ap3_chan(jj,:)) + randn(1,length(tag_ap3_chan(jj,:))) .* sqrt(noise_var); end %------------------------------------------------------- %------------------- AP 1's RECEIVER ------------------- % Correlator received_tag_ap1 = sum(tag_ap1_chan_wgn)/num_bits; xc = xcorr(y, received_tag_ap1); [a,delay1]=max(xc); TOA_ap1 = (length(sig) - delay1) * ts; TOA_1 = (TOA_ap1 + TOA_tag)/2; %------------------- AP 2's RECEIVER ------------------- % Correlator r