function [ssc]=testObserver() %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=ControlStateSpace mdl=1; % 0:hovering ball 1:fast stage A = [ 0 1 0 980 0 -2.8 0 0 -100 ]; B = [ 0 0 100 ]; C = [ 1 0 0 ]; D=0; Ap=A;Am=A; Bp=B;Bm=B; Cp=C;Cm=C; Dp=D;Dm=D;Dt=D; poles = eig(A); poles t = 0:0.01:2; u = zeros(size(t)); x0 = [0.01 0 0]; sys = ss(A,B,C,D); [y,t,x] = lsim(sys,u,t,x0); plot(t,y) title('Open-Loop Response to Non-Zero Initial Condition') xlabel('Time (sec)') ylabel('Ball Position (m)') p1 = -10 + 10i; p2 = -10 - 10i; p3 = -50; K = place(A,B,[p1 p2 p3]); sys_cl = ss(A-B*K,B,C,0); lsim(sys_cl,u,t,x0); xlabel('Time (sec)') ylabel('Ball Position (m)') t = 0:0.01:2; u = 0.001*ones(size(t)); sys_cl = ss(A-B*K,B,C,0); lsim(sys_cl,u,t); xlabel('Time (sec)') ylabel('Ball Position (m)') axis([0 2 -4E-6 0]) V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) lsim(sys_cl,V*u,t) title('Linear Simulation Results (with Nbar)') xlabel('Time (sec)') ylabel('Ball Position (m)') axis([0 2 0 1.2*10^-3]) op1 = -100; op2 = -101; op3 = -102; OP=[op1,op2,op3]; %OP=P*2; L = place(A',C',OP)'; At = [ A-B*K B*K zeros(size(A)) A-L*C ]; Bt = [ B*V zeros(size(B)) ]; Ct = [ C zeros(size(C)) ]; sys = ss(At,Bt,Ct,0); %lsim(sys,zeros(size(t)),t,[x0 x0]); lsim(sys,u,t,[x0 x0]); title('Linear Simulation Results (with observer)') xlabel('Time (sec)') ylabel('Ball Position (m)') t = 0:1E-6:0.1; x0 = [0.01 0.5 -5]; [y,t,x] = lsim(sys,zeros(size(t)),t,[x0 x0]); n = 3; e = x(:,n+1:end); x = x(:,1:n); x_est = x - e; % Save state variables explicitly to aid in plotting h = x(:,1); h_dot = x(:,2); i = x(:,3); h_est = x_est(:,1); h_dot_est = x_est(:,2); i_est = x_est(:,3); plot(t,h,'-r',t,h_est,':r',t,h_dot,'-b',t,h_dot_est,':b',t,i,'-g',t,i_est,':g') legend('h','h_{est}','hdot','hdot_{est}','i','i_{est}') xlabel('Time (sec)') K,L,V end