clear all;
clc;

fs=2500;N=2500;
t=0:1/fs:(N-1)/fs;
global y yd

% 傳感器的二階模型的階躍響應 2500點
num_cgq=[7318];
den_cgq=[1,127.1,7318];
sys_cgq=tf(num_cgq,den_cgq);
[y]=step(sys_cgq,t);
%是補償器輸入

%等效系統的階躍響應 2500點
num_lx=[18225 ]  ;den_lx=[ 1, 190.89,18225 ]; 
sys_lx=tf(num_lx,den_lx);
zpk(sys_lx);
[yd]=step(sys_lx,t);
%size(y_lx)
%yd(1:2500)是參考模型輸出

% 15行7列
popsize=25;  % 種群規模，即粒子數15個,
dimen=7;     % 維數，即要求7個參數    x=[b0,b1,b2,b3,a1,a2,a3]
L=1000;        % 最大迭代次數,         區別大小寫
error=10^(-6); %迭代終止的精度要求
%wmax=0.95;      % 最大慣性權重
%wmin=0.4;      % 最小慣性權重
w=1;
c1=2;
c2=1.5;   % 學習因子

% 隨機初始化種群的位置和速度
x=randn(popsize,dimen);

%%x(1,:)為局部最優
v=(rand(popsize,dimen)-0.5)*2;              % 隨機產生速度范圍[-1,1]
vmax=ones(1,dimen);
% 先計算每個粒子的適應度，并初始化個體最優Pbest和群體最優Pgbest
for i=1:popsize
    pbest_fit(i)=fitness(x(i,:),dimen);
    xpbest(i,:)=x(i,:);                %  初始化個體最優Pbest
end
                      
[gbest_fit index]=min(pbest_fit(i));      % 初始化群體最優Pgbest的初始值
temp_gbest_fit=gbest_fit;
xgbest=x(index,:);

%進入主循環，直到迭代結束
for diedai=1:L
    disp(['No.Loop : ' num2str(diedai) ]) ;
       for i=1:popsize
        for j=1:dimen
            v(i,j)=w*v(i,j)+c1*rand(1)*(xpbest(i,j)-x(i,j))+c2*rand(1)*(xgbest(j)-x(i,j));
            if abs(v(i,j))>vmax(j)             % 速度限制范圍
                v(i,j)=sign(v(i,j))*vmax(j);
            end
            x(i,j)=x(i,j)+v(i,j);
        end
        temp_x_fit=fitness(x(i,:),dimen);
        if temp_x_fit<pbest_fit(i)
            pbest_fit(i)=temp_x_fit;
            xpbest(i,:)=x(i,:);    %  第i個粒子個體最優更新
        end
        if pbest_fit(i)<temp_gbest_fit
            temp_gbest_fit=pbest_fit(i);
            xgbest=xpbest(i,:);    % 用i粒子的個體最優來更新群體最優
        end
    end
    gbest_fit(diedai)=temp_gbest_fit;      % 記錄第*次迭代得到的全局最好適應度函數
end

disp('全局最優位置為：')
xgbest'
disp('全局最優適應度函數值：')
gbest_fit(diedai)

%傳感器輸出的實驗數據,即y(1:2500)是補償器輸入
fp=fopen('liu50.txt','r');
out=fscanf(fp,'%f',2500);
fclose(fp);
for i=1:2500
    y_data(i)=out(i)-0.94404;
end
y_data=y_data';
[b,a]=butter(2,0.1);
y_data=filter(b,a,y_data);
%y(1:2500)是補償器輸入

%帶入實驗數據，看濾波后的效果
yc(1:3)=0;
for k=4:2500
    yc(k)=xgbest(1)*y_data(k)+xgbest(2)*y_data(k-1)+xgbest(3)*y_data(k-2)+xgbest(4)*y_data(k-3)-xgbest(5)*yc(k-1)-xgbest(6)*yc(k-2)-xgbest(7)*yc(k-3);
end
%[b2,a2]=butter(2,0.01);
%yc=filter(b2,a2,yc);
figure(1)
plot(t,yc,'k',t,y_data,'k')
xyh_plot
axis([0,1,-0.5,2.5]);
title('flux=50g/s')
annotation1 = annotation(figure(1),'line',[0.5335 0.492],[0.8127 0.7745]);
annotation1 = annotation(figure(1),'line',[0.5716 0.598],[0.7314 0.6644]);
annotation1 = annotation(...
  figure(1),'textbox',...
  'Position',[0.5902 0.5305 0.3396 0.1489],...
  'LineStyle','none',...
  'FontName','Times New Roman',...
  'FontSize',8,...
  'String',{'1'},...
  'FitHeightToText','on');
annotation1 = annotation(...
  figure(1),'textbox',...
  'Position',[0.4448 0.6426 0.3396 0.1489],...
  'LineStyle','none',...
  'FontName','Times New Roman',...
  'FontSize',8,...
  'String',{'2'},...
  'FitHeightToText','on');

figure(3)
plot(gbest_fit,'k') % 適應值的變化
xlabel('迭代次數(L)','fontname','Times New Roman','Fontsize',8);
ylabel('適應度函數(V)','fontname','Times New Roman','Fontsize',8);
set(gcf,'color',[1,1,1]);
set(gca,'xcolor',[0,0,0],'ycolor',[0,0,0]);
set(gcf,'units','centimeters','position',[5,10,6.8,5.2]);
set(gca,'box','on','fontname','Times New Roman','fontsize',8);