Question

R(s) (s+a) Plant (3rd-order system) C(s) Mission Find control system parameters (K, a) Approach Plant modeling Gain and phase margin/ Maximum overshoot/ Bandwidth MATLAB

Answer 1

function [V,S] = alphavol(X,R,fig)

%ALPHAVOL Alpha shape of 2D or 3D point set.

% V = ALPHAVOL(X,R) gives the area or volume V of the basic alpha shape

% for a 2D or 3D point set. X is a coordinate matrix of size Nx2 or Nx3.

%

% R is the probe radius with default value R = Inf. In the default case

% the basic alpha shape (or alpha hull) is the convex hull.

%

% [V,S] = ALPHAVOL(X,R) outputs a structure S with fields:

% S.tri - Triangulation of the alpha shape (Mx3 or Mx4)

% S.vol - Area or volume of simplices in triangulation (Mx1)

% S.rcc - Circumradius of simplices in triangulation (Mx1)

% S.bnd - Boundary facets (Px2 or Px3)

%

% ALPHAVOL(X,R,1) plots the alpha shape.

%

% % 2D Example - C shape

% t = linspace(0.6,5.7,500)';

% X = 2*[cos(t),sin(t)] + rand(500,2);

% subplot(221), alphavol(X,inf,1);

% subplot(222), alphavol(X,1,1);

% subplot(223), alphavol(X,0.5,1);

% subplot(224), alphavol(X,0.2,1);

%

% % 3D Example - Sphere

% [x,y,z] = sphere;

% [V,S] = alphavol([x(:),y(:),z(:)]);

% trisurf(S.bnd,x,y,z,'FaceColor','blue','FaceAlpha',1)

% axis equal

%

% % 3D Example - Ring

% [x,y,z] = sphere;

% ii = abs(z) < 0.4;

% X = [x(ii),y(ii),z(ii)];

% X = [X; 0.8*X];

% subplot(211), alphavol(X,inf,1);

% subplot(212), alphavol(X,0.5,1);

if nargin < 2 || isempty(R), R = inf; end

if nargin < 3, fig = 0; end

% Check coordinates

dim = size(X,2);

if dim < 2 || dim > 3

error('alphavol:dimension','X must have 2 or 3 columns.')

end

% Check probe radius

if ~isscalar(R) || ~isreal(R) || isnan(R)

error('alphavol:radius','R must be a real number.')

end

% Unique points

[X,imap] = unique(X,'rows');

% Delaunay triangulation

T = delaunay(X);

% Remove zero volume tetrahedra since

% these can be of arbitrary large circumradius

if dim == 3

n = size(T,1);

vol = volumes(T,X);

epsvol = 1e-12*sum(vol)/n;

T = T(vol > epsvol,:);

holes = size(T,1) < n;

end

% Limit circumradius of simplices

[~,rcc] = circumcenters(TriRep(T,X));

T = T(rcc < R,:);

rcc = rcc(rcc < R);

% Volume/Area of alpha shape

vol = volumes(T,X);

V = sum(vol);

% Return?

if nargout < 2 && ~fig

return

end

% Turn off TriRep warning

warning('off','MATLAB:TriRep:PtsNotInTriWarnId')

% Alpha shape boundary

if ~isempty(T)

% Facets referenced by only one simplex

B = freeBoundary(TriRep(T,X));

if dim == 3 && holes

% The removal of zero volume tetrahedra causes false boundary

% faces in the interior of the volume. Take care of these.

B = trueboundary(B,X);

end

else

B = zeros(0,dim);

end

% Plot alpha shape

if fig

if dim == 2

% Plot boundary edges and point set

x = X(:,1);

y = X(:,2);

h=plot(x(B)',y(B)','r','linewidth',2);

str = 'Area';

elseif ~isempty(B)

% Plot boundary faces

trisurf(TriRep(B,X),'FaceColor','red','FaceAlpha',1/3);

str = 'Volume';

else

cla

str = 'Volume';

end

axis equal

str = sprintf('Radius = %g, %s = %g',R,str,V);

title(str,'fontsize',12)

end

% Turn on TriRep warning

warning('on','MATLAB:TriRep:PtsNotInTriWarnId')

% Return structure

if nargout == 2

S = struct('tri',imap(T),'vol',vol,'rcc',rcc,'bnd',imap(B));

end

%--------------------------------------------------------------------------

function vol = volumes(T,X)

%VOLUMES Volumes/areas of tetrahedra/triangles

% Empty case

if isempty(T)

vol = zeros(0,1);

return

end

% Local coordinates

A = X(T(:,1),:);

B = X(T(:,2),:) - A;

C = X(T(:,3),:) - A;

  

if size(X,2) == 3

% 3D Volume

D = X(T(:,4),:) - A;

BxC = cross(B,C,2);

vol = dot(BxC,D,2);

vol = abs(vol)/6;

else

% 2D Area

vol = B(:,1).*C(:,2) - B(:,2).*C(:,1);

vol = abs(vol)/2;

end

%--------------------------------------------------------------------------

function B = trueboundary(B,X)

%TRUEBOUNDARY True boundary faces

% Remove false boundary caused by the removal of zero volume

% tetrahedra. The input B is the output of TriRep/freeBoundary.

% Surface triangulation

facerep = TriRep(B,X);

% Find edges attached to two coplanar faces

E0 = edges(facerep);

E1 = featureEdges(facerep, 1e-6);

E2 = setdiff(E0,E1,'rows');

% Nothing found

if isempty(E2)

return

end

% Get face pairs attached to these edges

% The edges connects faces into planar patches

facelist = edgeAttachments(facerep,E2);

pairs = cell2mat(facelist);

% Compute planar patches (connected regions of faces)

n = size(B,1);

C = sparse(pairs(:,1),pairs(:,2),1,n,n);

C = C + C' + speye(n);

[~,p,r] = dmperm(C);

% Count planar patches

iface = diff(r);

num = numel(iface);

% List faces and vertices in patches

facelist = cell(num,1);

vertlist = cell(num,1);

for k = 1:num

% Neglect singel face patches, they are true boundary

if iface(k) > 1

  

% List of faces in patch k

facelist{k} = p(r(k):r(k+1)-1);

  

% List of unique vertices in patch k

vk = B(facelist{k},:);

vk = sort(vk(:))';

ik = [true,diff(vk)>0];

vertlist{k} = vk(ik);

  

end

end

% Sort patches by number of vertices

ivert = cellfun(@numel,vertlist);

[ivert,isort] = sort(ivert);

facelist = facelist(isort);

vertlist = vertlist(isort);

% Group patches by number of vertices

p = [0;find(diff(ivert));num] + 1;

ipatch = diff(p);

% Initiate true boundary list

ibound = true(n,1);

% Loop over groups

for k = 1:numel(ipatch)

% Treat groups with at least two patches and four vertices

if ipatch(k) > 1 && ivert(p(k)) > 3

% Find double patches (identical vertex rows)

V = cell2mat(vertlist(p(k):p(k+1)-1));

[V,isort] = sortrows(V);

id = ~any(diff(V),2);

id = [id;0] | [0;id];

id(isort) = id;

% Deactivate faces in boundary list

for j = find(id')

ibound(facelist{j-1+p(k)}) = 0;

end

  

end

end

% Remove false faces

B = B(ibound,:);

function Aq = Aq(in1)

%AQ

% AQ = AQ(IN1)

% This function was generated by the Symbolic Math Toolbox version 6.1.

% 22-Jun-2015 10:11:29

q1 = in1(1,:);

q2 = in1(2,:);

q3 = in1(3,:);

t2 = q1+q2;

t3 = cos(t2);

t4 = sin(t2);

t5 = sin(q1);

t6 = t4+t5-5.0./4.0;

Aq = [t6.*(t3+cos(q1)).*2.0,t3.*t6.*2.0,q3.*2.0-1.0,0.0];

function Cs = Cf(in1,in2)

%CF

% CS = CF(IN1,IN2)

% This function was generated by the Symbolic Math Toolbox version 6.1.

% 22-Jun-2015 10:11:28

dq1 = in2(1,:);

dq2 = in2(2,:);

q2 = in1(2,:);

t2 = sin(q2);

Cs = reshape([dq2.*t2.*-2.5e1,dq1.*t2.*2.5e1,0.0,0.0,t2.*(dq1+dq2).*-2.5e1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[4, 4]);

function [phix,phiq,Aq,RB]=cnt(x,y,z,q,k,DHT)

n=length(q);

phix=sym('phix',[k,1]);

phiq=sym('phiq',[k,1]);

Aq=sym('Aq',[k,n]);

%declare k independent constraints in work space

RR=symDHmodel(DHT);

xq=RR(1,4);

yq=RR(2,4);

zq=RR(3,4);

RB=zeros(1,k);

for i=1:k

phix(i)=(y-1.25).^2+(z-0.5).^2-0.3^2;%(y).^2+(z-i).^2-0.5/i;

%sin(x).^2+sin(y-i).^2+sin(z-i).^2-0.75;

RB(i)=-1;

phiq(i)=simplify(subs(phix(i),[x y z],[xq,yq,zq]));

Aq(i,:)=jacobian(phiq(i),q);

end

end

function [phix,phiq,Aq,RB]=cntf(x,y,z,q,k,DHT)

n=length(q);

phix=sym('phix',[k,1]);

phiq=sym('phiq',[k,1]);

Aq=sym('Aq',[k,n]);

%declare k independent constraints in work space

RR=symDHmodel(DHT);

xq=RR(1,4);

yq=RR(2,4);

zq=RR(3,4);

RB=zeros(1,k);

for i=1:k

%phix(i)=(y).^2+(z-i).^2-0.5/i;

phix(i)=(y-1.25).^2+(z-0.5).^2-(0.3+0.01)^2;

%phix(i)=sin(x).^2+sin(y-i).^2+sin(z-i).^2-0.75;

RB(i)=-1;

phiq(i)=simplify(subs(phix(i),[x y z],[xq,yq,zq]));

Aq(i,:)=jacobian(phiq(i),q);

end

end

function [xs,ys,zs]= cntxyz(m,f)

L=400;

xt=linspace(-m,m,L);

yt=linspace(-m,m,L);

zt=linspace(-m,m,L);

[XX,YY,ZZ]=meshgrid(xt,yt,zt);

eko=phix(XX,YY,ZZ);

gamma=eko((f-1)*L+1:f*L,:,:);

idx=find(abs(gamma)<10/L);

[i,j,k]=ind2sub(size(gamma),idx);

xs=xt(j)';

ys=yt(i)';

zs=zt(k)';

end

function Ds = Df(in1)

%DF

% DS = DF(IN1)

% This function was generated by the Symbolic Math Toolbox version 6.1.

% 22-Jun-2015 10:11:28

dq1 = in1(1,:);

dq2 = in1(2,:);

dq3 = in1(3,:);

dq4 = in1(4,:);

Ds = [dq1+sign(dq1);dq2+sign(dq2);dq3.*(3.0./1.0e1)+sign(dq3).*(3.0./1.0e1);dq4+sign(dq4)];

function T=DH(X)

tet=X(1);d=X(2);alp=X(3);r=X(4);

R=[cos(tet),-sin(tet).*cos(alp),sin(tet).*sin(alp);...

sin(tet),cos(tet).*cos(alp),-cos(tet).*sin(alp);...

0,sin(alp),cos(alp)];

P=[r.*cos(tet);r.*sin(tet);d];

T=[[R;0,0,0],[P;1]];

end

function d=diseko(x,y)

e=x-y;

e2=e.^2;

d=sqrt(sum(e2));

end