location_定位_matlab_无源定位_TDOA_

clc; clear; close all; SNR = 60; %设置信噪比的大小 N = 3; %基站数目 c = 3*10^8; %定义光速 LL = 100 ; %距离指数 %基站1 P1x = 0; P1y = 0; %基站2 P2x =- LL; P2y =1.5*LL; %基站3 P3x = LL; P3y =1.5*LL ; %基站坐标 BS = [[P1x,P1y];[P2x,P2y];[P3x,P3y]]; %这里随机生成移动物体的位置坐标 ; P0x = LL; P0y = 2*LL; %定义发送信号，这里发送信号根据要求使用AM发送信号 fc = 40e6 ; %载波频率 fs = 100e6; %采样频率 t = 0:1/fs:14999/fs; No = length(t); %信号的长度 A = 10; %发送信号的幅度 s=A*cos(2*pi*fc*t) ; %原始信号 %通过计算理论距离，来模拟出到三个基站的通过延迟后的实际信号 R0 = zeros(1,N); %生成零矩阵 目标到基站距离 for i = 1:N R0(i) = sqrt((P0x-BS(i,1))^2 + (P0y-BS(i,2))^2); end %通过理论距离计算三个理论延迟大小 tDelay = zeros(1,N); %定义延迟 tDot = zeros(1,N); %定义延迟点数 r = zeros(N,No); %定义时延信号 for i = 1:N tDelay(i) = R0(i)/c; tDot(i)=round(tDelay(i)*fs); %计算得到实际的延迟后的信号 r(i,(tDot(i)+1):length(s))=s(1:length(s)-tDot(i)); end %加入噪声 r2(1,:) = awgn(r(1,:),SNR); r2(2,:) = awgn(r(2,:),SNR); r2(3,:) = awgn(r(3,:),SNR); %计算延迟 广义互相关运算 delay = zeros(N,1); m = zeros(N,1); maxval= zeros(N,1); locmax= zeros(N,1); for n=2:N b1f=fft(r(n,:),2*No-1); b2f=fft(r(1,:),2*No-1); b2fc=conj(b2f); neuma=(b1f).*(b2fc); deno=abs(neuma); %? GPHAT=neuma./deno; GPHATi=ifft(GPHAT); R12=fftshift(real( GPHATi)); [maxval(n),locmax(n)]=max(abs(R12)); delay(n)=locmax(n)-length(R12)/2-2; m(n)=delay(n)/fs; end %通过TDOA方法，根据理论估计延迟得到实际的坐标点位置 R_theory = zeros(1,N); Kj = zeros(1,N); for i = 2:N R_theory(i) = m(i)*c; Kj(i)= BS(i,1)^2 + BS(i,2)^2; end H_tdoa = BS(2:N,:); C_tdoa = -1*R_theory(2:N)'; D_tdoa = 0.5*(Kj(2:N)-R_theory(2:N).^2)'; % K_tdoa = inv(H_tdoa) *C_tdoa; % Q_tdoa = inv(H_tdoa)*D_tdoa; K_tdoa = H_tdoa\C_tdoa; Q_tdoa = H_tdoa\D_tdoa; %计算得到abc三个参数 as = sum(K_tdoa.^2) -1; bs = sum(2*K_tdoa.*Q_tdoa); cs = sum(Q_tdoa.^2); R1 = (-bs -sqrt(bs^2-4*as*cs))/(2*as); xyChan = K_tdoa.*R1 + Q_tdoa; %计算估计得到的XY坐标点 X_theory = xyChan(1); Y_theory = xyChan(2); %显示理论估计值 figure; plot(P1x,P1y,'b^','Markersize',8);hold on; plot(P2x,P2y,'b^','Markersize',8);hold on; plot(P3x,P3y,'b^','Markersize',8);hold on; axis([-5*LL,7*LL,-5*LL,7*LL]); grid on; xlabel('X axis'); ylabel('Y axis'); text(P1x+8,P1y+8,'BS1'); text(P2x+8,P2y+8,'BS2'); text(P3x+8,P3y+8,'BS3'); plot(P0x,P0y,'bo','Markersize',8); hold on; text(P0x-10,P0y-11,'M'); plot(X_theory,Y_theory,'rs','Markersize',8);hold on; text(X_theory+8,Y_theory+8,'估计位置'); title('TDOA估计目标位置');