clear all;
close all;
clc;

[x y]=meshgrid(-100:100,-100:100);
sigma=50;
img = (1/(2*pi*sigma^2))*exp(-(x.^2+y.^2)/(2*sigma^2)); %目标函数，高斯函数
mesh(img);
hold on;
n=10;   %粒子群粒子个数

%初始化粒子群，定义结构体
%结构体中八个元素，分别是粒子坐标，粒子速度，粒子适应度，粒子最佳适应度，粒子最佳坐标
par=struct([]);　　　　　　　　　　
for i=1:n
    par(i).x=-100+200*rand();   %[-100 100]对x位置随机初始化
    par(i).y=-100+200*rand();   %[-100 100]对y位置随机初始化
    par(i).vx=-1+2*rand();      %[-1 1]对vx速度随机初始化
    par(i).vy=-1+2*rand();      %[-1 1]对vy速度随机初始化
    par(i).fit=0;               %粒子适应度为0初始化
    par(i).bestfit=0;           %粒子最佳适应度为0初始化
    par(i).bestx=par(i).x;      %粒子x最佳位置初始化
    par(i).besty=par(i).y;      %粒子y最佳位置初始化
end
par_best=par(1);    %初始化粒子群中最佳粒子

for k=1:10    
    plot3(par_best.x+100,par_best.y+100,par_best.fit,'g*'); %画出最佳粒子的位置，+100为相对偏移
    for p=1:n
        [par(p) par_best]=update_par(par(p),par_best);  %更新每个粒子信息         
    end  
end

function [par par_best]=update_par(par,par_best)
    
    %Px=Px+Pv*t,这里t=1,Px为当前粒子的位置，Pv为当前粒子的速度
    par.x=par.x+par.vx;   
    par.y=par.x+par.vy;   
    
    par.fit=compute_fit(par);    %计算当前粒子适应度
    
    %Pv=Pv+(c1*rand*(Gx-Px))+(c2*rand*(PBx-Px))
    %这里c1,c2为加速因子
    %Gx为具有最佳适应度粒子的位置
    %PBx为当前粒子的最佳位置
    c1=1;
    c2=1;
    par.vx=par.vx+c1*rand()*(par_best.x-par.x)+c2*rand()*(par.bestx-par.x);   
    par.vy=par.vy+c1*rand()*(par_best.y-par.y)+c2*rand()*(par.besty-par.y);
 
    if par.fit>par.bestfit      %如果当前粒子适应度要好于当前粒子最佳适应度
        par.bestfit=par.fit;    %则更新当前粒子最佳适应度
        par.bestx=par.x;        %更新当前粒子最佳位置
        par.besty=par.y;
        if par.bestfit>par_best.fit     %如果当前粒子最佳适应度好于最佳粒子适应度
            par_best.fit=par.bestfit;   %则更新最佳粒子适应度
            par_best.x=par.x;           %更新最佳粒子位置
            par_best.y=par.y;
        end
    end

end

function re=compute_fit(par)
    x=par.x;
    y=par.y;
    sigma=50;
    if x<-100 || x>100 || y<-100 || y>100
        re=0;        %超出范围适应度为0
    else            %否则适应度按目标函数求解
        re= (1/(2*pi*sigma^2))*exp(-(x.^2+y.^2)/(2*sigma^2)); 
    end
end