%  PSO算法
% 仿真一個串級系統，并計算出絕對誤差矩積分，用PSO算法對PI控制器尋優
clc
close all;
clear all;

%隨機初始化
lp=50;         %循環代數

%初始化各個粒子
x=rand(10,2);  
[n,m]=size(x);

 %初始化各個粒子的第一步前進的速度
v=rand(size(x));
vmax=[1 1];
xmax=[5 5 ];
for i=1:n
    y(i)=PsoPidForObj(  x(i,1), x(i,2)  );     %計算原始粒子的目標值 
    py=0;
end

p=x; %原始粒子的歷史最優位置既是本身
ymin=inf;                   %原始粒子中的最優值
k=0;
for i=1:n
    if(  ( ymin )  >  y(i)  )
        ymin=y(i);
        k=i;
    end
end
g=x(k,:);                     %原始粒子中的最優值

   
c1=1.5;                     %常數
c2=1.5;
y0=y;                     %保留各個粒子目標值的前一時刻值
gy=inf;

for k=1:lp
     k
    %慣性因子
    %w=1-0.8*k/lp;
    w=0.73;
    % 個體最優值
    for i=1:n
        if(y0(i)>=y(i))
            for j=1:m
                p(i,j)=x(i,j);
            end
            py(i)=y(i);
        end 
    end
    y0=y ;
    
    % 全局最優值
    for i=1:n 
        if(  (gy )      >    (   py(i)   )      ) 
            g=p(i,:);
            gy=py(i) ;
        end
    end
     
    %計算個體前進速度，并限制最大值
    for i=1:n
        for j=1:m
            v(i,j)=w*v(i,j)+c1*rand(1)*( p(i,j)-x(i,j) )+c2*rand(1)*( g(j)-x(i,j) );         
        end
        %速度最大值限制
        for j=1:m
            if (  v(i,j)>vmax(j)  )
                v(i,j)=vmax(j);
            end
            if (  v(i,j)<-vmax(j)  )
                v(i,j)=-vmax(j);
            end
        end        
        %粒子前進一步
        for j=1:m
            x(i,j)=x(i,j)+v(i,j);      
        end
        %粒子活動范圍限制
        for j=1:m
            if (  x(i,j)>xmax(j)  )
                x(i,j)=xmax(j);
            end
            if (  x(i,j)<=0  )
                x(i,j)=0.001;
            end
        end         
    
        %當前粒子的當前目標函數值        
        y(i)=PsoPidForObj(  x(i,1), x(i,2)  );           
    end
    best(k)=gy;
    
    if (gy<=400)
        break;
    end
end

g
gy
figure(1);
plot(best);
PsoPidForOut(  g(1),g(2) );