highspeedlogic算法仿真-TDOA的matlab仿真

clc;

clear;

close all;

warning off;

 

 

%设置信噪比的大小

SNR    = [10,20,30,40,50,60,70,80];

%多径延迟

Mpd    = [0];

%基站之间的时间同步误差

Timesyn= [0];

%基站数目

N      = 3;

%定义光速

c      = 3*10^8;    

%距离指数

LL     = 15000;

%信号长度

SL     = 8;

P1x    = 0;

P1y    = 0;

%产生一个距离

P2x    = sqrt(2)*LL + LL;

P2y    = 0;

P3x    = 0;

%产生一个距离

P3y    = sqrt(3)*LL + LL;

BS = [[P1x,P1y];[P2x,P2y];[P3x,P3y]];

 

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;

text(P1x+800,P1y+800,'BS1');

text(P2x+800,P2y+800,'BS2');

text(P3x+800,P3y+800,'BS3');

xlabel('X axis');

ylabel('Y axis');

axis([-3*LL/20,3*LL,-3*LL/20,3*LL]);

grid on;

%这里随机生成移动物体的位置坐标

P0x = 2*LL/3;

P0y = 3*LL/4;

plot(P0x,P0y,'ro','Markersize',6);hold off;

text(P0x+800,P0y+800,'M');

title('基站位置');

 

 

%定义发送信号，这里发送信号根据要求使用AM发送信号

fc     = 1000;   %载波频率

tau    = 10^(-7);%时间间隔

T      = 1/fc/2; %频率周期

t      = -T*SL+tau:tau:T*SL;%信号的长度，这里使用8个周期进行

No     = length(t);       %信号的长度

A      = 1;%发送信号的幅度

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); %定义延迟

r      = zeros(N,No);%定义时延

for i = 1:N

    tDelay(i) = R0(i)/c;

    %计算得到实际的延迟后的信号

    r(i,:)    = A*cos(2*pi*fc*(t-tDelay(i)));

end

 

 

rng(1);%use matlab2013b else maybe error

%信道模拟

for j = 1:length(SNR)

    %加入噪声

    r2(1,:) = awgn(r(1,:),SNR(j));

    r2(2,:) = awgn(r(2,:),SNR(j));

    r2(3,:) = awgn(r(3,:),SNR(j));

    

    %加入多径 

    for jj = 1:N

        signals  = r2(jj,:);

        if Mpd == 0

           signals2 = signals;

        else

           signals2 = signals + 0.65*[zeros(1,Mpd),signals(1:end-Mpd)];   

        end

        r3(jj,:) = signals2;

    end

    %计算延迟相关运算

    Peak         = zeros(N,1);%定义相关峰的值

    delay_theory = zeros(N,1);%通过广义相关运算得到的延迟估计值

    for kk = 1:1:LL/10

        tau_theory = tau*kk;

        temp       = r3(1,(No/2-kk+1):(No-kk))*tau;

        for i =2:N

            data   = (sum(r3(i,No/2+1:No).*temp))^2;

            if Peak(i) < data

                Peak(i) = data;

                delay_theory(i) = tau_theory;

            end

        end

    end

 

    %通过TDOA方法，根据理论估计延迟得到实际的坐标点位置

    R_theory = zeros(1,N);

    Kj       = zeros(1,N);

    for i = 2:N

        R_theory(i) = delay_theory(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'*H_tdoa) *(H_tdoa')*C_tdoa;

    Q_tdoa = inv(H_tdoa'*H_tdoa) *(H_tdoa')*D_tdoa;

    %计算得到abc三个参数

    as     = sum(K_tdoa.^2) -1;

    bs     = sum(K_tdoa.*Q_tdoa);

    cs     = sum(Q_tdoa.^2);

    R1     = (-bs - sqrt(bs^2-as*cs))/as;

    xyChan = K_tdoa.*R1 + Q_tdoa;

    %计算估计得到的XY坐标点

    X_theory(j) = xyChan(1);

    Y_theory(j) = xyChan(2);

    %计算估计误差值

    Err(j) = 100*sqrt((X_theory(j)-P0x)^2+(Y_theory(j)-P0y)^2)/((X_theory(j)+Y_theory(j))/2);

     

    clear xyChan R1 cs bs as Q_tdoa K_tdoa D_tdoa C_tdoa H_tdoa Kj R_theory delay_theory Peak tau_theory temp

end

 

figure;

plot(SNR,Err,'b-*');

grid on;

title('误差曲线 % ');

xlabel('SNR');

ylabel('Err');

axis([10,80,0,5]);

 

%显示理论估计值

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([-3*LL/20,3*LL,-3*LL/20,3*LL]);

grid on;

xlabel('X axis');

ylabel('Y axis');

text(P1x+800,P1y+800,'BS1');

text(P2x+800,P2y+800,'BS2');

text(P3x+800,P3y+800,'BS3');

plot(P0x,P0y,'bo','Markersize',8);

hold on;

text(P0x-800,P0y-800,'M');

 

SEL = 1;

 

plot(X_theory(SEL),Y_theory(SEL),'rs','Markersize',8);hold on;

text(X_theory(SEL)+800,Y_theory(SEL)+800,'estimation');

 

R1 = sqrt((X_theory(SEL)-P1x)^2+(Y_theory(SEL)-P1y)^2); 

R2 = sqrt((X_theory(SEL)-P2x)^2+(Y_theory(SEL)-P2y)^2); 

R3 = sqrt((X_theory(SEL)-P3x)^2+(Y_theory(SEL)-P3y)^2);

 

alpha=0.01*pi:pi/100:0.55*pi;                     

x1= P1x+R1*cos(alpha); 

y1= P1y+R1*sin(alpha); 

hold on;

plot(x1,y1,'k-'); 

 

alpha=0.5*pi:pi/100:1.0*pi;                    

x2= P2x+R2*cos(alpha); 

y2= P2y+R2*sin(alpha); 

hold on;

plot(x2,y2,'k-'); 

 

alpha=1.1*pi:pi/100:1.95*pi;                    

x3= P3x+R3*cos(alpha); 

y3= P3y+R3*sin(alpha); 

hold on;

plot(x3,y3,'k-');