diff --git a/matlab/StateSpaceControlDesign.m b/matlab/StateSpaceControlDesign.m index 8a98205..26575cf 100644 --- a/matlab/StateSpaceControlDesign.m +++ b/matlab/StateSpaceControlDesign.m @@ -63,6 +63,7 @@ function [ssc]=StateSpaceControlDesign(mot) p1=-6300+280i; p2=-6200+150i; P=[p1 p1' p2 p2']; + P=[-4000 -4100 -1500+10j -1500-10j]; else p1=-3300+2800i; p2=-2700+500i; diff --git a/matlab/testObserver.m b/matlab/testObserver.m index f8c5abd..ce963dd 100644 --- a/matlab/testObserver.m +++ b/matlab/testObserver.m @@ -1,19 +1,50 @@ function [ssc]=testObserver(mot) %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=ControlStateSpace + mdl=1; % 0:hovering ball 1:fast stage + if mdl==0 + A = [ 0 1 0 + 980 0 -2.8 + 0 0 -100 ]; - A = [ 0 1 0 - 980 0 -2.8 - 0 0 -100 ]; + B = [ 0 + 0 + 100 ]; - B = [ 0 - 0 - 100 ]; + C = [ 1 0 0 ]; + D=0; + else + [A,B,C,D]=ssdata(mot.ssMdl); + C=C(3,:);D=D(3); + numc=(mot.mdl.numc); + denc=(mot.mdl.denc); + num1=(mot.mdl.num1); + den1=(mot.mdl.den1); - C = [ 1 0 0 ]; - D=0; - - %[A,B,C,D]=ssdata(mot.ssMdl) - %C=C(3,:);D=D(3); + g1=tf(numc,denc); % iqCmd->iqMeas + s1=ss(g1); + s1.C=[s1.C; 1E5* 2.4E-3 1E-3*s1.C(2)*8.8]; % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values) + g2=tf(num1,den1); %iqMeas->ActPos without resonance frequencies + s2=ss(g2); + s3=append(s1,s2); + s3.A(3,2)=s3.C(1,2)*s3.B(3,2); + + %d=.77;w0=9100; %1.44kHz -> T0=6.9046e-04 + %numc=[w0^2]; + %denc=[1 d*2*w0 w0^2]; + %T0=6E-4; %w=1/1E-3=1E3 -> f=w/(2pi)=1000/6.2=159Hz + %numc=[1]; + %denc=[T0^2 d*2*T0 1]; + %bode(numc,denc) + + num=conv(numc,num1);%num=1; + den=conv(denc,den1);%den=[1 0 0]; + g4=tf(num,den); %iqMeas->ActPos + s4=ss(g4); + mot.ssMdl + s3 + s4 + [A,B,C,D]=ssdata(s4); + end Ap=A;Am=A; Bp=B;Bm=B; @@ -24,97 +55,133 @@ function [ssc]=testObserver(mot) disp(poles); - t = 0:0.01:2; - u = zeros(size(t)); - x0 = [0.01 0 0]; - %x0 = [0.1 0 0 0]; + if mdl==0 + t = 0:0.01:2; + u = zeros(size(t)); + x0 = [0.01 0 0]; - sys = ss(A,B,C,D); + 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)') + [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)') + + p1 = -20 + 20i; p2 = -20 - 20i; p3 = -100; + P=[p1 p2 p3]; + K = place(A,B,P); + 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]) + else + x0 = [0 0 0 0]; + p1=-63+2.80i; + p2=-62+1.50i; + P=[p1 p1' p2 p2']; + P=[-4000 -4100 -500+10j -500-10j]; + K = place(A,B,P); + Ts=1/5000 + t = 0:Ts:2; + u = 0.001*ones(size(t)); + V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) + sys_cl = ss(A-B*K,B*V,C,0); + + lsim(sys_cl,u,t,x0); + xlabel('Time (sec)') + ylabel('Motor Position (m)') + axis([0 2 0 1.2E-3]) + end + + if mdl==0 + 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]*0); + title('Linear Simulation Results (with observer)') + xlabel('Time (sec)') + ylabel('Ball Position (m)') + else + 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,u,t,[x0 x0]*0); + title('Linear Simulation Results (with observer)') + xlabel('Time (sec)') + ylabel('Motor Position (m)') + axis([0 2 0 1.2E-3]) + end - 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)') + if mdl==0 + t = 0:1E-6:0.1; + x0 = [0.01 0.5 -5]; + [y,t,x] = lsim(sys,zeros(size(t)),t,[x0 x0]); - p1 = -20 + 20i; p2 = -20 - 20i; p3 = -100; - 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)') + n = 3; + e = x(:,n+1:end); + x = x(:,1:n); + x_est = x - e; - - 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; - - L = place(A',C',[op1 op2 op3])'; - - 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]); - - 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)') + % 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)') + end %discrete - Ts=1/50; % deltatau std. frq. is 5kHz + Ts=1/5000; % deltatau std. frq. is 5kHz %The discrete system works with sampling >100Hz,. the bigger the frequency the better the result %With sampling = 80Hz hte system already becomes instable. ss_t=ss(At,Bt,Ct,Dt); - figure();pzplot(ss_t) + %figure();pzplot(ss_t) ss_tz=c2d(ss_t,Ts); - figure();pzplot(ss_tz) + %figure();pzplot(ss_tz) [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz); diff --git a/matlab/testObserverSim.slx b/matlab/testObserverSim.slx index df07c9d..72b479a 100644 Binary files a/matlab/testObserverSim.slx and b/matlab/testObserverSim.slx differ