function [x4,y4,x5,y5,x6,y6]=QPSO(x1,y1,x2,y2,x3,y3,H,tao)
%量子粒子群
%三个水听器位置(x4,y4,0)(x5,y5,0)(x6,y6,0)
%%第一个船（x1,y1,H）
%s1=sqrt((x4-x1)^2+(y4-y1)^2+H^2);
%s2=sqrt((x5-x1)^2+(y5-y1)^2+H^2);
%s3=sqrt((x6-x1)^2+(y6-y1)^2+H^2);
%tao1=(s1-s2)/c;tao2=(s1-s3)/c;
%%第二个船（x2,y2,H）
%ss1=sqrt((x4-x2)^2+(y4-y2)^2+H^2);
%ss2=sqrt((x5-x2)^2+(y5-y2)^2+H^2);
%ss3=sqrt((x6-x2)^2+(y6-y2)^2+H^2);
%tao3=(ss1-ss2)/c;tao4=(ss1-ss3)/c;
%%第三个船（x3,y3,H）
%sss1=sqrt((x4-x3)^2+(y4-y3)^2+H^2);
%sss2=sqrt((x5-x3)^2+(y5-y3)^2+H^2);
%sss3=sqrt((x6-x3)^2+(y6-y3)^2+H^2);
%tao5=(sss1-sss2)/c;tao6=(sss1-sss3);
%tao=[tao1 tao2 tao3 tao4 tao5 tao6];
%%/////QPSO/////
%初始化
c=1500;%声速
pnum=2000;
pdim=6;
iter=2000;
p=100*rand(pnum,pdim)-50;%(-50,50)粒子随机初始位置
%适应度值计算
f1=(sqrt((p(:,1)-x1).^2+(p(:,2)-y1).^2+H^2)-sqrt((p(:,3)-x1).^2+(p(:,4)-y1).^2+H^2)-tao(1,1)*c).^2;
f2=(sqrt((p(:,1)-x1).^2+(p(:,2)-y1).^2+H^2)-sqrt((p(:,5)-x1).^2+(p(:,6)-y1).^2+H^2)-tao(1,2)*c).^2;
f3=(sqrt((p(:,1)-x2).^2+(p(:,2)-y2).^2+H^2)-sqrt((p(:,3)-x2).^2+(p(:,4)-y2).^2+H^2)-tao(1,3)*c).^2;
f4=(sqrt((p(:,1)-x2).^2+(p(:,2)-y2).^2+H^2)-sqrt((p(:,5)-x2).^2+(p(:,6)-y2).^2+H^2)-tao(1,4)*c).^2;
f5=(sqrt((p(:,1)-x3).^2+(p(:,2)-y3).^2+H^2)-sqrt((p(:,3)-x3).^2+(p(:,4)-y3).^2+H^2)-tao(1,5)*c).^2;
f6=(sqrt((p(:,1)-x3).^2+(p(:,2)-y3).^2+H^2)-sqrt((p(:,3)-x3).^2+(p(:,4)-y3).^2+H^2)-tao(1,6)*c).^2;
f=f1+f2+f3+f4+f5+f6;
pbestp=p;%粒子个体最优值
pbestf=f;%粒子个体适应度值
[gbestf,I]=min(f);%群体适应度值
gbestp=p(I,:);%群体最优值
%记录群体最优值
Best=zeros(iter+1,pdim+1);
Best(1,1)=gbestf;
Best(1,2:pdim+1)=gbestp;
for i=1:iter
mbest=mean(pbestp);%粒子群平均最优位置
a=rand(pnum,1);
pp=[a a a a a a].*pbestp+(1-a)*gbestp;
u=rand;
beita=1.2-0.8*i/iter;
if u>=0.5
p=pp-beita*abs(ones(pnum,1)*mbest-p)*log(1/u);
else
p=pp+beita*abs(ones(pnum,1)*mbest-p)*log(1/u);
end
%适应度值计算
f1=(sqrt((p(:,1)-x1).^2+(p(:,2)-y1).^2+H^2)-sqrt((p(:,3)-x1).^2+(p(:,4)-y1).^2+H^2)-tao(1,1)*c).^2;
f2=(sqrt((p(:,1)-x1).^2+(p(:,2)-y1).^2+H^2)-sqrt((p(:,5)-x1).^2+(p(:,6)-y1).^2+H^2)-tao(1,2)*c).^2;
f3=(sqrt((p(:,1)-x2).^2+(p(:,2)-y2).^2+H^2)-sqrt((p(:,3)-x2).^2+(p(:,4)-y2).^2+H^2)-tao(1,3)*c).^2;
f4=(sqrt((p(:,1)-x2).^2+(p(:,2)-y2).^2+H^2)-sqrt((p(:,5)-x2).^2+(p(:,6)-y2).^2+H^2)-tao(1,4)*c).^2;
f5=(sqrt((p(:,1)-x3).^2+(p(:,2)-y3).^2+H^2)-sqrt((p(:,3)-x3).^2+(p(:,4)-y3).^2+H^2)-tao(1,5)*c).^2;
f6=(sqrt((p(:,1)-x3).^2+(p(:,2)-y3).^2+H^2)-sqrt((p(:,5)-x3).^2+(p(:,6)-y3).^2+H^2)-tao(1,6)*c).^2;
f=f1+f2+f3+f4+f5+f6;
for n=1:pnum
if f(n,1)<pbestf(n,1)
pbestp(n,:)=p(n,:);
pbestf(n,1)=f(n,1);
end
if f(n,1)<gbestf
gbestf=f(n,1);
gbestp=p(n,:);
end
end
Best(i+1,1)=gbestf;
Best(i+1,2:pdim+1)=gbestp;
end
x4=Best(end,2);
y4=Best(end,3);
x5=Best(end,4);
y5=Best(end,5);
x6=Best(end,6);
y6=Best(end,7);
end
