Teljes Matlab script kiegészítő függvényekkel.
File: d2018_01_31_K_prelim_L_codesign_v5_rhofuggoBL_min.m Directory: projects/3_outsel/2017_11_13_lpv_passivity Author: Peter Polcz (ppolcz@gmail.com)
Created on 2018. January 31.
global VERBOSE SCOPE_DEPTH
VERBOSE = 1;
SCOPE_DEPTH = -1;
s = tf('s');
H = @(s) [
(s-1)/(s-2)/(s+1) 1/(s+3)/(s-0.1)
(s-7)/(s+1)/(s+5) (s-6)/(s^2+5*s+6)
];
sys = minreal( ss( H(s) ) );
[A0,B0,C,D] = deal(sys.a, sys.b, sys.c, sys.d);
tol = 1e-10;
prec = -log10(tol);
A0 = round(A0,prec);
B0 = round(B0,prec);
C = round(C,prec);
D = round(D,prec);
[POLES,ZEROS] = pzmap(sys)
2 states removed.
POLES =
-5.0000
-1.0000
2.0000
-3.0000
-2.0000
0.1000
ZEROS =
5.8894 + 0.0000i
-4.3164 + 0.0000i
0.7635 + 0.7978i
0.7635 - 0.7978i
rho_lim = [
-1 1
];
rho_rand = @(varargin) rand(varargin{:})*(rho_lim(2) - rho_lim(1)) + rho_lim(1);
A1 = A0;
A1(abs(A0) < 1) = 0;
A1 = A1 .* randn(size(A1))/10;
B1 = [ B0(:,2)/2 B0(:,2)*0 ];
% _fh: `funcion handle`
A_fh = @(rho) A0 + rho*A1;
B_fh = @(rho) B0 + rho*B1;
% Dimenziok
n_x = size(A0,1)
n_u = size(B0,2)
n_y = size(C,1)
n_r = n_u
n_yp = n_r
n_x =
6
n_u =
2
n_y =
2
n_r =
2
n_yp =
2
Kvadratikus stabilitas
Q = sdpvar(n_x);
N = sdpvar(n_u,n_x,'full');
Big_M = @(rho) A_fh(rho)'*Q + Q*A_fh(rho) - B_fh(rho)*N - N'*B_fh(rho)';
I = eye(n_x);
Constraints = [
Q - I >= 0
Big_M(rho_lim(1)) + 10*I <= 0
Big_M(rho_lim(2)) + 10*I <= 0
];
optimize(Constraints)
N = value(N);
Q = value(Q);
K = N/Q;
G = eye(n_u);
% for rho = rho_lim(1):0.25:rho_lim(2)
% eig(A_fh(rho) - B_fh(rho)*K)
% end
Problem
Name :
Objective sense : min
Type : CONIC (conic optimization problem)
Constraints : 33
Cones : 0
Scalar variables : 0
Matrix variables : 3
Integer variables : 0
Optimizer started.
Presolve started.
Linear dependency checker started.
Linear dependency checker terminated.
Eliminator - tries : 0 time : 0.00
Lin. dep. - tries : 1 time : 0.00
Lin. dep. - number : 0
Presolve terminated. Time: 0.00
Problem
Name :
Objective sense : min
Type : CONIC (conic optimization problem)
Constraints : 33
Cones : 0
Scalar variables : 0
Matrix variables : 3
Integer variables : 0
Optimizer - threads : 4
Optimizer - solved problem : the primal
Optimizer - Constraints : 33
Optimizer - Cones : 0
Optimizer - Scalar variables : 0 conic : 0
Optimizer - Semi-definite variables: 3 scalarized : 63
Factor - setup time : 0.00 dense det. time : 0.00
Factor - ML order time : 0.00 GP order time : 0.00
Factor - nonzeros before factor : 561 after factor : 561
Factor - dense dim. : 0 flops : 5.11e+04
ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME
0 2.4e+00 6.0e+00 6.2e+01 0.00e+00 -2.520000000e+02 0.000000000e+00 1.0e+00 0.00
1 3.5e-01 8.8e-01 1.8e+00 -1.27e+00 -7.743281494e+02 0.000000000e+00 1.5e-01 0.01
2 5.7e-02 1.5e-01 3.7e-01 -1.30e+00 -2.047512950e+02 0.000000000e+00 2.4e-02 0.01
3 4.9e-04 1.2e-03 5.7e-02 8.45e-01 -1.462405612e+00 0.000000000e+00 2.1e-04 0.01
4 8.1e-13 2.1e-12 2.1e-11 9.96e-01 -3.200525853e-09 0.000000000e+00 3.4e-13 0.01
Optimizer terminated. Time: 0.01
Interior-point solution summary
Problem status : PRIMAL_AND_DUAL_FEASIBLE
Solution status : OPTIMAL
Primal. obj: -3.2005258526e-09 nrm: 8e-11 Viol. con: 4e-10 barvar: 1e-17
Dual. obj: 0.0000000000e+00 nrm: 1e+02 Viol. con: 0e+00 barvar: 1e-10
Optimizer summary
Optimizer - time: 0.01
Interior-point - iterations : 4 time: 0.01
Basis identification - time: 0.00
Primal - iterations : 0 time: 0.00
Dual - iterations : 0 time: 0.00
Clean primal - iterations : 0 time: 0.00
Clean dual - iterations : 0 time: 0.00
Simplex - time: 0.00
Primal simplex - iterations : 0 time: 0.00
Dual simplex - iterations : 0 time: 0.00
Mixed integer - relaxations: 0 time: 0.00
ans =
struct with fields:
yalmiptime: 0.2253
solvertime: 0.0204
info: 'Successfully solved (MOSEK)'
problem: 0
C1 = sdpvar([n_yp n_yp], [n_x n_x],'full');
D1 = sdpvar([n_yp n_yp], [n_y n_y],'full');
Q = sdpvar(n_x,n_x,'symmetric');
S = sdpvar(n_x,n_x,'symmetric');
N = sdpvar([n_x n_x],[n_y n_y],'full');
P = blkdiag(Q,S);
C1_fh = @(rho) C1{1} + rho*C1{2};
D1_fh = @(rho) D1{1} + rho*D1{2};
N_fh = @(rho) N{1} + rho*N{2};
% Open loop matrices
Bo_fh = @(rho) [
B_fh(rho)
zeros(n_x,n_u)
];
Co_fh = @(rho) [
D1_fh(rho)*C+C1_fh(rho) -C1_fh(rho)
];
% Closed loop matrices
Bc_fh = @(rho) Bo_fh(rho)*G;
AcP_PAc_fh = @(rho) [
Q*(A_fh(rho)-B_fh(rho)*K) + (A_fh(rho)-B_fh(rho)*K)'*Q , Q*B_fh(rho)*K
K'*B_fh(rho)'*Q , S*A_fh(rho) + A_fh(rho)'*S - N_fh(rho)*C - C'*N_fh(rho)'
];
W = eye(n_yp);
Lambda2 = @(rho) [
AcP_PAc_fh(rho) , P*Bc_fh(rho)-Co_fh(rho)' , Co_fh(rho)'
Bc_fh(rho)'*P-Co_fh(rho) , zeros(n_r,n_r) , zeros(n_r,n_yp)
Co_fh(rho) , zeros(n_yp,n_r) , -inv(W)
];
Constraints = [
Lambda2(rho_lim(1)) <= 0
Lambda2(rho_lim(2)) <= 0
P - 0.0001*eye(size(P)) >= 0
]
++++++++++++++++++++++++++++++++++ | ID| Constraint| ++++++++++++++++++++++++++++++++++ | #1| Matrix inequality 16x16| | #2| Matrix inequality 16x16| | #3| Matrix inequality 12x12| ++++++++++++++++++++++++++++++++++
sdpopts = sdpsettings('solver','sedumi');
optimize(Constraints,[],sdpopts)
check(Constraints)
The coefficient matrix is not full row rank, numerical problems may occur.
SeDuMi 1.3 by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.
Alg = 2: xz-corrector, theta = 0.250, beta = 0.500
eqs m = 98, order n = 45, dim = 657, blocks = 4
nnz(A) = 1962 + 0, nnz(ADA) = 9604, nnz(L) = 4851
it : b*y gap delta rate t/tP* t/tD* feas cg cg prec
0 : 3.12E+01 0.000
1 : 0.00E+00 8.26E+00 0.000 0.2647 0.9000 0.9000 2.00 1 1 8.9E+00
2 : 0.00E+00 2.86E+00 0.000 0.3461 0.9000 0.9000 1.26 1 1 3.2E+00
3 : 0.00E+00 1.24E+00 0.000 0.4338 0.9000 0.9000 1.08 1 1 1.7E+00
4 : 0.00E+00 6.00E-01 0.000 0.4843 0.9000 0.9000 1.05 1 1 1.1E+00
5 : 0.00E+00 1.93E-01 0.000 0.3219 0.9000 0.9000 1.01 1 1 7.5E-01
6 : 0.00E+00 7.75E-02 0.000 0.4010 0.9000 0.9000 1.00 1 1 6.5E-01
7 : 0.00E+00 3.47E-02 0.000 0.4474 0.9000 0.9000 0.99 1 1 6.2E-01
8 : 0.00E+00 1.20E-02 0.000 0.3460 0.9000 0.9000 0.98 1 1 6.1E-01
9 : 0.00E+00 4.86E-03 0.000 0.4051 0.9000 0.9000 0.96 1 1 2.7E-01
10 : 0.00E+00 1.68E-03 0.000 0.3464 0.9000 0.9000 0.96 1 1 9.8E-02
11 : 0.00E+00 6.59E-04 0.000 0.3917 0.9000 0.9000 0.93 1 1 4.1E-02
12 : 0.00E+00 2.30E-04 0.000 0.3484 0.9000 0.9000 0.90 1 1 1.6E-02
13 : 0.00E+00 9.18E-05 0.000 0.3997 0.9000 0.9000 0.83 1 1 7.4E-03
14 : 0.00E+00 3.18E-05 0.000 0.3459 0.9000 0.9000 0.74 1 1 3.2E-03
15 : 0.00E+00 1.27E-05 0.000 0.3994 0.9000 0.9000 0.57 1 1 1.8E-03
16 : 0.00E+00 4.32E-06 0.000 0.3408 0.9000 0.9000 0.41 1 1 8.7E-04
17 : 0.00E+00 1.68E-06 0.000 0.3885 0.9000 0.9000 0.24 1 1 2.8E-04
18 : 0.00E+00 5.66E-07 0.000 0.3370 0.9000 0.9000 0.16 1 1 3.1E-06
19 : 0.00E+00 2.15E-07 0.000 0.3807 0.9000 0.9000 0.08 1 1 1.9E-06
20 : 0.00E+00 7.25E-08 0.000 0.3366 0.9000 0.9000 0.06 1 1 1.0E-06
21 : 0.00E+00 2.75E-08 0.000 0.3794 0.9000 0.9000 0.02 1 1 6.7E-07
22 : 0.00E+00 9.34E-09 0.000 0.3393 0.9000 0.9000 0.03 1 1 3.7E-07
23 : 0.00E+00 3.56E-09 0.000 0.3817 0.9000 0.9000 -0.01 1 1 2.4E-07
24 : 0.00E+00 1.22E-09 0.000 0.3416 0.9000 0.9000 0.02 1 1 1.3E-07
25 : 0.00E+00 4.68E-10 0.000 0.3844 0.9000 0.9000 -0.01 1 1 8.6E-08
26 : 0.00E+00 1.60E-10 0.000 0.3419 0.9000 0.9000 0.01 1 1 4.8E-08
27 : 0.00E+00 6.16E-11 0.000 0.3850 0.9000 0.9000 -0.02 1 2 3.1E-08
28 : 0.00E+00 2.10E-11 0.000 0.3408 0.9000 0.9000 0.01 1 2 1.8E-08
29 : 0.00E+00 8.06E-12 0.000 0.3838 0.9000 0.9000 -0.02 2 2 1.1E-08
30 : 0.00E+00 2.74E-12 0.000 0.3399 0.9000 0.9000 0.01 2 2 6.3E-09
31 : 0.00E+00 1.05E-12 0.000 0.3824 0.9000 0.9000 -0.02 2 2 4.1E-09
32 : 0.00E+00 3.55E-13 0.000 0.3394 0.9000 0.9000 0.01 2 2 2.3E-09
33 : 0.00E+00 1.36E-13 0.000 0.3817 0.9000 0.9000 -0.02 2 2 1.5E-09
34 : 0.00E+00 4.60E-14 0.000 0.3392 0.9000 0.9000 0.01 2 2 8.2E-10
iter seconds digits c*x b*y
34 0.4 11.4 -4.3928608475e-07 0.0000000000e+00
|Ax-b| = 8.2e-10, [Ay-c]_+ = 3.1E-11, |x|= 1.1e+05, |y|= 3.9e+05
Detailed timing (sec)
Pre IPM Post
5.253E-02 1.524E-01 1.472E-02
Max-norms: ||b||=0, ||c|| = 1,
Cholesky |add|=0, |skip| = 2, ||L.L|| = 7.87591.
ans =
struct with fields:
yalmiptime: 0.1328
solvertime: 0.2237
info: 'Numerical problems (SeDuMi-1.3)'
problem: 4
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
| ID| Constraint| Primal residual| Dual residual|
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
| #1| Matrix inequality| -3.3097e-11| 9.4478e-12|
| #2| Matrix inequality| -3.1e-11| 9.5949e-12|
| #3| Matrix inequality| 3.6921e-05| 1.186e-12|
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Q = value(Q)
S = value(S)
P = blkdiag(Q,S);
N = cellfun(@value, N, 'UniformOutput', false);
C1 = cellfun(@value, C1, 'UniformOutput', false);
D1 = cellfun(@value, D1, 'UniformOutput', false);
L = cellfun(@(N) S\N, N, 'UniformOutput', false);
L_fh = @(rho) L{1} + rho*L{2};
C1_fh = @(rho) C1{1} + rho*C1{2};
D1_fh = @(rho) D1{1} + rho*D1{2};
Q =
1.0e-03 *
0.3236 0.0494 -0.0774 -0.0055 0.1387 0.0864
0.0494 0.4693 -0.0994 -0.0773 0.0898 0.1885
-0.0774 -0.0994 0.2833 0.0894 -0.1216 -0.1036
-0.0055 -0.0773 0.0894 0.6160 0.0051 -0.0804
0.1387 0.0898 -0.1216 0.0051 0.3360 0.0352
0.0864 0.1885 -0.1036 -0.0804 0.0352 0.3619
S =
1.0e+05 *
0.1248 0.0837 0.0016 0.2247 0.0083 -0.1453
0.0837 0.2988 0.0654 0.3575 -0.0816 -0.1194
0.0016 0.0654 0.1783 0.4302 -0.0440 0.0193
0.2247 0.3575 0.4302 2.9222 -0.2104 -0.6963
0.0083 -0.0816 -0.0440 -0.2104 0.0906 -0.0187
-0.1453 -0.1194 0.0193 -0.6963 -0.0187 0.3832
\begin{align}
L(\rho) =
\left(\begin{array}{cc}
0.741\rho -27.0 & -0.193\rho -7.39 \\
2.31\rho -44.8 & 1.08\rho -11.2 \\
115.0-5.21\rho & 28.4-2.27\rho \\
2.28\rho -46.8 & 0.957\rho -11.7 \\
6.73\rho -128.0 & 2.84\rho -33.2 \\
5.74\rho -119.0 & 2.26\rho -30.6 \\
\end{array}\right)
\end{align}
\begin{align}
C_1(\rho) =
\left(\begin{array}{cccccc}
1.5710^{-4}-5.9310^{-6}\rho & 3.8310^{-5}\rho -2.210^{-4} & 1.6810^{-4}\rho +4.1910^{-4} & -6.1410^{-6}\rho -1.8910^{-4} & 1.7210^{-4}\rho -3.7610^{-4} & 3.3110^{-5}\rho -1.6510^{-4} \\
-1.6410^{-9}\rho -1.1910^{-5} & 1.3510^{-8}\rho +7.6610^{-5} & 6.3410^{-9}\rho +3.3710^{-4} & 1.3410^{-8}\rho -1.2310^{-5} & 3.4310^{-4}-9.2810^{-9}\rho & 2.4610^{-9}\rho +6.6210^{-5} \\
\end{array}\right)
\end{align}
\begin{align}
D_1(\rho) =
\left(\begin{array}{cc}
9.9310^{-5}\rho +2.2310^{-4} & 7.210^{-5}\rho +1.5310^{-4} \\
1.9910^{-4}-1.4610^{-8}\rho & 5.8710^{-9}\rho +1.4410^{-4} \\
\end{array}\right)
\end{align}
Ao_fh = @(rho) [
A_fh(rho) zeros(n_x,n_x)
zeros(n_x,n_x) A_fh(rho)-L_fh(rho)*C
];
Bo_fh = @(rho) [
B_fh(rho)
zeros(n_x,n_u)
];
Co_fh = @(rho) [
D1_fh(rho)*C+C1_fh(rho) -C1_fh(rho)
];
Do = zeros(n_yp,n_r);
% Closed loop matrices
Ac_fh = @(rho) [
A_fh(rho)-B_fh(rho)*K B_fh(rho)*K
zeros(n_x,n_x) A_fh(rho)-L_fh(rho)*C
];
Bc_fh = @(rho) Bo_fh(rho)*G;
for rho = rho_lim(1):0.25:rho_lim(2)
sys = ss(Ao_fh(rho),Bo_fh(rho),Co_fh(rho),Do);
[POLES,ZEROS] = pzmap(sys);
fprintf('\nzeros if rho = %g\n', rho)
disp(ZEROS)
pcz_info(all(real(ZEROS) < 0), 'Zeros are all negative');
end
zeros if rho = -1 -11.3675 + 3.0830i -11.3675 - 3.0830i -3.5872 + 0.0000i -1.8797 + 1.3067i -1.8797 - 1.3067i -0.7504 + 0.0000i -1.0898 + 1.0446i -1.0898 - 1.0446i -0.3183 + 0.4436i -0.3183 - 0.4436i [ OK ] Zeros are all negative zeros if rho = -0.75 -11.1880 + 2.8377i -11.1880 - 2.8377i -3.4987 + 0.0000i -1.9440 + 1.3558i -1.9440 - 1.3558i -0.7611 + 0.0000i -1.1050 + 1.0536i -1.1050 - 1.0536i -0.3122 + 0.4464i -0.3122 - 0.4464i [ OK ] Zeros are all negative zeros if rho = -0.5 -11.0123 + 2.5887i -11.0123 - 2.5887i -3.3820 + 0.0000i -2.0188 + 1.4158i -2.0188 - 1.4158i -0.7712 + 0.0000i -1.1201 + 1.0623i -1.1201 - 1.0623i -0.3062 + 0.4489i -0.3062 - 0.4489i [ OK ] Zeros are all negative zeros if rho = -0.25 -10.8416 + 2.3363i -10.8416 - 2.3363i -3.2361 + 0.0000i -2.1036 + 1.4918i -2.1036 - 1.4918i -0.7805 + 0.0000i -1.1352 + 1.0707i -1.1352 - 1.0707i -0.3002 + 0.4513i -0.3002 - 0.4513i [ OK ] Zeros are all negative zeros if rho = 0 -10.6766 + 2.0809i -10.6766 - 2.0809i -3.0669 + 0.0000i -2.1948 + 1.5901i -2.1948 - 1.5901i -0.7890 + 0.0000i -1.1503 + 1.0788i -1.1503 - 1.0788i -0.2943 + 0.4536i -0.2943 - 0.4536i [ OK ] Zeros are all negative zeros if rho = 0.25 -10.5184 + 1.8234i -10.5184 - 1.8234i -2.2833 + 1.7149i -2.2833 - 1.7149i -2.8904 + 0.0000i -0.7966 + 0.0000i -1.1653 + 1.0865i -1.1653 - 1.0865i -0.2884 + 0.4557i -0.2884 - 0.4557i [ OK ] Zeros are all negative zeros if rho = 0.5 -10.3679 + 1.5650i -10.3679 - 1.5650i -2.3579 + 1.8636i -2.3579 - 1.8636i -2.7271 + 0.0000i -0.8032 + 0.0000i -1.1802 + 1.0939i -1.1802 - 1.0939i -0.2826 + 0.4576i -0.2826 - 0.4576i [ OK ] Zeros are all negative zeros if rho = 0.75 -10.2257 + 1.3073i -10.2257 - 1.3073i -2.4121 + 2.0277i -2.4121 - 2.0277i -2.5892 + 0.0000i -0.8089 + 0.0000i -1.1951 + 1.1011i -1.1951 - 1.1011i -0.2768 + 0.4595i -0.2768 - 0.4595i [ OK ] Zeros are all negative zeros if rho = 1 -10.0920 + 1.0522i -10.0920 - 1.0522i -2.4453 + 2.1984i -2.4453 - 2.1984i -2.4773 + 0.0000i -0.8136 + 0.0000i -1.2099 + 1.1079i -1.2099 - 1.1079i -0.2711 + 0.4611i -0.2711 - 0.4611i [ OK ] Zeros are all negative
concat = @(fh) { fh(0) , fh(1)-fh(0) };
A = concat(Ao_fh);
B = concat(Bo_fh);
C = concat(Co_fh);
D = {Do, Do*0};
AA = [A{:}];
BB = [B{:}];
Im_B = IMA(BB);
L = ORTCO(Im_B);
Ker_C = INTS(KER(C{1}), KER(C{2}));
[R,V] = CSA(AA, Im_B, Ker_C);
dim_V = rank(V);
dim_Ker_V = size(V,1) - dim_V;
dim_L = rank(L);
assert(rank(R) == 0 && rank([V Ker_C]) == rank(Ker_C), ...
'Strong invertibility condition is not satisfied!');
assert(rank(INTS(V, Im_B)) == 0)
assert(dim_L >= dim_V);
T = [ ORTCO(V) L(:,1:dim_V) ]';
N = numel(A);
tA = cell(1,N);
tB = cell(1,N);
tC = cell(1,N);
tD = cell(1,N);
for i = 1:numel(A)
tA{i} = T*A{i}/T;
tB{i} = T*B{i};
tC{i} = C{i}/T;
tD{i} = D{i};
end
tA_fh = @(rho) tA{1} + tA{2}*rho;
tB_fh = @(rho) tB{1} + tB{2}*rho;
tC_fh = @(rho) tC{1} + tC{2}*rho;
tD_fh = @(rho) tD{1} + tD{2}*rho;
tP = T' \ P / T;
tP22 = tP(dim_Ker_V+1:end,dim_Ker_V+1:end);
for rho = rho_lim
tA_rho = tA_fh(rho);
tA22 = tA_rho(dim_Ker_V+1:end,dim_Ker_V+1:end);
eig(tP22 * tA22 + tA22' * tP22)
end
ans = 0×1 empty double column vector ans = 0×1 empty double column vector