detect_rhpz.m

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

function [n_circle,data]=detect_RHPZ(mnummt,mdenmt,mdeadmt,flag)
% function [n_circle,data]=detect_RHPZ(mnummt,mdenmt,mdeadmt,flag)
% detect right half plane of the MIMO system described by matrix of numerator polynomial mnummt,
% and a dead time matrix mdeadmt . The Nyquist plot is calculated along a closed D-contour
% excluding the origin. The function return the number of encirclement around the origin. 
% It also plot a Nyquist plot ,if the flag variable =1.
[row,col]=size(mnummt);
if iscell(mdeadmt) % Conver a cell array of dead time to a matrix
temp=zeros(row,col);
   for i=1:row
      for j=1:col
         temp(i,j)=mt2dt(mdeadmt{i,j},[]);
      end
   end
mdeadmt=temp;
end
for j=1:col
   Ds{j}=lcmpoly(mdenmt(:,j),[]); 
   for i=1:row
      mdead{i,j}=mtx2str(mdeadmt(i,j),4);
      if isempty(mdenmt) | isempty(mdenmt{i,j})
         temp=mtx2str(mnummt{i,j},2);
         if strcmp(mdead{i,j},'')
            m{i,j}=temp;
         else
            m{i,j}=[temp '*' mdead{i,j}];
         end
      else
         mnummt{i,j}=conv(mt2poly(mnummt{i,j},[]),deconv(Ds{j},mt2poly(mdenmt{i,j},[])));
         temp=mtx2str(mnummt{i,j},2);
         if strcmp(mdead{i,j},'')
            m{i,j,1}=temp;
         else
            m{i,j,1}=[temp '*' mdead{i,j}];
         end
      end
   end
end
T=transform(mdeadmt);
m=sw_col(m,T);
model=cell2str(m);
% Find the number of right half plane zeros of the diagonal elements.
mnummt=sw_col(mnummt,T);
product=1;
for i=1:col
   product=conv(product,mt2poly(mnummt{i,i},[]));
end
rhpz=find(roots(product)>0);
lrhpz=length(rhpz); % number of right half plane zeros of the diagonal elements.

n_circle=0;
w_step=0.01; % frequency increment step size
w=0.0001; % starting frequency (small radius of D contour)
w_inf=1000; % stopping frequency (big raduis of D contour)
n_point=1;
% moving along the positive imaginary axis
s=eps+w*sqrt(-1);
value=eval(model);
data(n_point)=det(value)/prod(diag(value)); % Evaluate the determinant, and then factor 
n_point=n_point+1;		  % out the product of the diagonal element.
while w < w_inf & abs(data(n_point-1))>1e-4
   temp=w;
   w=exp(log(w)+w_step);
   s=eps+w*sqrt(-1);
   value=eval(model);
   data(n_point)=det(value)/prod(diag(value));
   n_point=n_point+1;
end
% turning clockwise around the big radius
for ceta=pi/2-pi/20:-pi/20:0
   s=w*(cos(ceta)+sin(ceta)*sqrt(-1));
	value=eval(model);
   data(n_point)=det(value)/prod(diag(value));
   n_point=n_point+1;
end
data=[conj(fliplr(data)) data];
% ************************************
% Counting the number of encirclement
initial_angle = rem(angle(data(1)) + 2*pi, 2*pi);
angle_data = rem(angle(data) - initial_angle + 4*pi,2*pi);
diff_angle=diff(angle_data);
Ipd = find(diff_angle < -pi);
Ind = find(diff_angle > pi);
n_circle= (length(Ind))-(length(Ipd));
n_circle=n_circle+lrhpz;

% **************************************
if flag
f=crfig(50,50,520,400,['Nyquist plot of determinant of the part of MIMO model to be decoupled'],...
   'w','none','off');
xlabel('Real');
ylabel('Imaginary');
hold on
grid on
uicontrol(f,...
'Style','text',...
'Position',[150 355 150 23],...
'Backgroundcolor','w',...
'Foregroundcolor','k',...
'HorizontalAlignment','center',...
'String','Number of RHP zeros = ');
uicontrol(f,...
 'Style','frame',...
 'Position',[290 357 100 24],...
 'Foregroundcolor','k');
uicontrol(f,...
 'Style','edit',...
 'Position',[295 360 90 18],...
 'Backgroundcolor','w',...
 'Foregroundcolor','k',...
 'String',num2str(n_circle));
uicontrol(f,...
  'Style','push',...
  'Position',[380 15 80 25],...
  'String','Close',...
 'Callback','close'); 

plot(real(data),imag(data),'b');
temp1=min(real(data));
temp2=max(real(data));
plot(0,0,'or');
temp3=(temp2-temp1)/20;
[temp,temp1]=min(imag(data));
temp2=data(temp1-1)-data(temp1);
ceta=angle(temp2)+0.3;
temp=data(temp1)+(temp3*cos(ceta)+temp3*sin(ceta)*sqrt(-1));
plot(real([data(temp1) temp]),imag([data(temp1) temp]),'r');
ceta=angle(temp2)-0.3;
temp=data(temp1)+(temp3*cos(ceta)+temp3*sin(ceta)*sqrt(-1));
plot(real([data(temp1) temp]),imag([data(temp1) temp]),'r');
end