sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

could be first stable observer of motor1

Browse Source
This commit is contained in:
zamofing_t
2018-11-12 11:22:30 +01:00
parent 408e3797e9
commit 4927a02706
3 changed files with 157 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
matlab/StateSpaceControlDesign.m
View File

@@ -63,6 +63,7 @@ function [ssc]=StateSpaceControlDesign(mot)
p1=-6300+280i; p1=-6300+280i;
p2=-6200+150i; p2=-6200+150i;
P=[p1 p1' p2 p2']; P=[p1 p1' p2 p2'];
P=[-4000 -4100 -1500+10j -1500-10j];
else else
p1=-3300+2800i; p1=-3300+2800i;
p2=-2700+500i; p2=-2700+500i;

223
matlab/testObserver.m
View File

@@ -1,19 +1,50 @@
function [ssc]=testObserver(mot) function [ssc]=testObserver(mot)
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=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 B = [ 0
980 0 -2.8 0
0 0 -100 ]; 100 ];
B = [ 0 C = [ 1 0 0 ];
0 D=0;
100 ]; 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 ]; g1=tf(numc,denc); % iqCmd->iqMeas
D=0; 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);
%[A,B,C,D]=ssdata(mot.ssMdl) %d=.77;w0=9100; %1.44kHz -> T0=6.9046e-04
%C=C(3,:);D=D(3); %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; Ap=A;Am=A;
Bp=B;Bm=B; Bp=B;Bm=B;
@@ -24,97 +55,133 @@ function [ssc]=testObserver(mot)
disp(poles); disp(poles);
t = 0:0.01:2; if mdl==0
u = zeros(size(t)); t = 0:0.01:2;
x0 = [0.01 0 0]; u = zeros(size(t));
%x0 = [0.1 0 0 0]; 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); [y,t,x] = lsim(sys,u,t,x0);
plot(t,y) plot(t,y)
title('Open-Loop Response to Non-Zero Initial Condition') title('Open-Loop Response to Non-Zero Initial Condition')
xlabel('Time (sec)') xlabel('Time (sec)')
ylabel('Ball Position (m)') ylabel('Ball Position (m)')
p1 = -10 + 10i; p2 = -10 - 10i; p3 = -50; p1 = -10 + 10i; p2 = -10 - 10i; p3 = -50;
K = place(A,B,[p1 p2 p3]); K = place(A,B,[p1 p2 p3]);
sys_cl = ss(A-B*K,B,C,0); sys_cl = ss(A-B*K,B,C,0);
lsim(sys_cl,u,t,x0); lsim(sys_cl,u,t,x0);
xlabel('Time (sec)') xlabel('Time (sec)')
ylabel('Ball Position (m)') ylabel('Ball Position (m)')
p1 = -20 + 20i; p2 = -20 - 20i; p3 = -100; p1 = -20 + 20i; p2 = -20 - 20i; p3 = -100;
K = place(A,B,[p1 p2 p3]); P=[p1 p2 p3];
sys_cl = ss(A-B*K,B,C,0); K = place(A,B,P);
lsim(sys_cl,u,t,x0); sys_cl = ss(A-B*K,B,C,0);
xlabel('Time (sec)') lsim(sys_cl,u,t,x0);
ylabel('Ball Position (m)') xlabel('Time (sec)')
ylabel('Ball Position (m)')
t = 0:0.01:2;
u = 0.001*ones(size(t));
t = 0:0.01:2; sys_cl = ss(A-B*K,B,C,0);
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])
lsim(sys_cl,u,t); V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
xlabel('Time (sec)') lsim(sys_cl,V*u,t)
ylabel('Ball Position (m)') title('Linear Simulation Results (with Nbar)')
axis([0 2 -4E-6 0]) 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);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) lsim(sys_cl,u,t,x0);
lsim(sys_cl,V*u,t) xlabel('Time (sec)')
title('Linear Simulation Results (with Nbar)') ylabel('Motor Position (m)')
xlabel('Time (sec)') axis([0 2 0 1.2E-3])
ylabel('Ball Position (m)') end
axis([0 2 0 1.2*10^-3])
op1 = -100;
op2 = -101;
op3 = -102;
L = place(A',C',[op1 op2 op3])'; 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)) ];
At = [ A-B*K B*K sys = ss(At,Bt,Ct,0);
zeros(size(A)) A-L*C ]; %lsim(sys,zeros(size(t)),t,[x0 x0]);
Bt = [ B*V lsim(sys,u,t,[x0 x0]*0);
zeros(size(B)) ]; title('Linear Simulation Results (with observer)')
Ct = [ C zeros(size(C)) ]; 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); 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('Motor Position (m)')
axis([0 2 0 1.2E-3])
end
title('Linear Simulation Results (with observer)') if mdl==0
xlabel('Time (sec)') t = 0:1E-6:0.1;
ylabel('Ball Position (m)') 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;
t = 0:1E-6:0.1; % Save state variables explicitly to aid in plotting
x0 = [0.01 0.5 -5]; h = x(:,1); h_dot = x(:,2); i = x(:,3);
[y,t,x] = lsim(sys,zeros(size(t)),t,[x0 x0]); h_est = x_est(:,1); h_dot_est = x_est(:,2); i_est = x_est(:,3);
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)')
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 %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 %The discrete system works with sampling >100Hz,. the bigger the frequency the better the result
%With sampling = 80Hz hte system already becomes instable. %With sampling = 80Hz hte system already becomes instable.
ss_t=ss(At,Bt,Ct,Dt); ss_t=ss(At,Bt,Ct,Dt);
figure();pzplot(ss_t) %figure();pzplot(ss_t)
ss_tz=c2d(ss_t,Ts); ss_tz=c2d(ss_t,Ts);
figure();pzplot(ss_tz) %figure();pzplot(ss_tz)
[Atz,Btz,Ctz,Dtz]=ssdata(ss_tz); [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz);

BIN
matlab/testObserverSim.slx
View File

Binary file not shown.