📄 php2.m
字号:
clf;
% color setup
black = [0 0 0];
blue = [0 0 1];
green = [0 1 0];
blue2 = [0 1 1];
red = [1 0 0];
magenta = [1 0 1];
yellow = [1 1 0];
white = [1 1 1];
%----------------------------------------------------------------------%
% setup block diagram parameters %
%----------------------------------------------------------------------%
% BLOCK SCS Driveline
% BLOCKS p1p2,p1p4,p2p3 and p3p4 (rectangle)
% BLOCKS reach_zero, reach_180
% BLOCKS h000,h001,h010,h011,h100,h101,h110,h111,
%----------------------------------------------------------------------%
%Switched Continuous System driveline: %
%----------------------------------------------------------------------%
A_hat = [-0.07586965064353 0 0 0;...
0 -3.32031761672556 -25.73259491082008 0;...
0 25.73259491082008 -3.32031761672556 0;...
0 0 0 -43.48779511590537 ];
B_hat = [7.0748; 23.3824; -36.38729; 2.8608];
%B_hat = [7.68250299631948 8.46817952181634 -42.33663803344659 2.83483616620560];
T_2 = 0.0064227;
v = [0.495;-0.868];
I =[1 0;0 1];
A23 = [A_hat(2:3,2:3)];
B23 = [B_hat(2);B_hat(3)];
M = (expm(A23*T_2));
NU = inv(A23)*(M - I)*B23*10;
A_1 = inv(A23);
b001 = M*NU;
b010 = M*M*NU;
b100 = M*M*M*NU;
com000 = [0;0];
com001 = b001 + 0 + 0 ;
com010 = 0 + b010 + 0 ;
com011 = 0 + b010 + b001;
com100 = b100 + 0 + 0 ;
com101 = b100 + 0 + b001;
com110 = b100 + b010 + 0 ;
com111 = b100 + b010 + b001;
%adjust = inv([0.9 0.43;-0.43 0.9]);
adjust = 1;
VTM4 = v'*M*M*M*M*adjust;
VTNU = v'*NU;
Csimetric = [-VTM4 0];
Cdouble = [ 2*VTM4 0];
% v'*Xo for each value of comXXX:
%com000 for zero torque at x(k+4)
C000_0 = v'*com000;
C001_0 = v'*com001;
C010_0 = v'*com010;
C011_0 = v'*com011;
C100_0 = v'*com100;
C101_0 = v'*com101;
C110_0 = v'*com110;
C111_0 = v'*com111;
%offset = 1.1141;
offset = 0;
%com000 for torque at x(k+4)
C000_1 = C000_0 + VTNU + offset;
C001_1 = C001_0 + VTNU + offset;
C010_1 = C010_0 + VTNU + offset;
C011_1 = C011_0 + VTNU + offset;
C100_1 = C100_0 + VTNU + offset;
C101_1 = C101_0 + VTNU + offset;
C110_1 = C110_0 + VTNU + offset;
C111_1 = C111_0 + VTNU + offset;
e000 = -2*C000_0 - VTNU - 2*offset;
e001 = -2*C001_0 - VTNU - 2*offset;
e010 = -2*C010_0 - VTNU - 2*offset;
e011 = -2*C011_0 - VTNU - 2*offset;
e100 = -2*C100_0 - VTNU - 2*offset;
e101 = -2*C101_0 - VTNU - 2*offset;
e110 = -2*C110_0 - VTNU - 2*offset;
e111 = -2*C111_0 - VTNU - 2*offset;
%V'X0 >=0
%v'(M^4*Xk + F(M,U))>=0
%V'(M^4*Xk)>= -v'(F(M,U))
%-v'(M^4*Xk)<= v'(F(M,U))
%V'X1 >=0
%v'(M^4*Xk + F(M,U) + NU)>=0
%V'(M^4*Xk)>= -v'(F(M,U) + NU)
%-v'(M^4*Xk)<= v'(F(M,U) + NU)
% V'*X0 <= -V'X1
% V'(M^4*Xk + F(M,U))<= -V'*(M^4*Xk + F(M,U) + NU)
% 2*V'(M^4*Xk) <= -2v'*F(M,U)) - V'*NU
% Matrix C = 2*V'*M^4
% vector d = -2F(M,U)) - V'*NU = exxx
%----------------------------------------------------------------------------------------%
% CALCULATING RETANGLES VALUES %
%----------------------------------------------------------------------------------------%
pi1 = [6.04;12.03];
pi2 = [10.34;8.62];
pi3 = [ 4.82;1.67];
pi4 = [ 0.53;5.07];
%scl = [1 0 0 0;0 0.9 0.43 0; 0 -0.43 0.9 0;0 0 0 1];
scl =eye(4);
p1=scl(2:3,2:3)*pi1;
p2=scl(2:3,2:3)*pi2;
p3=scl(2:3,2:3)*pi3;
p4=scl(2:3,2:3)*pi4;
% normal vector v1 and distance d1 for line p3_p4
a1 = (p3(2) - p4(2))/(p3(1) - p4(1));
b1 = -a1*p3(1) + p3(2);
norm1 = sqrt(a1*a1 +1);
a1_norm = -a1/norm1;
d1 = b1/norm1;
v1(1) = a1_norm;
v1(2) = 1/norm1;
v1(3) = 0;
% normal vector and v2 distance d2 for line p2_p1
a2 = (p2(2) - p1(2))/(p2(1) - p1(1));
b2 = -a2*p2(1) + p2(2);
norm2 = sqrt(a2*a2 +1);
a2_norm = -a2/norm2;
d2 = b2/norm2;
v2(1) = a2_norm;
v2(2) = 1/norm2;
v2(3) = 0;
% normal vector and v3 distance d3 for line p1_p4
a3 = (p1(2) - p4(2))/(p1(1) - p4(1));
b3 = -a3*p1(1) + p1(2);
norm3 = sqrt(a3*a3 +1);
a3_norm = -a3/norm3;
d3 = b3/norm3;
v3(1) = a3_norm;
v3(2) = 1/norm3;
v3(3) = 0;
% normal vector and v4 distance d4 for line p2_p3
a4 = (p2(2) - p3(2))/(p2(1) - p3(1));
b4 = -a4*p2(1) + p2(2);
norm4 = sqrt(a4*a4 +1);
a4_norm = -a4/norm4;
d4 = b4/norm4;
v4(1) = a4_norm;
v4(2) = 1/norm4;
v4(3) = 0;
%d1 = 4.30261716695015;
%d2 = 13.17883615718490;
%d3 = 2.73141964096866;
%d4 = -2.73571591748454;
%HYPERPLANES p3p4, p1p2, p1p4 and p2p3:
%----------------------------------------------------------------------%
% setup block diagram parameters %
%----------------------------------------------------------------------%
% BLOCK SCS Driveline
% BLOCKS p1p2,p1p4,p2p3 and p3p4 (rectangle)
% BLOCKS reach_zero, reach_180
% BLOCKS h0001,h0002,h0003,h0011,h0012,h0013,h0101,h0102,
% h0103,h0111,h0112,h0113,h1001,h1002,h1003,h1011,
% h1012,h1013,h1101,h1102,h1103,h1111,h1112,h1113
%----------------------------------------------------------------------%
%Switched Continuous System driveline: %
%----------------------------------------------------------------------%
pi1 = [6.04;12.03];
pi2 = [10.34;8.62];
pi3 = [ 4.82;1.67];
pi4 = [ 0.53;5.07];
cxmax = max([pi1(1),pi2(1),pi3(1),pi4(1)]);
cxmin = min([pi1(1),pi2(1),pi3(1),pi4(1)]);
cymax = max([pi1(2),pi2(2),pi3(2),pi4(2)]);
cymin = min([pi1(2),pi2(2),pi3(2),pi4(2)]);
A = [1 0 0;-1 0 0;0 1 0; 0 -1 0];
b = [cxmax;-cxmin;cymax;-cymin];
A_hyp = [v2;-v1;v3;-v4];
b_hyp = [d2;-d1;d3;-d4];
in1 = [6.6;6.9];
in2 = [9.9;10.4];
in1a = scl(2:3,2:3)*[in1(1);in2(1)];
in1b = scl(2:3,2:3)*[in1(1);in2(2)];
in1c = scl(2:3,2:3)*[in1(2);in2(1)];
in1d = scl(2:3,2:3)*[in1(2);in2(2)];
cxmax = max([in1a(1),in1b(1),in1c(1),in1d(1)]);
cxmin = min([in1a(1),in1b(1),in1c(1),in1d(1)]);
cymax = max([in1a(2),in1b(2),in1c(2),in1d(2)]);
cymin = min([in1a(2),in1b(2),in1c(2),in1d(2)]);
ICS_CE = [0 0 1];
ICS_dE = 0;
ICS_CI = [1 0 0; -1 0 0; 0 1 0; 0 -1 0];
ICS_dI = [cxmax;-cxmin;cymax;-cymin];
IC =linearcon(ICS_CE,ICS_dE,ICS_CI,ICS_dI);
factor = 0.1;
AR_C1 = [v2;0 0 1];
AR_d1 = [d2;0];
AR_C2 = [v1;0 0 1];
AR_d2 =[d1-abs(d1)*factor;0];
AR_C3 = [v3;0 0 1];
AR_d3 = [d3;0];
AR_C4 = [v4;0 0 1];
AR_d4 = [d4;0];
AR1 = linearcon(AR_C1,AR_d1,A,b);
AR2 = linearcon(AR_C2,AR_d2,A,b);
AR3 = linearcon(AR_C3,AR_d3,A,b);
AR4 = linearcon(AR_C4,AR_d4,A,b);
%-------------------------------------------------------------------------%
%HYPERPLANE V (decision line for chattering %
%-------------------------------------------------------------------------%
%hyperplane v
aux = scl(2:4,2:4)*[0.495 -0.868 0]';
C = [aux';0 0 1];
d = [0;0];
v = linearcon(C,d,A,b);
%hyperplane gama
aux = scl(2:4,2:4)*[0.7835 -0.621 0]';
C = [aux';0 0 1];
d = [0;0];
gama = linearcon(C,d,A,b);
%-------------------------------------------------------------------------%
%HYPERPLANES p3p4, p1p2, p1p4 and p2p3: %
%-------------------------------------------------------------------------%
%hyperplane p3p4
C = [v1;0 0 1];
d = [d1;0];
p3p4 = linearcon(C,d,A,b);
%-------------------------------------------------------------------------%
% BLOCKS h0001,h0002,h0003,h0011,h0012,h0013,h0101,h0102,
% h0103,h0111,h0112,h0113,h1001,h1002,h1003,h1011,
% h1012,h1013,h1101,h1102,h1103,h1111,h1112,h1113
%-------------------------------------------------------------------------%
%BASIC SETUP :
C1 = Csimetric;
C3 = [Cdouble;0 0 1];
d0001 = C000_0;
d0002 = C000_1;
d0003 = e000 ;
d0011 = C001_0;
d0012 = C001_1;
d0013 = e001 ;
d0101 = C010_0;
d0102 = C010_1;
d0103 = e010 ;
d0111 = C011_0;
d0112 = C011_1;
d0113 = e011 ;
d1001 = C100_0;
d1002 = C100_1;
d1003 = e100 ;
d1011 = C101_0;
d1012 = C101_1;
d1013 = e101 ;
d1101 = C110_0;
d1102 = C110_1;
d1103 = e110 ;
d1111 = C111_0;
d1112 = C111_1;
d1113 = e111 ;
%HYPERPLANES h000,h001,h010,h011,h100,h101,h110,h111:
h000 = linearcon(C3,[d0003;0],A,[6;-2;4;0]);
h001 = linearcon(C3,[d0013;0],A_hyp,b_hyp);
h010 = linearcon(C3,[d0103;0],A_hyp,b_hyp);
h011 = linearcon(C3,[d0113;0],A_hyp,b_hyp);
h100 = linearcon(C3,[d1003;0],A_hyp,b_hyp);
h101 = linearcon(C3,[d1013;0],A_hyp,b_hyp);
h110 = linearcon(C3,[d1103;0],A_hyp,b_hyp);
h111 = linearcon(C3,[d1113;0],A_hyp,b_hyp);
fprintf('Drawing Analysis region... ');
plot(AR1,red);
plot(AR2,red);
plot(AR3,red);
plot(AR4,red);
fprintf('done! \n');
fprintf('Drawing retangle of interest... ');
plot(p3p4,blue);
fprintf('done! \n');
fprintf('Drawing Initial conditions... ');
plot(IC,blue);
fprintf('done! \n');
fprintf('Drawing Hyperplanes ... ');
plot(h000,black);
plot(h001,black);
plot(h010,black);
plot(h011,black);
plot(h100,black);
plot(h101,black);
plot(h110,black);
plot(h111,black);
fprintf('done! \n');
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -