diff --git a/matlab/StateSpaceControlDesign.m b/matlab/StateSpaceControlDesign.m index a63f972..8a98205 100644 --- a/matlab/StateSpaceControlDesign.m +++ b/matlab/StateSpaceControlDesign.m @@ -16,7 +16,7 @@ function [ssc]=StateSpaceControlDesign(mot) % % https://www.youtube.com/watch?v=Lax3etc837U - ssPlt=mot.ssPlt; %real plant (model of real plant) + ssPlt=mot.ssMdl;%ssPlt; %real plant (model of real plant) ssPlt.Name='open loop plant'; ssMdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer ssMdl.Name='open loop model'; @@ -139,7 +139,7 @@ function [ssc]=StateSpaceControlDesign(mot) Ts=1/5000; % 5kHz ss_tz = c2d(ss_t,Ts); [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz ); - ss_tz .Name='discrete obsvr ctrl'; + ss_tz.Name='discrete obsvr ctrl'; % step answer on closed loop with disctrete observer controller: t = 0:Ts:.05; figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete'); diff --git a/matlab/observer.slx b/matlab/observer.slx index 25462e5..974f9b3 100644 Binary files a/matlab/observer.slx and b/matlab/observer.slx differ diff --git a/matlab/testObserver.m b/matlab/testObserver.m new file mode 100644 index 0000000..f8c5abd --- /dev/null +++ b/matlab/testObserver.m @@ -0,0 +1,143 @@ +function [ssc]=testObserver(mot) + %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=ControlStateSpace + + A = [ 0 1 0 + 980 0 -2.8 + 0 0 -100 ]; + + B = [ 0 + 0 + 100 ]; + + C = [ 1 0 0 ]; + D=0; + + %[A,B,C,D]=ssdata(mot.ssMdl) + %C=C(3,:);D=D(3); + + Ap=A;Am=A; + Bp=B;Bm=B; + Cp=C;Cm=C; + Dp=D;Dm=D;Dt=D; + + poles = eig(A); + disp(poles); + + + t = 0:0.01:2; + u = zeros(size(t)); + x0 = [0.01 0 0]; + %x0 = [0.1 0 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)') + + 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)') + + + 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)') + + + + %discrete + Ts=1/50; % 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) + ss_tz=c2d(ss_t,Ts); + figure();pzplot(ss_tz) + [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz); + + + [Apz,Bpz,Cpz,Dpz]=ssdata(c2d(ss(Ap,Bp,Cp,Dp),Ts)); + [Amz,Bmz,Cmz,Dmz]=ssdata(c2d(ss(Am,Bm,Cm,Dm),Ts)); + + + Ao=Am-L*Cm; + Bo=[Bm L]; + Co=K; + Do=zeros(size(Co,1),size(Bo,2)); + + [Aoz,Boz,Coz,Doz]=ssdata(c2d(ss(Ao,Bo,Co,Do),Ts)); + + + %state space controller + ssc=struct(); + for k=["Ts","At","Bt","Ct","Dt","Atz","Btz","Ctz","Dtz","Ap","Bp","Cp","Dp","Am","Bm","Cm","Dm","Ao","Bo","Co","Do","Apz","Bpz","Cpz","Dpz","Aoz","Boz","Coz","Doz","V","K","L"] + ssc=setfield(ssc,k,eval(k)); + end + mdlName='testObserverSim'; + open(mdlName); + + + +end diff --git a/matlab/testObserverSim.slx b/matlab/testObserverSim.slx new file mode 100644 index 0000000..df07c9d Binary files /dev/null and b/matlab/testObserverSim.slx differ