function [pso F] = pso_2D_new()

clc;
clear;close all;
N = 10;          %   N 種群大小
D = 38;          %   D 粒子維數(shù),                                                                      ** =n-D
r=5;             %   移動(dòng)錨節(jié)點(diǎn)的發(fā)射半徑，單位為米
L=20;            %   監(jiān)測(cè)區(qū)域?qū)捙c高，單位為米
S=30;            %   未知節(jié)點(diǎn)個(gè)數(shù)
M = [1 3;4 7;3 10;7 2;9 8;3 16;14 17;16 13;20 10;17 18;
     5 9;6 6;3 5;8 17;4 20;5 18;7 14;10 12;11 14;13 15;
     15 14;16 20;13 8;14 11;12 5;15 3;18 7;16 9;11 10;8 11];
max_gen = 100;          %   max_gen 最大迭代次數(shù)
%region=zeros(D,2);  %   設(shè)定搜索空間范圍
region=[0,20;0,20;0,20;0,20;
    0,20;0,20;0,20;0,20;
    0,20;0,20;0,20;0,20;
    0,20;0,20;0,20;0,20;
    0,20;0,20;0,20;0,20;
    0,20;0,20;0,20;0,20;
    0,20;0,20;0,20;0,20;
    0,20;0,20;0,20;0,20;
    0,20;0,20;0,20;0,20;
    0,20;0,20];          %                                                                                      **每一維設(shè)定不同范圍


rand('state',sum(100*clock));   %   重置隨機(jī)數(shù)發(fā)生器狀態(tài)
pop = ini_pos(N,D);   %   present 當(dāng)前位置,隨機(jī)初始化,rand()的范圍為0~1

v=ini_v(N,D);             %   初始化當(dāng)前速度

gbest = zeros(1,D);            %   gbest 當(dāng)前搜索到的最小的值
pbest = zeros(N,D);      %   pbest 粒子以前搜索到的最優(yōu)值，最后一列包括這些值的適應(yīng)度
w_max = 0.9;                            %   w_max 權(quán)系數(shù)最大值
w_min = 0.4;
v_max = 6;             %                                                                                          **最大速度,為粒子的范圍寬度
c1 = 2;                   %   學(xué)習(xí)因子
c2 = 2;                   %   學(xué)習(xí)因子
best_record = zeros(1,max_gen);     %   best_record記錄發(fā)生max_gen次迭代得到的最好的粒子的適應(yīng)度。
%  ————————————————————————
%   計(jì)算原始種群的適應(yīng)度,及初始化
%  ————————————————————————
for j=1:N
    f_pop(j)=fitness(pop(j,:),D,r,L);     %將N個(gè)粒子的適應(yīng)值賦給f_pop
end
%pbest = pop;                                        %初始化各個(gè)粒子最優(yōu)值

f_pbest=f_pop;                %f_pbest各個(gè)粒子的個(gè)體極值(最好適應(yīng)值)
[best_value best_index] = min(f_pbest);         %初始化全局最優(yōu)，即適應(yīng)度為全局最小的值，根據(jù)需要也可以選取為最大值
gbest = pop(best_index,:);    %最小適應(yīng)值所在行的所有列賦給gbest，
f_gbest=f_pbest(best_index);  %f_gbest為全局最優(yōu)適應(yīng)值

x=[0:0.01:20];
y=[0:0.01:20];
%z=@(x,y) (1-x).^2 - (y+1).^2+(1+x).^2-(y+2).^2 ;
%z=@(x,y) (3+x).^2 - (y+1).^2 ;
z=@(x,y) sum(x-y)^2;
h=waitbar(0,'Please wait...');
for i=1:max_gen
    grid on;
    ezmesh(z),hold on,grid on,plot3(pop(:,1),pop(:,2),pop(:,3),pop(:,4),pop(:,5),pop(:,6),pop(:,7),pop(:,8),pop(:,9),pop(:,10),pop(:,11),pop(:,12), pop(:,13),pop(:,14),pop(:,15),pop(:,16),pop(:,17),pop(:,18),pop(:,19),pop(:,20),pop(:,21),pop(:,22),pop(:,23),pop(:,24),pop(:,25),pop(:,26),pop(:,27),pop(:,28),pop(:,29),pop(:,30),pop(:,31),pop(:,32),pop(:,33),pop(:,34),pop(:,35),pop(:,36),pop(:,37),pop(:,38),f_pop','r*'),hold off;
    %ezmesh(z),hold on,grid on,plot3(pop(:,1),pop(:,2),f_pop','r*'),hold off;
    F(i)=getframe;
    
    %   確定是否對(duì)打散已經(jīng)收斂的粒子群——————————————————————————————
    reset = 0;          %   reset = 1時(shí)設(shè)置為粒子群過(guò)分收斂時(shí)將其打散，如果＝1則不打散
    if reset==1
        bit = 1;
        for k=1:D
            bit = bit&(range(pop(:,k))<0.1);
        end
        if bit==1       %   bit=1時(shí)對(duì)粒子位置及速度進(jìn)行隨機(jī)重置
            pop = ini_pos(N,D);   %   present 當(dāng)前位置,隨機(jī)初始化
            v = ini_v(N,D);           %   速度初始化
            for j=1:N
                f_pop(j)=fitness(pop(j,:),D,r,L);
            end
            warning('粒子過(guò)分集中！重新初始化……');      %   給出信息
            display(i);
        end
    end

    w = w_max-(w_max-w_min)*i/max_gen;
    %根據(jù)基本粒子群迭代公式進(jìn)行一次進(jìn)化迭代產(chǎn)生中間子代pop(j,1:D)
    for j=1:N
        v(j,:) = w.*v(j,:)+c1.*rand.*(pbest(j,1:D)-pop(j,1:D))...
            +c2.*rand.*(gbest(1:D)-pop(j,1:D));                        %  粒子速度更新 (a)

        %   判斷v的大小，限制v的絕對(duì)值小于5————————————————————————————
       % c = find(abs(v)>v_max);                                                                                              %**最大速度設(shè)置，粒子的范圍寬度
       % v(c) = sign(v(c))*v_max;   %如果速度大于6則，速度為6

        pop(j,1:D) = pop(j,1:D)+v(j,1:D);              %  粒子位置更新 (b)
        f_pop(j) = fitness(pop(j,1:D),D,r,L);
        if violent(M,pop(j,1:D),S,D,r)>=3
           if (f_pop(j)<f_pbest(j))&(Region_in(pop(j,:),region))     %   根據(jù)條件更新pbest,如果是最小的值為小于號(hào),相反則為大于號(hào)
             pbest(j,:) = pop(j,:);
             f_pbest(j)=f_pop(j);
           end
        end

    end

    [best_value best_index] = min(f_pbest);                                      %   如果是最小的值為min,相反則為max
    
    if (best_value<f_gbest)&(Region_in(pop(best_index,:),region))                  %   如果當(dāng)前最好的結(jié)果比以前的好，則更新最優(yōu)值gbest,如果是最小的值為小于號(hào),相反則為大于號(hào)
        gbest = pop(best_index,:);
        f_gbest=f_pbest(best_index);
    end

    best_record(i)=f_gbest;
    waitbar(i/max_gen);
end
close(h);
pso = gbest;
display(f_gbest);
figure;plot(gbest(1:2:end-1),gbest(2:2:end),'go'); 


%名稱：粒子違反約束程度函數(shù)
%輸出：粒子違反約束條件的程度（若違反輸出大于，不違反輸出等于零）
%形式：保存為M文件被調(diào)用

%約束函數(shù)
function  number=violent(M,present,S,D,r) 
for k=1:S
    number=0;
    for t=1:2:D-1
      if(M(k,1)-present(t))^2+(M(k,2)-present(t+1))^2<=r^2
          number=number+1;
      end
    end
end  


%   ***************************************************************************
%      計(jì)算適應(yīng)度
%   ***************************************************************************
function fit = fitness(present,D,r,L)
%fit=(3+present(:,1)).^2 - (present(:,2)+1).^2 +(3+present(:,3)).^2-(present(:,4)+1).^2
   % +(3+present(:,5)).^2 - (present(:,6)+1).^2 +(3+present(:,7)).^2-(present(:,8)+1).^2
   % +(3+present(:,9)).^2 - (present(:,10)+1).^2 +(3+present(:,11)).^2-(present(:,12)+1).^2
   % +(3+present(:,13)).^2 - (present(:,14)+1).^2 +(3+present(:,15)).^2-(present(:,16)+1).^2
   % +(3+present(:,17)).^2 - (present(:,18)+1).^2 +(3+present(:,19)).^2-(present(:,20)+1).^2
   % +(3+present(:,21)).^2 - (present(:,22)+1).^2 +(3+present(:,23)).^2-(present(:,24)+1).^2
    %+(3+present(:,25)).^2 - (present(:,26)+1).^2 +(3+present(:,27)).^2-(present(:,28)+1).^2
    %+(3+present(:,29)).^2 - (present(:,30)+1).^2 +(3+present(:,31)).^2-(present(:,32)+1).^2
    %+(3+present(:,33)).^2 - (present(:,34)+1).^2 +(3+present(:,35)).^2-(present(:,36)+1).^2
    %+(3+present(:,37)).^2 - (present(:,38)+1).^2 ;
    
    
%適應(yīng)函數(shù)    
for j=1:D/2-1
    count=0;
    fit=0;
    average=pi*r*r*D/2*L*L;
      for i=j+1:D/2
        if (present(j)-present(2*i-1))^2+(present(j+1)-present(2*i))^2<=r^2
           count=count+1;
        end
      end
      fit=fit+(count-average)^2;
end



function ini_position=ini_pos(N,D)
ini_position = 20*rand(N,D);        %初始化當(dāng)前粒子位置,使其隨機(jī)的分布在工作空間                         %** 6即為自變量范圍


function ini_velocity=ini_v(N,D)
ini_velocity =3/2*(rand(N,D));        %初始化當(dāng)前粒子速度,使其隨機(jī)的分布在速度范圍內(nèi)


function flag=Region_in(present,region)
flag=1;
len=length(present);
for k=1:len
    flag=flag&(present(k)>=region(k,1))&(present(k)<=region(k,2));
end