?? php.m
字號(hào):
function php(input)
% 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 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: %
%----------------------------------------------------------------------%
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; 26.3824; -34.9729; 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;
VTM4 = v'*M*M*M*M;
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: %
%----------------------------------------------------------------------%
A = [1 0 0;-1 0 0;0 1 0; 0 -1 0;0 0 1;0 0 -1];
b = [15;1;15;1;1;0];
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;
AR_d1 = d2+abs(d2)*factor;
AR_C2 = v1;
AR_d2 =d1-abs(d1)*factor;
AR_C3 = v3;
AR_d3 = d3+abs(d3)*factor;
AR_C4 = v4;
AR_d4 = d4-abs(d4)*factor;
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 %
%-------------------------------------------------------------------------%
A = [1 0 0;-1 0 0;0 1 0; 0 -1 0;0 0 1;0 0 -1];
b = [15;1;15;1;1;0];
%hyperplane v
aux = scl(2:4,2:4)*[0.495 -0.868 0]';
C = aux';
d = 0;
v = linearcon(C,d,A,b);
%hyperplane v
aux = scl(2:4,2:4)*[0.7835 -0.621 0]';
C = aux';
d = 0;
gama = linearcon(C,d,A,b);
%-------------------------------------------------------------------------%
%HYPERPLANES p3p4, p1p2, p1p4 and p2p3: %
%-------------------------------------------------------------------------%
A = [1 0 0;-1 0 0;0 1 0; 0 -1 0;0 0 1;0 0 -1];
b = [15;1;15;1;1;0];
%hyperplane p1p2
C = v2;
d = d2;
p1p2 = linearcon(C,d,A,b);
%hyperplane p3p4
C = v1;
d = d1;
p3p4 = linearcon(C,d,A,b);
%hyperplane p1p4
C = v3;
d = d3;
p1p4 = linearcon(C,d,A,b);
%hyperplane p2p3
C = v4;
d = d4;
p2p3 =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;
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 h0001,h0002 and h0003:
%hyperplane h0001
h0001 = linearcon(C1,d0001,A,b);
%hyperplane h0002
h0002 = linearcon(C1,d0002,A,b);
%hyperplane h0003
h0003 = linearcon(C3,d0003,A,b);
%HYPERPLANES h0011,h0012 and h0013:
%hyperplane h0011
h0011 = linearcon(C1,d0011,A,b);
%hyperplane h0012
h0012 = linearcon(C1,d0012,A,b);
%hyperplane h0013
h0013 = linearcon(C3,d0013,A,b);
%HYPERPLANES h0101,h0102 and h0103:
%hyperplane h0101
h0101 = linearcon(C1,d0101,A,b);
%hyperplane h0102
h0102 = linearcon(C1,d0102,A,b);
%hyperplane h0103
h0103 = linearcon(C3,d0103,A,b);
%HYPERPLANES h0111,h0112 and h0113:
%hyperplane h0111
h0111 = linearcon(C1,d0111,A,b);
%hyperplane h0112
h0112 = linearcon(C1,d0112,A,b);
%hyperplane h0113
h0113 = linearcon(C3,d0113,A,b);
%HYPERPLANES h1001,h1002 and h1003:
%hyperplane h1001
h1001 = linearcon(C1,d1001,A,b);
%hyperplane h1002
h1002 = linearcon(C1,d1002,A,b);
%hyperplane h1003
h1003 = linearcon(C3,d1003,A,b);
%HYPERPLANES h1011,h1012 and h1013:
%hyperplane h1011
h1011 = linearcon(C1,d1011,A,b);
%hyperplane h1012
h1012 = linearcon(C1,d1012,A,b);
%hyperplane h1013
h1013 = linearcon(C3,d1013,A,b);
%HYPERPLANES h1101,h1102 and h1103:
%hyperplane h1101
h1101 = linearcon(C1,d1101,A,b);
%hyperplane h1102
h1102 = linearcon(C1,d1102,A,b);
%hyperplane h1103
h1103 = linearcon(C3,d1103,A,b);
%HYPERPLANES h1111,h1112 and h1113:
%hyperplane h1111
h1111 = linearcon(C1,d1111,A,b);
%hyperplane h1112
h1112 = linearcon(C1,d1112,A,b);
%hyperplane h1113
h1113 = linearcon(C3,d1113,A,b);
fprintf('Drawing Analysis region... ');
plot(AR1,white);
plot(AR2,white);
plot(AR3,white);
plot(AR4,white);
fprintf('done! \n');
pause
fprintf('Drawing retangle of interest... ');
plot(p1p2,magenta);
plot(p2p3,magenta);
plot(p3p4,magenta);
plot(p1p4,magenta);
fprintf('done! \n');
pause;
fprintf('Drawing Initial conditions... ');
plot(IC,white);
fprintf('done! \n');
pause;
fprintf('Drawing decision line... ');
plot(v,green);
fprintf('done! \n');
pause;
fprintf('Drawing gama... ');
plot(gama,green);
fprintf('done! \n');
pause;
fprintf('Drawing Hyperplanes h0001,h0002 and h0003... ');
plot(h0001,yellow);
plot(h0002,yellow);
plot(h0003,green);
fprintf('done! \n');
pause;
fprintf('Drawing Hyperplanes h0011,h0012 and h0013... ');
plot(h0011,yellow);
plot(h0012,yellow);
plot(h0013,green);
fprintf('done! \n');
pause;
fprintf('Drawing Hyperplanes h0101,h0102 and h0103... ');
plot(h0101,yellow);
plot(h0102,yellow);
plot(h0103,green);
fprintf('done! \n');
pause;
fprintf('Drawing Hyperplanes h0111,h0112 and h0113... ');
plot(h0111,yellow);
plot(h0112,yellow);
plot(h0113,green);
fprintf('done! \n');
pause;
fprintf('Drawing Hyperplanes h1001,h1002 and h1003... ');
plot(h1001,yellow);
plot(h1002,yellow);
plot(h1003,green);
fprintf('done! \n');
pause;
fprintf('Drawing Hyperplanes h1011,h1012 and h1013... ');
plot(h1011,yellow);
plot(h1012,yellow);
plot(h1013,green);
fprintf('done! \n');
pause;
fprintf('Drawing Hyperplanes h1101,h1102 and h1103... ');
plot(h1101,yellow);
plot(h1102,yellow);
plot(h1103,green);
fprintf('done! \n');
pause;
fprintf('Drawing Hyperplanes h1111,h1112 and h1113... ');
plot(h1111,yellow);
plot(h1112,yellow);
plot(h1113,green);
fprintf('done! \n');
pause;
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -