php.m

一个matlab的将军模型

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;