function [ssc]=StateSpaceControlDesign(mot) % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!! % % builds a state space controller designed for the plant. % shows step answers of open and closed loop, also for the observer controller and the final discrete observer % % finally it opens a simulink observer file for testing %References: %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=ControlStateSpace %space state controller: % web(fullfile(docroot, 'simulink/examples.html')) % web(fullfile(docroot, 'simulink/examples/inverted-pendulum-with-animation.html')) % web(fullfile(docroot, 'simulink/examples/double-spring-mass-system.html')) % % https://www.youtube.com/watch?v=Lax3etc837U ssPlt=mot.ssPlt;%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'; [Ap,Bp,Cp,Dp]=ssdata(ssPlt); [Am,Bm,Cm,Dm]=ssdata(ssMdl); %sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only figure();h=bodeplot(ssPlt,ssMdl); setoptions(h,'IOGrouping','all') % step answer on open loop: t = 0:1E-4:.5; u = ones(size(t)); xp0 = zeros(1,length(Ap)); xm0 = zeros(1,length(Am)); [yp,t,x] = lsim(ssPlt,u,t,xp0); [ym,t,x] = lsim(ssMdl,u,t,xm0); figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)'); legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos') poles = eig(Am); %w0=abs(poles); %ang=angle(-poles); %------------------- %p=w0.*exp(j.*ang) % *** space state controller *** % %place poles for the controller feedback use_lqr=0; if use_lqr %use the lqr controller Q=eye(length(ssMdl.A)); R=1; [K,P,E]=lqr(ssMdl,Q,R,0); else if mot.id==1 %2500rad/s = 397Hz -> locate poles here %6300rad/s = 1027Hz -> locate poles here if length(poles)==4 p1=-6300+280i; p2=-6200+150i; P=[p1 p1' p2 p2']; P=[-4100 -4000 -1500+10j -1500-10j]; else p1=-3300+2800i; p2=-2700+500i; p3=-2500+10i; %p1=-3300+2800i; %p2=-1500+500i; %p3=-1200+10i; P=[p1 p1' p2 p2' p3 p3']; end else %2500rad/s = 397Hz -> locate poles here %6300rad/s = 1027Hz -> locate poles here if length(poles)==4 p1=-6300+2800i; p2=-6200+1500i; P=[p1 p1' p2 p2']; P=[-2500 -2800 -1500+10j -1500-10j]; else p1=-3300+2800i; p2=-1900+130i; p3=-2900+80i; p4=-2300+450i; p5=-2000+20i; p6=-1500+10i; %p1=-3300+2800i; %p2=-1500+500i; %p3=-1200+10i; P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6']; end end %P=P*.1; % P was too aggressive K = place(Am,Bm,P); %K = acker(Am,Bm,Pm); end %if lqr V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) %Nbar(2)=1; %the voltage stuff is crap for now if length(V)>1 V=V(3); % only the position scaling needed end %prefilter to compensate non observable resonance frequencies numV=mot.prefilt.Numerator{1}; denV=mot.prefilt.Denominator{1}; % step answer on closed loop with space state controller: t = 0:1E-4:.5; ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',mot.ssMdl.InputName,'OutputName',mot.ssMdl.OutputName); [y,t,x]=lsim(ss_cl,V*u,t,xm0); figure();plot(t,y);title('step on closed loop'); % *** observer controller *** % %observer poles-> 5 times farther left than system poles OP=2*P; L=place(Am',Cm',OP)'; %L=acker(A',C',OP)'; At = [ Am-Bm*K Bm*K zeros(size(Am)) Am-L*Cm ]; Bt = [ Bm*V zeros(size(Bm)) ]; Ct = [ Cm zeros(size(Cm)) ]; Dt=0; % step answer on closed loop with observer controller: ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName); figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer'); % *** disctrete observer controller *** % Ts=1/5000; % 5kHz ss_tz = c2d(ss_t,Ts); [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz ); 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'); %plot all bode diagrams of desPos->actPos figure(); h=bodeplot(ss_cl(3),ss_t(3),ss_tz(3)); setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw'); figure(); h=pzplot(ss_cl(3),ss_t(3),ss_tz(3)); setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw'); %calculate matrices for the simulink system Ao=Am-L*Cm; Bo=[Bm L]; Co=K; Do=zeros(size(Co,1),size(Bo,2)); ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'}); %discrete plant ssPltz = c2d(ssPlt,Ts); [Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz); %discrete observer controller ss_oz = c2d(ss_o,Ts); [Aoz,Boz,Coz,Doz]=ssdata(ss_oz); mdlName='observer'; %open(mdlName); %discrete prefilter prefiltz=c2d(mot.prefilt,Ts); numVz=prefiltz.Numerator{1}; denVz=prefiltz.Denominator{1}; %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","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"] ssc=setfield(ssc,k,eval(k)); end save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc'); end %code snipplets from an example on youtube (see reference at top) function SCRATCH() %import numpy as np %fh=np.load('mode1.npz') %import scipy.io %scipy.io.savemat('mode1.mat',fh,do_compression=True) %matlab: load('mode1.mat'); plot(pts(:,1),pts(:,2),'.');hold on; plot(rec(:,5),rec(:,6),'-');%despos plot(rec(:,2),rec(:,3),'-');%actPos %sig.time = [0 1 1 5 5 8 8 10]; %sig.signals.values = [0 0 2 2 2 3 3 3]'; %sig.signals.dimensions = 1; sig.time=0:2E-4:(length(rec)-1)*2E-4; sig.signals.values=rec(:,5); sig.signals.dimensions = 1; sum(desPos_actPos.Data(:,1)-desPos_actPos.Data(:,2)) A = [ 0 1 0 980 0 -2.8 0 0 -100 ]; B = [ 0 0 100 ]; C = [ 1 0 0 ]; poles = eig(A) t = 0:0.01:2; u = zeros(size(t)); x0 = [0.01 0 0]; sys = ss(A,B,C,0); [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]) Nbar = rscale(sys,K) lsim(sys_cl,Nbar*u,t) title('Linear Simulation Results (with Nbar)') xlabel('Time (sec)') ylabel('Ball Position (m)') axis([0 2 0 1.2*10^-3]) end