97 lines
2.1 KiB
Matlab
97 lines
2.1 KiB
Matlab
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
|