%-------------------------------PSO-----------------------------------%
%                          粒子群優化算法                             %
%---------------------------------------------------------------------%
A=[4.7	3.8	 3.0 2.0 0.2
   1.1	2.2	 3.9 4.6 1.7
   3.0	0.1  4.6 4.5 4.0
   2.4	4.1	 3.6 2.0 0.0
   4.4	2.2	 0.8 4.4 0.6
];
Y=[41.8
   47.0
   54.7
   41.0
   32.3
];
nn=10;                                    %粒子群規模，即粒子數
m=5;                                      %粒子的維數，即尋優參數的個數
X_pso=rand(nn,m);                         %初始化粒子群的位置
V=rand(nn,m);                             %初始化粒子群的速度
Pbest=X_pso;                              %粒子群的個體最優解
Gbest=rand(1,m);                          %粒子群的全體最優解
Wmax=0.9;
Wmin=0.1; 
ITERmax=1000;                                      %進化代數
iter=1;
C1=2.05;                                           %學習因子
C2=2.05;                                           %學習因子
K=2/abs(2-(C1+C2)-sqrt((C1+C2)^2-4*(C1+C2)));      %壓縮因子
a=1 ;                                              %約束因子
Xmax=100;               
Vmax=10;                                  
while(iter<=ITERmax)
    for i=1:nn
        for j=1:m
            R1=rand;
            R2=rand;
            W_pso=Wmax-(Wmax-Wmin)/ITERmax*iter;   %慣性因子
            Vinter=K*(W_pso*V(i,j)+R1*C1*(Pbest(i,j)-X_pso(i,j))+R2*C2*(Gbest(1,j)-X_pso(i,j)));
            if Vinter>Vmax
                V(i,j)=Vmax;
            elseif Vinter<-Vmax
                V(i,j)=-Vmax;   
            else 
                V(i,j)=Vinter;
            end
            Xinter=X_pso(i,j)+a*V(i,j);
            if Xinter>Xmax
                X_pso(i,j)=Xmax;
            elseif Xinter<-Xmax
                X_pso(i,j)=-Xmax; 
            else
                X_pso(i,j)=Xinter;
            end
        end
    end
    for i=1:nn
        pres_X=fit_m(A,Y,X_pso(i,:));
        pres_Pbest=fit_m(A,Y,Pbest(i,:));
        pres_Gbest=fit_m(A,Y,Gbest(1,:));
        if pres_X<pres_Pbest
            Pbest(i,:)=X_pso(i,:);
        end
        if pres_X<pres_Gbest
            Gbest(1,:)=X_pso(i,:);
        end
    end
    err(iter)=pres_Gbest;
    if pres_Gbest<=10^(-30)
        iter=ITERmax;
    else
        iter=iter+1;
    end
end
figure(1)
ploterr(err/5)
Gbest'
A*Gbest'
err/5
figure(2)
plot(Gbest)
%------------------------------備注-----------------------------------%
%                                                                     %
%---------------------------------------------------------------------%