%2014.7.1XV %yp.119,120z %state_space.m A=[0 1;0 -0.01]; B=[0;1]; C=[1 0]; D=0; sys=ss(A,B,C,D) sys.a(2,2)=-0.1 %yp.120z %state_space2.m A=[0 1;0 -0.01]; B=[]; C=[1 0]; D=[]; sys0=ss(A,B,C,D) A=[0 1;0 -0.01]; B=[0;1]; C=[]; D=[]; sys1=ss(A,B,C,D) A=[0 1;0 -0.01]; B=[0;1]; C=eye(size(A)); D=[]; sys2=ss(A,B,C,D) %yp.126z %coordinate_transformation.m om=1; zeta=0.01; A=[0 1;-om^2 -2*zeta*om]; B=[0;om^2]; C=[1 0]; D=0; sys1=ss(A,B,C,D); T=[1 0;-zeta*om om*sqrt(1-zeta^2)]; sys2=ss2ss(sys1,inv(T)) %yp.127z %serial_connection.m A1=[0 1;0 0]; B1=[0;1]; C1=[1 0]; D1=0; sys1=ss(A1,B1,C1,D1); L=1; A2=[0 1;-12/L^2 -6/L]; B2=[0;1]; C2=[0 -12/L]; D2=1; sys2=ss(A2,B2,C2,D2); sys=sys1*sys2 tf(sys2) %yp.133z %stability_check A=[0 1;1 -1]; n=size(A,1) poles=eig(A) sum(real(poles)<0)==n %yp.135,p247z %step_resp1.m sys=ss(-1,1,1,0); t=0:0.1:5; step(sys,t), grid % hold on plot([0 5],(1-exp(-1))*[1 1]) [T,K]=ginput(1) %yp.137,p247z %step_resp2.m A=[0 1;-1 -0.02]; B=[0;1]; C=[1 0]; sys=ss(A,B,C,0); t=0:0.1:10; step(sys,t), grid % [Tp,p0]=ginput(1); p0=p0-1; wn=sqrt(log(p0)^2+pi^2)/Tp %zeta=sqrt(log(p0)^2/(log(p0)^2+pi^2)) zeta=abs(log(p0))/sqrt(log(p0)^2+pi^2) %yp.138,p247z %sin_resp.m sys=ss(-1,1,1,0); x0=1; t=0:0.05:10; u=sin(10*t); y=lsim(sys,u,t,x0); plot(t,y),grid % x0=1; u=zeros(1,length(t)); y1=lsim(sys,u,t,x0); x0=0; u=sin(10*t); y2=lsim(sys,u,t,x0); hold on, plot(t,y1,'g',t,y2,'r') %yp.139z %bode_diag.m sys=ss(-1,1,1,0); w={1e-1,1e1}; bode(sys,w), grid %yp.p247z %bode_diag2.m A=[0 1;-1 -0.02]; B=[0;1]; C=[1 0]; sys=ss(A,B,C,0); w={1e-1,1e1}; bode(sys,w),grid %yp.p.139z %free_resp.m sys=ss(-1,[],1,[]); x0=1; t=0:0.1:5; initial(sys,x0,t), grid %yp.247z %impulse_resp.m A=[0 1;-1 -0.02]; B=[0;1]; C=[1 0]; sys=ss(A,B,C,0); t=0:0.1:10; initial(sys,B,t); grid %yp.247z %sf1.m T1=1; K1=1; a1=-1/T1; b1=K1/T1; sys1=ss(a1,b1,1,0); t=0:0.1:5; step(sys1,t); [T2,K2]=ginput(1) f=T1/K1*(1/T2-1/T1); g=T1/K1*K2/T2; a2=a1-b1*f; b2=b1*g; sys2=ss(a2,b2,1,0); hold on, step(sys2,t) %yp.248z %sf2.m w1=1; z1=0.01; A1=[0 1;-w1^2 -2*z1*w1]; B1=[0;w1^2]; C=[1 0]; sys1=ss(A1,B1,C,0); t=0:0.05:10; step(sys1,t); [Tp,p0]=ginput(1); p0=p0-1; w2=sqrt(log(p0)^2+pi^2)/Tp, z2=abs(log(p0))/sqrt(log(p0)^2+pi^2) Kp=(w2^2-w1^2)/w1^2, Kd=(2/w1^2)*(z2*w2-z1*w1) F=[Kp Kd]; G=w2^2/w1^2; A2=A1-B1*F; B2=B1*G; sys2=ss(A2,B2,C,0); hold on, step(sys2,t) %yp.249z %sf_minputs.m A=[0 0;0 -1]; B=[1 1;1 -1]; r1=-2; r2=-3; disp('(1)'), X1=[1 1;1 -1]; V1=[((A-r1*eye(2))\B)*X1(:,1) ((A-r2*eye(2))\B)*X1(:,2)]; F1=X1/V1, AF1=A-B*F1, ev1=eig(AF1) disp('(2)'), X2=[1 1;1 2]; V2=[((A-r1*eye(2))\B)*X2(:,1) ((A-r2*eye(2))\B)*X2(:,2)]; F2=X2/V2, AF2=A-B*F2, ev2=eig(AF2) %yp.150,p.249z %controllability_check A=[0 1;0 -1]; B=[0;1]; n=size(A,1); r=eig(A), tol=0.01; for i=1:n, rank([B A-r(i)*eye(n)],tol)==n, end A=[0 1 0;0 -1 1;0 0 -1]; B=[0;1;0]; n=size(A,1); r=eig(A), tol=0.01; for i=1:n, rank([B A-r(i)*eye(n)],tol)==n, end A=[0 1 0;-1 -1 0;0 0 2]; B=[0 0;1 -1;0 1]; n=size(A,1); r=eig(A), tol=0.01; for i=1:n, rank([B A-r(i)*eye(n)],tol)==n, end %yp.151,p.250z %stablilizalibity_check.m A=[0 1;0 -1]; B=[0;1]; n=size(A,1); r=eig(A), tol=0.01; for i=1:n if real(r(i))>=0, rank([B A-r(i)*eye(n)],tol)==n, end end A=[0 1 0;0 -1 1;0 0 -1]; B=[0;1;0]; n=size(A,1); r=eig(A), tol=0.01; for i=1:n if real(r(i))>=0, rank([B A-r(i)*eye(n)],tol)==n, end end A=[0 1 0;-1 -1 0;0 0 2]; B=[0 0;1 -1;0 1]; n=size(A,1); r=eig(A), tol=0.01; for i=1:n if real(r(i))>=0, rank([B A-r(i)*eye(n)],tol)==n, end end %yp.156z %obs_err.m A=[0 1;0 0]; C=[1 0]; H=[3;2]; sys1=ss(A,[],eye(2),[]); x0=[1;0.5]; sys2=ss(A-H*C,[],eye(2),[]); xh0=[0;0]; t=0:0.1:5; x=initial(sys1,x0,t); e=initial(sys2,xh0-x0,t); xh=x+e; figure(1),subplot(121),plot(t,x(:,1),t,xh(:,1)),axis([0 5 0 4]),grid figure(1),subplot(122),plot(t,x(:,2),t,xh(:,2)),axis([0 5 0 2]),grid %yp.250z %obs_err2.m A=[0 1;0 0]; C=[1 1]; H=[0;4]; sys1=ss(A,[],eye(2),[]); x0=[1;0.5]; sys2=ss(A-H*C,[],eye(2),[]); xh0=[0;0]; t=0:0.1:5; x=initial(sys1,x0,t); e=initial(sys2,xh0-x0,t); xh=x+e; figure(2),subplot(121),plot(t,x(:,1),t,xh(:,1)),axis([0 5 0 4]),grid figure(2),subplot(122),plot(t,x(:,2),t,xh(:,2)),axis([0 5 0 2]),grid %yp.251z %observer_based_controller.m a=-1; b=1; c=1; f=4; sys=ss(a-b*f,[],c,[]); h1=9; A1=[a-b*f -b*f;0 a-h1*c]; sys1=ss(A1,[],[1 0],[]); h2=2; A2=[a-b*f -b*f;0 a-h2*c]; sys2=ss(A2,[],[1 0],[]); t=0:0.1:3; x0=1; xh0=0; X0=[x0; xh0-x0]; y=initial(sys,x0,t); y1=initial(sys1,X0,t); y2=initial(sys2,X0,t); figure(3),plot(t,y,t,y1,t,y2),grid legend('h=0','h=9','h=2') %yp.252z %obs_err3.m A=[0 1;0 0]; B=[0;1]; C=[1 0]; A21=0; A22=0; B2=1; L=2; U=[-L 1]; AH=A22-L; BH=A21-(A22-L)*L; CH=[0;1]; DH=[1;L]; JH=B2; sys1=ss(A,[],eye(2),[]); x0=[1;0.5]; sys2=ss(AH,[],1,[]); xh0=0; t=0:0.1:5; x=initial(sys1,x0,t)'; y=C*x; e=initial(sys2,xh0-U*x0,t)'; xh=U*x+e; z=CH*xh+DH*y; figure(4),subplot(121),plot(t,x(1,:),t,z(1,:)),axis([0 5 0 4]),grid figure(4),subplot(122),plot(t,x(2,:),t,z(2,:)),axis([0 5 0 2]),grid %yp.252z %obs_err4.m A=[0 1;0 0]; B=[0;1]; C=[1 0]; F=[1 2]; A21=0; A22=0; B2=1; K1=-1; K2=-2; lambda=-1; L=A22-lambda; U=K2*[-L 1]; AH=lambda; BH=K2*(A21+lambda*L); JH=K2*B2; CH=1; DH=K1+K2*L; AA=[A-B*F B*CH;0 0 AH]; BB=[B; 0]; CC=[-F 0]; sys1=ss(A-B*F,[],-F,[]); x0=[1;0.5]; sys2=ss(AA,[],CC,[]); xh0=0; X0=[x0; xh0-U*x0]; t=0:0.1:5; u1=initial(sys1,x0,t); u2=initial(sys2,X0,t); figure(5),plot(t,u1,t,u2),axis([0 5 -2 2]),grid legend('sf','obc') %yp.253z %lqr1.m T=1; K=1; a=-1/T; b=K/T; c=1; t=0:0.1:5; x0=1; s0=ss(a,[],c,[]); y0=initial(s0,x0,t); f1=1+sqrt(2); s1=ss(a-b*f1,[],c,[]); y1=initial(s1,x0,t); u1=-f1*y1; f2=1+sqrt(3); s2=ss(a-b*f2,[],c,[]); y2=initial(s2,x0,t); u2=-f2*y2; f3=1+sqrt(11); s3=ss(a-b*f3,[],c,[]); y3=initial(s3,x0,t); u3=-f3*y3; figure(1),subplot(121),plot(t,y0,t,y1,t,y1,t,y2,t,y3),grid,title('y') figure(1),subplot(122),plot(t,u1,t,u1,t,u2,t,u3),grid,title('u') %yp.171z %opt.m function [F,p]=opt(A,B,C,Q,R) n=size(A,1); W=R\B'; [V,R]=eig([A -B*W;-C'*Q*C -A']); p=diag(R); [wk,id]=sort(real(p)); p=p(id(1:n)); V1=V(1:n,id(1:n)); V2=V(n+1:2*n,id(1:n)); X=real(V2/V1); F=W*X; %yp.171z %lqr.m A=[0 1;0 0]; B=[0;1]; C=[1 0]; Q=1; R=1; [F,p]=opt(A,B,C,Q,R) %yp.255z %lqr2.m A=[0 1;0 -1]; B=[0;1]; C=[1 0]; Q=1; R=1; [F,p]=opt(A,B,C,Q,R) sys=ss(A-B*F,[],eye(2),[]); x0=[1;0]; t=0:0.1:10; x=initial(sys,x0,t)'; y=C*x; u=-F*x; figure(2),subplot(121),plot(t,y),grid,title('y') figure(2),subplot(122),plot(t,u),grid,title('u') %yp.255z %lqr3.m A=[0 1 0;0 0 0;0 0 -1]; B=[0 0;1 -1;0 1]; C=[1 0 0;0 0 1]; [F,p]=opt(A,B,C,1,1) %yp.255z %lqr4.m A=[0 0 1 0;0 0 0 1;-1 1 0 0;1 -1 0 0]; B=[0;0;1;0]; C=[0 1 0 0]; [F,p]=opt(A,B,C,1,1) %yp.176z %optobc.m function [AK,BK,CK,pK,pcare,pfare]=optobc(A,B,C,CC,Q,R,BB,W,V) [F,pcare]=opt(A,B,CC,Q,R); [H,pfare]=opt(A',C',BB',W,V); H=H'; AK=A-H*C-B*F; BK=H; CK=F; pK=eig(AK); %yp.255z %lqr5.m [AK,BK,CK,pK,pcare,pfare]=optobc(-1,1,1,1,1,1,1,1,0.2^2) %yp.256z %lqi1.m A=0; B=1; C=1; S=[A B;C 0]; F=2; FI=1; Fr=[F 1]*(S\[0;1])/2; AA=[A-B*F -B*FI;C 0]; CC=[C 0;-F -FI]; DD=[0 0;0 Fr]; t=0:0.1:10; u=ones(2,length(t)); X0=[0;0]; r=1; %----- w=0; BB1=[w 0;0 -r]; BB2=[w Fr;0 -r]; sys1=ss(AA,BB1,CC,0); y1=lsim(sys1,u,t,X0); sys2=ss(AA,BB2,CC,DD); y2=lsim(sys2,u,t,X0); figure(1),subplot(121),plot(t,y1(:,1),t,y2(:,1)), axis([0 10 0 2]),grid,title('y under disturbance') figure(1),subplot(122),plot(t,y1(:,2),t,y2(:,2)), axis([0 10 -2 2]),grid,title('u under disturbance') %----- w=1; BB1=[w 0;0 -r]; BB2=[w Fr;0 -r]; sys1=ss(AA,BB1,CC,0); y1=lsim(sys1,u,t,X0); sys2=ss(AA,BB2,CC,DD); y2=lsim(sys2,u,t,X0); figure(2),subplot(121),plot(t,y1(:,1),t,y2(:,1)), axis([0 10 0 2]),grid,title('y under disturbance') figure(2),subplot(122),plot(t,y1(:,2),t,y2(:,2)), axis([0 10 -2 2]),grid,title('u under disturbance') %yp.256z %lqi2.m A=[0 1;0 0]; B=[0;1]; C=[1 0]; S=[A B;C 0]; F=[3 3]; FI=1; Fr=[F 1]*(S\[0;0;1])/2; AA=[A-B*F -B*FI;C 0]; CC=[C 0;-F -FI]; DD=[0 0;0 Fr]; t=0:0.1:10; u=ones(2,length(t)); X0=[0;0;0]; r=1; %----- w=[0;1]; BB1=[w [0;0];0 -r]; BB2=[w [0;Fr];0 -r]; sys1=ss(AA,BB1,CC,0); y1=lsim(sys1,u,t,X0); sys2=ss(AA,BB2,CC,DD); y2=lsim(sys2,u,t,X0); figure(1),subplot(121),plot(t,y1(:,1),t,y2(:,1)), axis([0 10 0 2]),grid,title('y under disturbance') figure(1),subplot(122),plot(t,y1(:,2),t,y2(:,2)), axis([0 10 -2 2]),grid,title('u under disturbance') %----- w=[0;0]; BB1=[w [0;0];0 -r]; BB2=[w [0;Fr];0 -r]; sys1=ss(AA,BB1,CC,0); y1=lsim(sys1,u,t,X0); sys2=ss(AA,BB2,CC,DD); y2=lsim(sys2,u,t,X0); figure(2),subplot(121),plot(t,y1(:,1),t,y2(:,1)), axis([0 10 0 2]),grid,title('y under disturbance') figure(2),subplot(122),plot(t,y1(:,2),t,y2(:,2)), axis([0 10 -2 2]),grid,title('u under disturbance') %yp.187z %lqi.m A=[0 1;0 0]; B=[0;1]; C=[1 0]; S=[A B;C 0]; AE2=[A zeros(2,1);C 0]; BE2=[B; 0]; CE2=eye(3); QE2=diag([1 1 1].^2); RE2=0.5^2; [FE2,pE2]=opt(AE2,BE2,CE2,QE2,RE2) F=FE2(1,1:2); FI=FE2(1,3); Fr=[F 1]*(S\[0;0;1])/2; AA=[A-B*F -B*FI;C 0]; CC=[C 0;-F -FI]; DD=[0 0;0 Fr]; t=0:0.1:20; u=ones(2,length(t)); X0=[0;0;0]; r=1; %----- w=[0;1]; BB1=[w [0;0];0 -r]; BB2=[w [0;Fr];0 -r]; sys1=ss(AA,BB1,CC,0); y1=lsim(sys1,u,t,X0); sys2=ss(AA,BB2,CC,DD); y2=lsim(sys2,u,t,X0); figure(1),subplot(121),plot(t,y1(:,1),t,y2(:,1)), axis([0 10 0 2]),grid,title('y under disturbance') figure(1),subplot(122),plot(t,y1(:,2),t,y2(:,2)), axis([0 10 -2 2]),grid,title('u under disturbance') %----- w=[0;0]; BB1=[w [0;0];0 -r]; BB2=[w [0;Fr];0 -r]; sys1=ss(AA,BB1,CC,0); y1=lsim(sys1,u,t,X0); sys2=ss(AA,BB2,CC,DD); y2=lsim(sys2,u,t,X0); figure(2),subplot(121),plot(t,y1(:,1),t,y2(:,1)), axis([0 10 0 2]),grid,title('y under disturbance') figure(2),subplot(122),plot(t,y1(:,2),t,y2(:,2)), axis([0 10 -2 2]),grid,title('u under disturbance') %yp.257z %lqi3.m A=[0 1;0 0]; B=[0;1]; C=[1 0]; S=[A B;C 0]; AE3=[A B;zeros(1,3)]; BE3=[zeros(2,1);1]; CE3=eye(3); M1=1; T1=1; M2=1; T2=0.5; q1=1/M1; q2=T1/M1; q3=1/M2; r1=T2/M2; QE3=diag([q1 q2 q3].^2); RE3=r1^2; [KE3,pE3]=opt(AE3,BE3,CE3,QE3,RE3) FE3=KE3/S; F=FE3(1,1:2); FI=FE3(1,3); Fr=[F 1]*(S\[0;0;1])/2; AA=[A-B*F -B*FI;C 0]; CC=[C 0;-F -FI]; DD=[0 0;0 Fr]; t=0:0.1:20; u=ones(2,length(t)); X0=[0;0;0]; r=1; %----- w=[0;1]; BB1=[w [0;0];0 -r]; BB2=[w [0;Fr];0 -r]; sys1=ss(AA,BB1,CC,0); y1=lsim(sys1,u,t,X0); sys2=ss(AA,BB2,CC,DD); y2=lsim(sys2,u,t,X0); figure(1),subplot(121),plot(t,y1(:,1),t,y2(:,1)), axis([0 10 0 2]),grid,title('y under disturbance') figure(1),subplot(122),plot(t,y1(:,2),t,y2(:,2)), axis([0 10 -2 2]),grid,title('u under disturbance') %----- w=[0;0]; BB1=[w [0;0];0 -r]; BB2=[w [0;Fr];0 -r]; sys1=ss(AA,BB1,CC,0); y1=lsim(sys1,u,t,X0); sys2=ss(AA,BB2,CC,DD); y2=lsim(sys2,u,t,X0); figure(2),subplot(121),plot(t,y1(:,1),t,y2(:,1)), axis([0 10 0 2]),grid,title('y under disturbance') figure(2),subplot(122),plot(t,y1(:,2),t,y2(:,2)), axis([0 10 -2 2]),grid,title('u under disturbance') %yp.190,p.191z /*pen*/ dth:'diff(th(t),t); ddth:'diff(th(t),t,2); x:ell*sin(th(t)); y:ell*cos(th(t)); J:(1/3)*m*ell^2; T:(1/2)*m*(diff(x,t)^2+diff(y,t)^2)+(1/2)*J*dth^2; V:m*g*ell*cos(th(t)); L:T-V; LE:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce; sol:solve(LE,ddth); f:matrix([dth],[rhs(sol[1])]); A:matrix([diff(f[1,1],th(t)),diff(f[1,1],dth)],[diff(f[2,1],th(t)),diff(f[2,1],dth)]); %yp.192,p.194,p.196z /*pend*/ dr:'diff(r(t),t); ddr:'diff(r(t),t,2); dth:'diff(th(t),t); ddth:'diff(th(t),t,2); T0:(1/2)*M*diff(r(t),t)^2; x1:r(t)+ell*sin(th(t)); y1:ell*cos(th(t)); J1:(1/3)*m*ell^2; T1:(1/2)*m*(diff(x1,t)^2+diff(y1,t)^2)+(1/2)*J1*dth^2; V1:m*g*y1; L:T0+T1-V1; LE1:diff(diff(L,dr),t)-diff(L,r(t))=F,trigreduce,ratsimp; LE2:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce,ratsimp; sol:solve([LE1,LE2],[ddr,ddth]); f:matrix([dr],[dth],[rhs(sol[1][1])],[rhs(sol[1][2])]); a31:diff(f[3,1],r(t)); a32:diff(f[3,1],th(t)); a33:diff(f[3,1],dr); a34:diff(f[3,1],dth); a41:diff(f[4,1],r(t)); a42:diff(f[4,1],th(t)); a43:diff(f[4,1],dr); a44:diff(f[4,1],dth); b3:diff(f[3,1],F); b4:diff(f[4,1],F); A:matrix([0,0,1,0],[0,0,0,1],[a31,a32,a33,a34],[a41,a42,a43,a44]); B:matrix([0],[0],[b3],[b4]); A1:A,th(t)=0,F=0,trigreduce,ratsimp; B1:B,th(t)=0,F=0,trigreduce,ratsimp; A2:A,th(t)=%pi,F=0,trigreduce,ratsimp; B2:B,th(t)=%pi,F=0,trigreduce,ratsimp; lambda:eigenvalues(A1); rank(addcol(A1-lambda[1][1]*ident(4),B1)); rank(addcol(A1-lambda[1][2]*ident(4),B1)); rank(addcol(A1-lambda[2][1]*ident(4),B1)); rank(addcol(A1-lambda[2][2]*ident(4),B1)); lambda:eigenvalues(A2); rank(addcol(A2-lambda[1][1]*ident(4),B2)); rank(addcol(A2-lambda[1][2]*ident(4),B2)); rank(addcol(A2-lambda[2][1]*ident(4),B2)); rank(addcol(A2-lambda[2][2]*ident(4),B2)); %yp.258z /*pend2*/ dr:'diff(r(t),t); ddr:'diff(r(t),t,2); dth:'diff(th(t),t); ddth:'diff(th(t),t,2); x0:r(t)*cos(alpha); y0:r(t)*sin(alpha); T0:(1/2)*M*(diff(x0,t)^2+diff(y0,t)^2); V0:M*g*y0; x1:x0+ell*sin(th(t)); y1:y0+ell*cos(th(t)); J1:(1/3)*m*ell^2; T1:(1/2)*m*(diff(x1,t)^2+diff(y1,t)^2)+(1/2)*J1*dth^2; V1:m*g*y1; L:T0+T1-V0-V1; LE1:diff(diff(L,dr),t)-diff(L,r(t))=F,trigreduce,ratsimp; LE2:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce,ratsimp; sol:solve([LE1,LE2],[ddr,ddth]); f:matrix([dr],[dth],[rhs(sol[1][1])],[rhs(sol[1][2])]); a31:diff(f[3,1],r(t)); a32:diff(f[3,1],th(t)); a33:diff(f[3,1],dr); a34:diff(f[3,1],dth); a41:diff(f[4,1],r(t)); a42:diff(f[4,1],th(t)); a43:diff(f[4,1],dr); a44:diff(f[4,1],dth); b3:diff(f[3,1],F); b4:diff(f[4,1],F); A:matrix([0,0,1,0],[0,0,0,1],[a31,a32,a33,a34],[a41,a42,a43,a44]); B:matrix([0],[0],[b3],[b4]); A1:A,th(t)=0,F=(M+m)*g*sin(alpha),trigreduce,ratsimp; B1:B,th(t)=0,F=(M+m)*g*sin(alpha),trigreduce,ratsimp; A2:A,th(t)=%pi,F=(M+m)*g*sin(alpha),trigreduce,ratsimp; B2:B,th(t)=%pi,F=(M+m)*g*sin(alpha),trigreduce,ratsimp; %yp.259z %pend_controllability.m M=1; m=0.1; ell=0.25; g=9.8; E=[M+m m*ell;m*ell (4/3)*m*ell^2]; A21=-E\[0 0;0 -m*ell*g]; A=[zeros(2) eye(2);A21 zeros(2)] B2=E\[1;0]; B=[zeros(2,1);B2] r=eig(A) n=size(A,1); for i=1:n, w(i)=rank([B A-r(i)*eye(n)],0.01)==n; end controllability=[r,w'] %yp,198z %pend.m global M m ell g th0 M=1; m=0.1; ell=0.25; g=9.8; th0=0; r=0.5; E=[M+m m*ell;m*ell (4/3)*m*ell^2]; A21=-E\[0 0;0 -m*ell*g]; A=[zeros(2) eye(2);A21 zeros(2)] B2=E\[1;0]; B=[zeros(2,1);B2] w2=E\[0;0]; w=[zeros(2,1);w2] CM=[1 0 0 0;0 1 0 0]; CS=[1 0]; C=CS*CM; AE=[A B;zeros(1,5)]; BE=[zeros(4,1); 1]; CE=eye(5); Tcart=0.1; Kr=0.5; Tpend=(1/4)*(2*pi*sqrt(4/3*ell/g)); Kth=5/180*pi; Tamp=0.1; Kamp=5; q1=1/Kr; q2=1/Kth; q3=Tcart/Kr; q4=Tpend/Kth; q5=1/Kamp; QE=diag([q1 q2 q3 q4 q5].^2); RE=(Tamp/Kamp)^2; [FE,pAE]=opt(AE,BE,CE,QE,RE) FE=FE/[A B;C 0]; F=FE(:,1:4); FI=FE(:,5); ACL=[A-B*F -B*FI;C 0]; BCL=[w;-r]; CCL=[CM zeros(2,1);-F -FI]; DCL=[zeros(2,1);0]; sys=ss(ACL,BCL,CCL,DCL); step(sys) %yp.199z %spend.m function [sys,x0]=spend(t,state,input,flag) global M m ell g th0 if abs(flag)==1 u =input(1); r =state(1); th =state(2); dr =state(3); dth=state(4); Mp=[M+m m*ell*cos(th); m*ell*cos(th) (4/3)*m*ell^2]; Cp=[0 -m*ell*dth*sin(th);0 0]; Gp=[0; -m*ell*g*sin(th)]; sys=[dr;dth;Mp\(-Cp*[dr;dth]-Gp+[u;0])]; elseif flag==3, sys=state(1:2); elseif flag==0 sys=[4;0;2;1;0;0]; x0=[0;th0;0;0]; else sys=[]; end %yp.260z %pend2.m global M m ell g th0 alpha M=1; m=0.1; ell=0.25; g=9.8; th0=0; alpha=5/180*pi; r=0.5; E=[M+m m*ell*cos(alpha);m*ell*cos(alpha) (4/3)*m*ell^2]; A21=-E\[0 0;0 -m*ell*g]; A=[zeros(2) eye(2);A21 zeros(2)] B2=E\[1;0]; B=[zeros(2,1);B2] w2=E\[-(M+m)*g*sin(alpha);0]; w=[zeros(2,1);w2] CM=[1 0 0 0;0 1 0 0]; CS=[1 0]; C=CS*CM; AE=[A B;zeros(1,5)]; BE=[zeros(4,1); 1]; CE=eye(5); Tcart=0.1; Kr=0.5; Tpend=(1/4)*(2*pi*sqrt(4/3*ell/g)); Kth=5/180*pi; Tamp=0.1; Kamp=5; q1=1/Kr; q2=1/Kth; q3=Tcart/Kr; q4=Tpend/Kth; q5=1/Kamp; QE=diag([q1 q2 q3 q4 q5].^2); RE=(Tamp/Kamp)^2; [FE,pAE]=opt(AE,BE,CE,QE,RE) FE=FE/[A B;C 0]; F=FE(:,1:4); FI=FE(:,5); ACL=[A-B*F -B*FI;C 0]; BCL=[w;-r]; CCL=[CM zeros(2,1);-F -FI]; DCL=[zeros(2,1);0]; sys=ss(ACL,BCL,CCL,DCL); step(sys) %yp.260z %spend2.m function [sys,x0]=spend2(t,state,input,flag) global M m ell g th0 alpha if abs(flag)==1 u =input(1); r =state(1); th =state(2); dr =state(3); dth=state(4); Mp=[M+m m*ell*cos(th+alpha); m*ell*cos(th+alpha) (4/3)*m*ell^2]; Cp=[0 -m*ell*dth*sin(th+alpha);0 0]; Gp=[0; -m*ell*g*sin(th)]; sys=[dr;dth;Mp\(-Cp*[dr;dth]-Gp+[u-(M+m)*g*sin(alpha);0])]; elseif flag==3, sys=state(1:2); elseif flag==0 sys=[4;0;2;1;0;0]; x0=[0;th0;0;0]; else sys=[]; end %yp.209z %staircase.m function [T,m]=staircase(A,B,tol) [n,r]=size(B); j=0; s=0; T=eye(n); B1=B; A1=A; while j