clc;
clear all;
close all;
N=5;%%%%%阵元个数为5;
c=3e8;
d1=0.1*0.3048;%%%%%每个阵元之间的距离为10英尺;
x1_0=-45000*0.3048;%%%%%目标1的初始位置为-45000英尺;
x2_0=-15000*0.3048;%%%%%目标2的初始位置为-15000英尺;
x3_0=45000*0.3048;%%%%%目标3的初始位置是45000英尺;
v1=1500*0.3048;%%%%%%目标1的速度为300ft/s;
v2=1000*0.3048;%%%%%%目标2的速度为300ft/s;
v3=300*0.3048;%%%%%目标3的速度为-1500ft/s；
h=30000*0.3048;%%%%%%目标的高度为30000英尺;
f0=9e9;%%射频为9GHz
f0=3e8/(2*d1);
sigama1=1;
sigama2=1;
sigama3=1;
sigama_noise=0.3;
beta=0.97;%%%遗忘因子
%%%%%2.每个阵元得到的信号%%%%%%
t=0:0.01:30;%%%%时间
x1=x1_0+v1*t;%%%%目标1的轨迹
x2=x2_0+v2*t;%%%%目标2的轨迹
x3=x3_0+v3*t;%%%%目标3的轨迹
xita1=atan(x1/h);%%%计算目标1的真实角度
xita2=atan(x2/h);%%%计算目标2的真实角度
xita3=atan(x3/h);%%%计算目标3的真实角度
figure(1)
plot(t,xita1*180/pi);grid on;hold on;
plot(t,xita2*180/pi);hold on;
plot(t,xita3*180/pi);
fc=1;
s1=sigama1*cos(2*pi*fc*t);%%%%%目标1
s2=sigama2*cos(2*pi*fc*t);%%%%%目标2
s3=sigama3*cos(2*pi*fc*t);%%%%%目标3
%%%%%%%初始化W和d%%%%%%%%
N=5;
I=eye(5);
w=zeros(N,3*3000);
x1=zeros(N,3*3000);
w(:,1)=[1 0 0 0 0].';
w(:,2)=[0 1 0 0 0].';
w(:,3)=[0 0 1 0 0].';
d(1)=1;
d(2)=1;
d(3)=1;
n1=sigama_noise*(randn(1,3000)+j*randn(1,3000));%%%阵元1噪声的功率为0.1^2;
n2=sigama_noise*(randn(1,3000)+j*randn(1,3000));%%%阵元2噪声的功率为0.1^2;
n3=sigama_noise*(randn(1,3000)+j*randn(1,3000));%%%阵元3噪声的功率为0.1^2;
n4=sigama_noise*(randn(1,3000)+j*randn(1,3000));%%%阵元4噪声的功率为0.1^2;
n5=sigama_noise*(randn(1,3000)+j*randn(1,3000));%%%阵元5噪声的功率为0.1^2;
for k=1:3000
k
% tao12=(d1/c)*sin(xita1(k));%%%%%%相对于第一个目标,两个子阵之间的时延
% tao22=(d1/c)*sin(xita2(k));%%%%%%相对于第二个目标.两个子阵之间的时延
% tao32=(d1/c)*sin(xita3(k));%%%%%%相对于第二个目标.两个子阵之间的时延
phase12=exp(-j*pi*sin(xita1(k)));%%%%相对于第一个目标,两个子阵之间的时延所造成的相位差
phase22=exp(-j*pi*sin(xita2(k)));%%%%相对于第二个目标,两个子阵之间的时延所造成的相位差
phase32=exp(-j*pi*sin(xita3(k)));%%%%相对于第二个目标,两个子阵之间的时延所造成的相位差
A=[1 1 1;phase12 phase22 phase32;phase12^2 phase22^2 phase32^2;phase12^3 phase22^3 phase32^3;phase12^4 phase22^4 phase32^4];
s1=sigama1*cos(100*t);%%%%%目标1
s2=sigama2*cos(200*t);%%%%%目标2
s3=sigama3*cos(300*t);%%%%%目标3
x=A*[s1(k) s2(k) s3(k)].'+[n1(k) n2(k) n3(k) n4(k) n5(k)].';%%%%%%每个时刻阵元接收到的数据
x1(:,k*3+1)=x;
for k1=1:3%%%%%%%%pastd算法，得到信号子空间
y(k*3+k1)=w(:,(k-1)*3+k1).'*x1(:,k*3+k1);
d(k*3+k1)=beta*d((k-1)*3+k1)+(abs(y(k*3+k1))).^2;
e(:,k*3+k1)=x1(:,k*3+k1)-w(:,(k-1)*3+k1)*y(k*3+k1);
w(:,k*3+k1)=w(:,(k-1)*3+k1)+e(:,k*3+k1)*[conj(y(k*3+k1))/d(k*3+k1)];
x1(:,k*3+k1+1)=x1(:,k*3+k1)-w(:,k*3+k1)*y(k*3+k1);
end
V=[w(:,k*3+1) w(:,k*3+2) w(:,k*3+3)];
V=orth(V);
% %%%%%%%MUSIC%%%%%%%%
% n=0;
% for angle=-60:0.1:60
% a=[1 exp(-j*pi*sin(angle*pi/180)) exp(-j*2*pi*sin(angle*pi/180)) exp(-j*3*pi*sin(angle*pi/180)) exp(-j*4*pi*sin(angle*pi/180))].';
% n=n+1;
% P(n)=1/(a'*(I-V*V')*a);
% end
% [c1,I1]=max(abs(P(1:200)));
% [c2,I2]=max(abs(P(200:600)));
% [c3,I3]=max(abs(P(1000:1200)));
% xita1_estimated(k)=-60+(I1-1)*0.1;
% xita2_estimated(k)=-40.1+(I2-1)*0.1;
% xita3_estimated(k)=39.9+(I3-1)*0.1;
%%%%%%%%%ESPRIT%%%%%%%%%%%%%
U1=V(1:4,:);%%%%%把子空间的前N-1行提取出来
U2=V(2:5,:);%%%5%把子空间的后N-1行提取出来
P=inv((U1'*U1))*U1'*U2;
dd=eig(P);
angle=log(dd)*(j);
angle1=asin(angle/pi);
xita1_estimated(k)=real(angle1(1))*180/pi;
xita2_estimated(k)=real(angle1(2))*180/pi;
xita3_estimated(k)=real(angle1(3))*180/pi;
end
%%%%%%%%%%关联算法%%%%%%%%%%
xita1_estimated_(1)=xita1(1)*180/pi;
xita2_estimated_(1)=xita2(1)*180/pi;
xita3_estimated_(1)=xita3(1)*180/pi;
xita1_estimated_(2)=xita1(2)*180/pi;
xita2_estimated_(2)=xita2(2)*180/pi;
xita3_estimated_(2)=xita3(2)*180/pi;
for kk=3:3000
% xita1_predicted(kk)=xita1_estimated_(kk-1)+[xita1_estimated_(kk-1)-xi
% ta1_estimated_(kk-2)];
%%%%%xita1关联%%%%%%%%%%%%%%%%
xita1_predicted(kk)=xita1(kk)*180/pi;%%%可在此加入Kalman滤波作为预测值
dis1=abs(xita1_estimated(kk)-xita1_predicted(kk));%xita1估计与xita1预测相比
dis2=abs(xita2_estimated(kk)-xita1_predicted(kk));%xita2估计与xita1预测相比
dis3=abs(xita3_estimated(kk)-xita1_predicted(kk));%xita3估计与xita1预测相比
if (dis1<=dis2)&(dis1<=dis3)%如果与xita1相差最小，则=xita1估计
xita1_estimated_(kk)=xita1_estimated(kk);
end
if (dis2<=dis1)&(dis2<=dis3)%如果xita2误差最小，则=xita2估计
xita1_estimated_(kk)=xita2_estimated(kk);
end
if (dis3<=dis1)&(dis3<=dis2)%如果xita3误差最小，则=xita3估计
xita1_estimated_(kk)=xita3_estimated(kk);
end
% xita2_predicted(kk)=xita2_estimated_(kk-1)+[xita2_estimated_(kk-1)-xita2_estimated_(kk-2)];
%%%%%xita2关联%%%%%%%%%%%%%%%%
xita2_predicted(kk)=xita2(kk)*180/pi;
dis1=abs(xita1_estimated(kk)-xita2_predicted(kk));
dis2=abs(xita2_estimated(kk)-xita2_predicted(kk));
dis3=abs(xita3_estimated(kk)-xita2_predicted(kk));
if (dis1<=dis2)&(dis1<=dis3)
xita2_estimated_(kk)=xita1_estimated(kk);
end
if (dis2<=dis1)&(dis2<=dis3)
xita2_estimated_(kk)=xita2_estimated(kk);
end
if (dis3<=dis1)&(dis3<=dis2)
xita2_estimated_(kk)=xita3_estimated(kk);
end
%%%%%xita3关联%%%%%%%%%%%%%%%%
% xita3_predicted(kk)=xita3_estimated_(kk-1)+[xita3_estimated_(kk-1)-xita3_estimated_(kk-2)];
xita3_predicted(kk)=xita3(kk)*180/pi;
dis1=abs(xita1_estimated(kk)-xita3_predicted(kk));
dis2=abs(xita2_estimated(kk)-xita3_predicted(kk));
dis3=abs(xita3_estimated(kk)-xita3_predicted(kk));
if (dis1<=dis2)&(dis1<=dis3)
xita3_estimated_(kk)=xita1_estimated(kk);
end
if (dis2<=dis1)&(dis2<=dis3)
xita3_estimated_(kk)=xita2_estimated(kk);
end
if (dis3<=dis1)&(dis3<=dis2)
xita3_estimated_(kk)=xita3_estimated(kk);
end
end
xita1_estimated_pastd_jiaohui=xita1_estimated_;
xita2_estimated_pastd_jiaohui=xita2_estimated_;
xita3_estimated_pastd_jiaohui=xita3_estimated_;
figure(2)
plot(xita1(3:3000)*180/pi,'k');grid on;hold on;
plot(xita1_estimated_(3:3000),'k:');hold on;
plot(xita2(3:3000)*180/pi,'r');grid on;hold on;
plot(xita2_estimated_(3:3000),'r:')
plot(xita3(3:3000)*180/pi,'b');grid on;hold on;
plot(xita3_estimated_(3:3000),'b:')
figure(3)
plot(abs(xita1(3:3000)*180/pi-xita1_estimated_(3:3000)),'k');grid on;hold on;
plot(abs(xita2(3:3000)*180/pi-xita2_estimated_(3:3000)),'r');grid on;
plot(abs(xita3(3:3000)*180/pi-xita3_estimated_(3:3000)),'b');grid on;
% plot(sqrt((xita1(3:3000)*180/pi-xita1_estimated_(3:3000)).^2+(xita2(3:3000)*180/pi-xita2_estimated_(3:3000)).^2+(xita3(3:3000)*180/pi-xita3_estimated_(3:3000).^2)),'k');
% save('xita1_estimated_pastd_jiaohui','xita1_estimated_pastd_jiaohui');
% save('xita2_estimated_pastd_jiaohui','xita2_estimated_pastd_jiaohui');
% save('xita3_estimated_pastd_jiaohui','xita3_estimated_pastd_jiaohui');
%
pastd.rar （1个子文件） 
pastd.m 7KB
我虽横行却不霸道
- 粉丝: 90
- 资源: 1万+
下载权益
C知道特权
VIP文章
课程特权
VIP享7折，此内容立减3.57元 开通VIP
评论2