msf2df_sim.m

内模控制器（IMC）工具箱。包括参数整定、PID控制器参数转换等

function [d_through_p]=msf2df_sim(simulate,pnummt,pdenmt,pdeadmt,mnummt,mdenmt,mdeadmt,...
   pdnummt,pddenmt,cnummt,cdenmt,cfnummt,cfdenmt,order,orderf,x,y,Epsilon,...
   subs,slbs,m,qf,qd,Tcanc,d_through_p,cnums,cdens)

global time TimeScale st_size plot_setpoint plot_disturb plot_effort setpoint_Enable
global disturb_Enable effort_Enable plot_IMC plot_IMC_sat plot_MSF clflag clmap
global gh1 gh2 old_handle keep numPs denPs Pdelay numMs denMs Mdelay Md Setpoint
global Disturb denFs numFs numPd denPd Ksp K Mnt Qdnum Qdden DrN NrD

if length(Epsilon) == 1
 Epsilon(2)=Epsilon(1);
end
%Model
%-----------------------------------------------
[Ksp,K]=msfgain(cnums,cdens,Epsilon(2));
[numMs]=mt2poly(mnummt,y);
[denMs]=mt2poly(mdenmt,y);
[mdeadx,mnumT,mdenT]=mt2dt(mdeadmt,y);

%Process
%-----------------------------------------------
if isempty(pnummt) | isempty(pdenmt)
   numPs=numMs;
   denPs=denMs;
   pdeadx=mdeadx;
   pnumT=mnumT;
   pdenT=mdenT;
else
   [numPs]=mt2poly(pnummt,x);
	[denPs]=mt2poly(pdenmt,x);
	[pdeadx,pnumT,pdenT]=mt2dt(pdeadmt,x);
end

% inv(F(esetpoint)*F(eDis.) part----------
denF=[];
for i=1:order
 denF(i,1)=Epsilon(1);
 denF(i,2)=1;
end
[r,c]=size(denF);
denFs=denF(1,:);
for i=2:r
 denFs=conv(denFs,denF(i,:));
end
if Epsilon==0
 denFs=1;
end

denF=[];
for i=1:orderf
 denF(i,1)=Epsilon(2);
 denF(i,2)=1;
end
[r,c]=size(denF);
numFs=denF(1,:);
for i=2:r
 numFs=conv(numFs,denF(i,:));
end
if Epsilon==0
 numFs=1;
end

if orderf==0
	denFs2=1;
else
denF2=[];
for i=1:orderf
 denF2(i,1)=Epsilon(2);
 denF2(i,2)=1;
end
denFs2=denF2(1,:);
for i=2:orderf
 denFs2=conv(denFs2,denF2(i,:));
end
end
if Epsilon(2)==0
 denFs2=1;
end

% cdenmt*cfnummt/(cnummt*cfdenmt) fro converting to MSF control structure---
% -----------------------
DrN=conv(cdenmt,cfnummt);
NrD=conv(cnummt,cfdenmt);

%Controller
[denCs]=mt2poly(cnummt,y);
[numCs]=mt2poly(cdenmt,y);

% Feedback path controller q(e) 
[denCs2]=mt2poly(cfnummt,y);
[numCs2]=mt2poly(cfdenmt,y);

%Approximated feed-back form controller 
[mq]=mq_gen(mnummt,mdenmt,mdeadmt,cfnummt,cfdenmt,orderf,y);
[Csnum,Csden]=pid_tf2(y,numMs,denMs,mnumT,mdenT,denCs2,numCs2,...
 	denFs,denFs2,Tcanc,Epsilon,mq,orderf);
 
denCs=conv(denCs,denFs);
denCs2=conv(denCs2,denFs2);
 
% Disturbance lag------------------
numPd=mt2poly(pdnummt,x);
denPd=mt2poly(pddenmt,x);


% Qd part of the controller---------------
if strcmp(qd,'1')
   Qdnem=[1];
   Qdden=[1];
else
	[qd,Qdnum,Qdden]=qd_mat(Tcanc(2,:),mq,Epsilon(2),orderf,y);
end

if any(real(roots(denPs))>0) | any(real(roots(denMs))>0) | any(real(roots(denPd))>0)
	% Pre-filter part 
	Pre_fil_num=conv(numCs,denCs2);
	Pre_fil_num=conv(Pre_fil_num,Qdden);
	Pre_fil_den=conv(denCs,numCs2);
	Pre_fil_den=conv(Pre_fil_den,Qdnum);
	[Pre_fil_num,Pre_fil_den]=z_p_cancel(Pre_fil_num,Pre_fil_den,1);
   if d_through_p==0
      d_through_p=1;
      msgbox('The plant is unstable --> Disturbance pass through the plant','Warning');
   end
end

%Delays and Saturation bounds
%---------------------------------------------------------

Mdelay=mdeadx;
Pdelay=pdeadx;
Satub=str2num(subs);
Satlb=str2num(slbs);
dimn=length(numMs);
dimd=length(denMs);
numMs=numMs/denMs(dimd);
denMs=denMs/denMs(dimd);
Md=denMs;
Md(dimd+1:9)=0;
Mnt=[];
for i=1:(dimd-1)
	if (dimn-i+1)>0
		Mnt(dimd-i)=numMs(dimn-i+1);
	else (dimn-i+1)<=0
		Mnt(dimd-i)=0;
	end
end
Mnt(dimd:8)=0;

if length(K)<dimd;
   Ktemp=K;
   for i=1:dimd;
      if i <=length(K);
         K(1,dimd+1-i)=Ktemp(1,length(K)+1-i);
      else
         K(1,dimd+1-i)=0;
      end
   end
end
K(dimd+1:9)=0;   
Setpoint=1;
Disturb=0;
if ~simulate
   return
end

if isempty(keep) | ~keep
   if exist('old_handle') & ~isempty(old_handle) & old_handle==gcf
      figure(old_handle);
   if ~isempty(x)
      set(old_handle,'Name',['Step responses for process with current parameter x = [ ',...
            num2str(x),' ]']);
   else
      set(old_handle,'Name','Step responses for process with perfect model');
   end
   clf
 uicontrol(old_handle,...
  'Style','push',...
  'Position',[550 170 85 25],...
  'String','Change Process',...
  'Callback',['change_P1(''main'',x,Epsilon{1,1});']);
 uicontrol(old_handle,...
  'Style','push',...
  'Position',[555 140 75 25],...
  'String','Add&Remove',...
  'Callback',['add_remove3(''main'');']);
uicontrol(old_handle,...
  'Style','push',...
  'Position',[555 110 75 25],...
  'String','Change time',...
  'Callback',['change_time2(''main'');']);
uicontrol(old_handle,...
  'Style','push',...
  'Position',[565 80 55 25],...
  'String','Close',...
  'Callback','close');
uicontrol(old_handle,...
  'Style','push',...
  'Position',[565 50 55 25],...
  'String','Help',...
  'Callback','sayhelp(4260)');
   else
      time=15; TimeScale=200; st_size=1; plot_setpoint=1; plot_disturb=0;
      plot_effort=1; setpoint_Enable='on'; disturb_Enable='off';
      effort_Enable='on'; plot_IMC=0; plot_IMC_sat=0; plot_MSF=1;
      store=Epsilon; Etry=Epsilon;storex=x; clflag=1; 
      clmap=[1,0,0;1,1,0;1,0,1;0,0,1;1,1,1];
      f=crfig(3,5,634,410,'MSF_SAT time responses','k','figure','on');
      whitebg('white');
      
%colordef black;
 uicontrol(f,...
  'Style','push',...
  'Position',[550 170 85 25],...
  'String','Change Process',...
  'Callback',['change_P1(''main'',x,Epsilon{1,1});']);
 uicontrol(f,...
  'Style','push',...
  'Position',[555 140 75 25],...
  'String','Add&Remove',...
  'Callback',['add_remove3(''main'');']);
uicontrol(f,...
  'Style','push',...
  'Position',[555 110 75 25],...
  'String','Change time',...
  'Callback',['change_time2(''main'');']);
uicontrol(f,...
  'Style','push',...
  'Position',[565 80 55 25],...
  'String','Close',...
  'Callback','close');
uicontrol(f,...
  'Style','push',...
  'Position',[565 50 55 25],...
  'String','Help',...
  'Callback','sayhelp(4260)');
end
else % keep
   if isempty(old_handle) | ~ishandle(old_handle)
      errordlg('The original figure was gone');
      return
   end
   figure(old_handle)
   set(old_handle,'Name',['Step responses for process with current parameter x = [ ',...
        num2str(x),' ]']);
end % keep

options = simset('SrcWorkspace','current');
temp='';
for i=1:length(Epsilon);
  temp=[temp num2str(Epsilon(i)) ' '];
end
if plot_MSF
   if plot_setpoint
 		Setpoint=1;
 		Disturb=0;
	[t]=sim('msf2df',time,options);
	axis('auto');
	if  plot_effort  
		gh1=subplot(2,1,1);h=plot(t,simout(:,1),'r');
		title(['Setpoint Step Response when Epsilon = ',temp])
		xlabel(' '); ylabel('Output'); grid on;
		hold on;
   
   	gh2=subplot(2,1,2);h=plot(t,simout(:,2),'r');	
		title(['Control effort when Epsilon = ',temp])
		xlabel('Time'); ylabel('Control effort'); grid on;
		hold on	
   elseif plot_disturb
		gh1=subplot(2,1,1);h=plot(t,simout(:,1),'r');
		title(['Setpoint Step Response when Epsilon = ',temp])
		xlabel(' '); ylabel('Output'); grid on;
		hold on;
   else
		gh1=plot(t,simout(:,1),'r');
		title(['Setpoint Step Response when Epsilon = ',temp])
		xlabel('Time'); ylabel('Output'); grid on;
		hold on;
   end
	end
	if plot_disturb
 	Setpoint=0;
 	Disturb=1;
	[t]=sim('msf2df',time,options);
	axis('auto');
	if  plot_effort  
	gh1=subplot(2,1,1);h=plot(t,simout(:,1),'r');
	title(['Disturbance Step Response when Epsilon = ',temp])
	xlabel(' '); ylabel('Output'); grid on;
	hold on;
   
   gh2=subplot(2,1,2);h=plot(t,simout(:,2),'r');	
	title(['Control effort when Epsilon = ',temp])
	xlabel('Time'); ylabel('Control effort'); grid on;
	hold on	
   elseif plot_setpoint
	gh2=subplot(2,1,2);h=plot(t,simout(:,1),'r');
	title(['Disturbance Step Response when Epsilon = ',temp])
	xlabel('Time'); ylabel('Output'); grid on;
	hold on;
   else
	gh1=plot(t,simout(:,1),'r');
	title(['Disturbance Step Response when Epsilon = ',temp])
	xlabel('Time'); ylabel('Output'); grid on;
	hold on;
   end
end
end

if plot_IMC_sat
   if plot_setpoint
 		Setpoint=1;
 		Disturb=0;
	if d_through_p
      [t]=sim('imc2dfu',time,options);
   else
      [t]=sim('imc2df',time,options);
   end
	axis('auto');
	if  plot_effort  
		gh1=subplot(2,1,1);h=plot(t,simout,'m');
		title(['Setpoint Step Response when Epsilon = ',temp])
		xlabel(' '); ylabel('Output'); grid on;
		hold on;
   
   	gh2=subplot(2,1,2);h=plot(t,effort,'m');	
		title(['Control effort when Epsilon = ',temp])
		xlabel('Time'); ylabel('Control effort'); grid on;
		hold on	
   elseif plot_disturb
		gh1=subplot(2,1,1);h=plot(t,simout,'m');
		title(['Setpoint Step Response when Epsilon = ',temp])
		xlabel(' '); ylabel('Output'); grid on;
		hold on;
   else
		gh1=plot(t,simout,'m');
		title(['Setpoint Step Response when Epsilon = ',temp])
		xlabel('Time'); ylabel('Output'); grid on;
		hold on;
   end
	end
	if plot_disturb
 	Setpoint=0;
 	Disturb=1;
	if d_through_p
      [t]=sim('imc2dfu',time,options);
   else
      [t]=sim('imc2df',time,options);
   end
	axis('auto');
	if  plot_effort  
	gh1=subplot(2,1,1);h=plot(t,simout,'m');
	title(['Disturbance Step Response when Epsilon = ',temp])
	xlabel(' '); ylabel('Output'); grid on;
	hold on;
   
   gh2=subplot(2,1,2);h=plot(t,effort,'m');	
	title(['Control effort when Epsilon = ',temp])
	xlabel('Time'); ylabel('Control effort'); grid on;
	hold on	
   elseif plot_setpoint
	gh2=subplot(2,1,2);h=plot(t,simout,'m');
	title(['Disturbance Step Response when Epsilon = ',temp])
	xlabel('Time'); ylabel('Output'); grid on;
	hold on;
   else
	gh1=plot(t,simout,'m');
	title(['Disturbance Step Response when Epsilon = ',temp])
	xlabel('Time'); ylabel('Output'); grid on;
	hold on;
   end
end
end

if plot_IMC
   if plot_setpoint
 		Setpoint=1;
 		Disturb=0;
	if d_through_p
      [t]=sim('unsat_imc2dfu',time,options);
   else
      [t]=sim('unsat_imc2df',time,options);
   end
	axis('auto');
	if  plot_effort  
		gh1=subplot(2,1,1);h=plot(t,simout,'g');
		title(['Setpoint Step Response when Epsilon = ',temp])
		xlabel(' '); ylabel('Output'); grid on;
		hold on;
   
   	gh2=subplot(2,1,2);h=plot(t,effort,'g');	
		title(['Control effort when Epsilon = ',temp])
		xlabel('Time'); ylabel('Control effort'); grid on;
		hold on	
   elseif plot_disturb
		gh1=subplot(2,1,1);h=plot(t,simout,'g');
		title(['Setpoint Step Response when Epsilon = ',temp])
		xlabel(' '); ylabel('Output'); grid on;
		hold on;
   else
		gh1=plot(t,simout,'g');
		title(['Setpoint Step Response when Epsilon = ',temp])
		xlabel('Time'); ylabel('Output'); grid on;
		hold on;
   end
	end
	if plot_disturb
 	Setpoint=0;
 	Disturb=1;
	if d_through_p
      [t]=sim('unsat_imc2dfu',time,options);
   else
      [t]=sim('unsat_imc2df',time,options);
   end
	axis('auto');
	if  plot_effort  
	gh1=subplot(2,1,1);h=plot(t,simout,'g');
	title(['Disturbance Step Response when Epsilon = ',temp])
	xlabel(' '); ylabel('Output'); grid on;
	hold on;
   
   gh2=subplot(2,1,2);h=plot(t,effort,'g');	
	title(['Control effort when Epsilon = ',temp])
	xlabel('Time'); ylabel('Control effort'); grid on;
	hold on	
   elseif plot_setpoint
	gh2=subplot(2,1,2);h=plot(t,simout,'g');
	title(['Disturbance Step Response when Epsilon = ',temp])
	xlabel('Time'); ylabel('Output'); grid on;
	hold on;
   else
	gh1=plot(t,simout,'g');
	title(['Disturbance Step Response when Epsilon = ',temp])
	xlabel('Time'); ylabel('Output'); grid on;
	hold on;
   end
end
end
y_limit=get(gh1,'Ylim');
y_scale=(y_limit(2)-y_limit(1))/6;
y_position=y_limit(1)+y_scale;
x_position=0.5*time;
if plot_MSF
   subplot(gh1);
	h=text(x_position,y_position,'* MSF');
	set(h,'color','r');
   y_position=y_position+y_scale;
end
if plot_IMC_sat
   subplot(gh1);
   if d_through_p
      h=text(x_position,y_position,'* constrained IMC (feedback form)');
   else
      h=text(x_position,y_position,'* constrained IMC ');
   end
	set(h,'color','m');
   y_position=y_position+y_scale;
end
if plot_IMC
   subplot(gh1);
   if d_through_p
      h=text(x_position,y_position,'* unconstrained IMC (feedback form)');
   else
		h=text(x_position,y_position,'* unconstrained IMC');
   end
	set(h,'color','g');
   y_position=y_position+y_scale;
end