Teljes Matlab script kiegészítő függvényekkel.
File: d2018_01_09_K_prelim_L_codesign_v4.m Directory: projects/3_outsel/2017_11_13_lpv_passivity Author: Peter Polcz (ppolcz@gmail.com)
Created on 2018. January 09. Modified on 2018. January 17.
FIGYELEM, a rendszermodell, nem stabil!
s = tf('s');
% unstable MIMO
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,B,C,D] = deal(sys.a, sys.b, sys.c, sys.d);
tol = 1e-10;
prec = -log10(tol);
A0 = round(A0,prec);
B = round(B,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
\begin{align} {\LARGE(1) \quad}
H(s) =
\left(\begin{array}{cc}
-\frac{s-1}{-s^2+s+2} & \frac{10}{10s^2+29s-3} \\
\frac{s-7}{s^2+6s+5} & \frac{s-6}{s^2+5s+6} \\
\end{array}\right)
\end{align}
Az előző LTI modellt egy kicsit megperturbálom (kezdetben csak az A mátrixot).
$$ \begin{aligned} &\Sigma: \left\{\begin{aligned} &\dot x = A(\rho) x + B(\rho) u,~~~ \rho \in \mathcal P \\ &y = C x \\ \end{aligned}\right. \\ &\begin{aligned} \text{where: } & A(\rho) = A_0 + A_1 \rho \in \mathbb{R}^{n\times n} \\ & B(\rho) = B_0 + B_1 \rho \in \mathbb{R}^{n\times r} \\ & C \in \mathbb{R}^{m\times n} \\ & D = 0_{m\times r} \end{aligned} \end{aligned} $$
A1 = A0;
A1(abs(A0) < 1) = 0;
A1 = A1 .* randn(size(A1))/10;
B0 = B;
B1 = B*0;
% Indices = [3];
% B1(Indices) = rand(size(Indices));
A_fh = @(rho) A0 + rho*A1;
B_fh = @(rho) B0 + rho*B1;
Matrix $K$ is preliminarily design for the nominal value of $\rho$, than checked whether the closed loop is stable for other $\rho \in \mathcal P$ values. Let $G = I_m$.
G = eye(n_u);
K = place(A0,B,linspace(-5,-6,n_x));
Free matrix variables. All matrices are assumed to be parameter independent.
C1 = sdpvar(n_yp,n_x,'full');
D1 = sdpvar(n_yp,n_y,'full');
Q = sdpvar(n_x,n_x,'symmetric');
S = sdpvar(n_x,n_x,'symmetric');
N = sdpvar(n_x,n_y,'full');
P = blkdiag(Q,S);
where $N = S L$.
$$ \begin{align} &\wt A(\rho) = \pmqty{ A(\rho) & 0 \\ 0 & A(\rho) - L C },~ \wt B = \pmqty{B \\ 0} \\ &\wt C = \pmqty{ D_1 C + C_1 & -C_1 }. \end{align} $$
Ao = @(L,rho) [
A_fh(rho) zeros(n_x,n_x)
zeros(n_x,n_x) A_fh(rho)-L*C
];
Bo = @(rho) [B_fh(rho) ; zeros(n_x,n_u)];
Co = [D1*C+C1 -C1];
Do = zeros(n_yp,n_r);
$$ \begin{aligned} \wt A_c(\rho) = \begin{pmatrix} A(\rho)-B(\rho) K & B(\rho) K \\ 0 & A(\rho) - L C \end{pmatrix},~ \wt B_c(\rho) = \begin{pmatrix} B(\rho) G \\ 0 \end{pmatrix}. \end{aligned} $$
Ac = @(L,rho) [
A_fh(rho)-B_fh(rho)*K B_fh(rho)*K
zeros(n_x,n_x) A_fh(rho)-L*C
];
Bc = @(rho) Bo(rho)*G;
W = eye(n_yp);
$$ \begin{aligned} &\wt A_c({\color{red} \rho})^T P + P \wt A_c({\color{red} \rho}) = \spmqty{ {\blue Q} A_K({\red \rho}) + A_K^T({\red \rho}) {\blue Q} & {\blue Q} B({\red \rho}) K \\ K^T B^T({\red \rho}) {\blue Q} & {\blue S} A({\red \rho}) + A({\red \rho})^T {\blue S} - {\blue N}({\red \rho}) C - C^T {\blue N^T}({\red \rho}), } \\ &\text{where } A_K({\red \rho}) = A({\red \rho}) - B({\red \rho}) K. \nonumber \end{aligned} $$
AcP_PAc = @(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*C-C'*N'
];
$$ M_2 = \spmqty{ { \wt A_c({\color{red} \rho})^T} { P} + { P} { \wt A_c({\color{red} \rho})} & { P} \wt B_c({\red \rho}) - { \wt C^T({\red \rho})} & { \wt C^T({\red \rho})} \\ \wt B_c^T({\red \rho}) { P}- { \wt C({\red \rho})} & 0 & 0 \\ { \wt C({\red \rho})} & 0 & -W^{-1} } $$
M2 = @(rho) [
AcP_PAc(rho) P*Bc(rho)-Co' Co'
Bc(rho)'*P-Co zeros(n_r,n_r) zeros(n_r,n_yp)
Co zeros(n_yp,n_r) -inv(W)
];
% Constraints
Constraints = [
M2(-1) <= 0
M2(1) <= 0
P - 0.0001*eye(size(P)) >= 0
]
++++++++++++++++++++++++++++++++++ | ID| Constraint| ++++++++++++++++++++++++++++++++++ | #1| Matrix inequality 16x16| | #2| Matrix inequality 16x16| | #3| Matrix inequality 12x12| ++++++++++++++++++++++++++++++++++
optimize(Constraints)
check(Constraints)
Problem
Name :
Objective sense : min
Type : CONIC (conic optimization problem)
Constraints : 70
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 : 70
Cones : 0
Scalar variables : 0
Matrix variables : 3
Integer variables : 0
Optimizer - threads : 4
Optimizer - solved problem : the primal
Optimizer - Constraints : 70
Optimizer - Cones : 0
Optimizer - Scalar variables : 0 conic : 0
Optimizer - Semi-definite variables: 3 scalarized : 350
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 : 2485 after factor : 2485
Factor - dense dim. : 0 flops : 4.63e+05
ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME
0 2.4e+00 1.0e+00 5.0e+00 0.00e+00 3.998800000e+00 0.000000000e+00 1.0e+00 0.00
1 5.2e-01 2.2e-01 2.3e+00 1.00e+00 8.707527494e-01 0.000000000e+00 2.2e-01 0.01
2 1.8e-01 7.5e-02 1.4e+00 9.99e-01 3.003328125e-01 0.000000000e+00 7.5e-02 0.01
3 6.7e-02 2.8e-02 8.4e-01 9.96e-01 1.130547679e-01 0.000000000e+00 2.8e-02 0.01
4 2.3e-02 9.8e-03 4.9e-01 9.96e-01 3.938897574e-02 0.000000000e+00 9.8e-03 0.01
5 4.8e-03 2.0e-03 2.2e-01 9.92e-01 8.163890887e-03 0.000000000e+00 2.0e-03 0.01
6 9.5e-04 4.0e-04 9.6e-02 9.79e-01 1.624774403e-03 0.000000000e+00 4.0e-04 0.01
7 2.8e-04 1.2e-04 5.1e-02 9.64e-01 4.879303553e-04 0.000000000e+00 1.2e-04 0.01
8 7.8e-05 3.3e-05 2.7e-02 9.71e-01 1.371675058e-04 0.000000000e+00 3.3e-05 0.02
9 3.1e-05 1.3e-05 1.7e-02 9.56e-01 5.672656143e-05 0.000000000e+00 1.3e-05 0.02
10 6.9e-06 2.9e-06 7.3e-03 9.31e-01 1.282954394e-05 0.000000000e+00 2.9e-06 0.02
11 2.0e-06 8.6e-07 3.5e-03 8.39e-01 3.996112212e-06 0.000000000e+00 8.5e-07 0.02
12 6.2e-07 2.6e-07 1.5e-03 6.98e-01 1.356505947e-06 0.000000000e+00 2.6e-07 0.02
13 3.0e-07 1.3e-07 8.5e-04 4.74e-01 7.630308444e-07 0.000000000e+00 1.3e-07 0.02
14 7.3e-08 3.1e-08 2.6e-04 4.00e-01 1.408696426e-07 0.000000000e+00 3.1e-08 0.02
15 2.2e-08 9.2e-09 7.9e-05 1.51e-01 -7.347058531e-08 0.000000000e+00 9.2e-09 0.02
16 6.9e-09 2.9e-09 3.2e-05 2.21e-01 -8.952370110e-08 0.000000000e+00 2.9e-09 0.02
17 2.9e-09 1.2e-09 1.1e-05 -9.84e-03 -2.024105370e-07 0.000000000e+00 1.2e-09 0.03
18 5.9e-10 2.5e-10 2.5e-06 6.38e-02 -2.052818593e-07 0.000000000e+00 2.5e-10 0.03
19 1.7e-10 7.3e-11 7.6e-07 3.67e-02 -2.144482486e-07 0.000000000e+00 7.3e-11 0.03
20 4.7e-11 2.0e-11 2.1e-07 5.91e-03 -2.259000431e-07 0.000000000e+00 2.0e-11 0.03
21 1.8e-11 7.6e-12 7.9e-08 2.29e-02 -2.193365621e-07 0.000000000e+00 7.6e-12 0.03
22 4.7e-12 2.0e-12 2.0e-08 -8.77e-03 -2.317777451e-07 0.000000000e+00 2.0e-12 0.04
23 1.5e-12 6.3e-13 6.6e-09 3.02e-02 -2.210161629e-07 0.000000000e+00 6.2e-13 0.04
24 5.8e-13 2.5e-13 2.5e-09 -1.99e-02 -2.355462922e-07 0.000000000e+00 2.5e-13 0.04
25 1.5e-13 6.7e-14 6.6e-10 3.24e-02 -2.211940101e-07 0.000000000e+00 6.2e-14 0.04
26 6.3e-14 2.9e-14 2.7e-10 -2.72e-02 -2.412833002e-07 0.000000000e+00 2.7e-14 0.04
27 1.5e-14 1.1e-14 6.5e-11 1.23e-02 -2.270169311e-07 0.000000000e+00 6.2e-15 0.04
28 1.3e-14 9.8e-15 2.5e-11 -1.94e-02 -2.411796468e-07 0.000000000e+00 2.5e-15 0.04
29 1.9e-14 9.4e-15 6.3e-12 9.51e-03 -2.309901303e-07 0.000000000e+00 6.0e-16 0.04
30 4.0e-14 1.3e-14 2.4e-12 2.05e-03 -2.369628345e-07 0.000000000e+00 2.3e-16 0.05
31 1.1e-14 6.7e-15 5.7e-13 -2.17e-02 -2.367946886e-07 0.000000000e+00 5.7e-17 0.05
32 8.9e-15 3.9e-15 2.5e-13 2.06e-02 -2.384412054e-07 0.000000000e+00 2.5e-17 0.05
33 9.1e-15 9.3e-15 6.0e-14 -2.09e-02 -2.436127761e-07 0.000000000e+00 6.2e-18 0.05
34 1.3e-14 9.2e-15 2.4e-14 2.82e-02 -2.447050134e-07 0.000000000e+00 2.5e-18 0.05
35 1.9e-14 4.0e-15 6.4e-15 -2.83e-02 -2.487355876e-07 0.000000000e+00 6.7e-19 0.05
36 1.4e-14 8.7e-15 2.5e-15 1.61e-02 -2.477059828e-07 0.000000000e+00 2.6e-19 0.05
37 7.7e-15 7.1e-15 7.2e-16 -3.40e-02 -2.458013230e-07 0.000000000e+00 7.6e-20 0.05
38 2.9e-15 6.3e-15 2.8e-16 2.49e-02 -2.380730761e-07 0.000000000e+00 2.9e-20 0.05
39 7.5e-16 2.7e-15 7.0e-17 -2.54e-02 -2.400562061e-07 0.000000000e+00 7.4e-21 0.06
40 3.1e-16 8.4e-15 3.0e-17 1.43e-02 -2.332254340e-07 0.000000000e+00 3.0e-21 0.06
41 7.4e-17 8.4e-15 7.0e-18 -3.05e-02 -2.381459227e-07 0.000000000e+00 7.4e-22 0.06
42 3.3e-17 1.1e-14 3.1e-18 3.91e-02 -2.433704390e-07 0.000000000e+00 3.2e-22 0.06
43 7.4e-18 8.1e-15 6.9e-19 -5.46e-02 -2.444490121e-07 0.000000000e+00 7.6e-23 0.07
44 3.0e-18 5.9e-15 2.7e-19 5.29e-02 -2.535896650e-07 0.000000000e+00 3.0e-23 0.07
45 7.7e-19 6.8e-15 7.0e-20 -1.75e-02 -2.491384624e-07 0.000000000e+00 7.8e-24 0.07
46 2.9e-19 6.2e-15 2.6e-20 1.38e-02 -2.579986940e-07 0.000000000e+00 2.9e-24 0.07
47 3.2e-19 4.4e-15 7.2e-21 2.52e-02 -2.505204050e-07 0.000000000e+00 8.0e-25 0.08
48 2.6e-17 5.3e-15 6.7e-21 4.37e-03 -2.512546286e-07 0.000000000e+00 7.4e-25 0.08
49 3.2e-17 4.0e-15 6.4e-21 3.28e-03 -2.515487532e-07 0.000000000e+00 7.1e-25 0.08
50 3.3e-17 1.2e-14 6.4e-21 1.69e-03 -2.515807806e-07 0.000000000e+00 7.0e-25 0.08
51 3.4e-17 1.3e-14 6.4e-21 2.21e-03 -2.515966923e-07 0.000000000e+00 7.0e-25 0.09
52 3.4e-17 1.1e-14 6.4e-21 1.99e-03 -2.516045988e-07 0.000000000e+00 7.0e-25 0.09
53 3.4e-17 1.6e-14 6.4e-21 2.11e-03 -2.516208923e-07 0.000000000e+00 7.0e-25 0.09
54 3.4e-17 8.9e-15 6.4e-21 2.53e-03 -2.516213653e-07 0.000000000e+00 7.0e-25 0.10
55 3.4e-17 8.9e-15 6.4e-21 3.32e-03 -2.516218580e-07 0.000000000e+00 7.0e-25 0.10
56 3.4e-17 8.9e-15 6.4e-21 2.31e-03 -2.516228632e-07 0.000000000e+00 7.0e-25 0.10
57 3.4e-17 8.9e-15 6.4e-21 2.28e-03 -2.516228632e-07 0.000000000e+00 7.0e-25 0.11
Optimizer terminated. Time: 0.11
Interior-point solution summary
Problem status : PRIMAL_AND_DUAL_FEASIBLE
Solution status : OPTIMAL
Primal. obj: -2.5162286318e-07 nrm: 3e+09 Viol. con: 3e-07 barvar: 2e-07
Dual. obj: 0.0000000000e+00 nrm: 3e+09 Viol. con: 0e+00 barvar: 5e-06
Optimizer summary
Optimizer - time: 0.11
Interior-point - iterations : 58 time: 0.11
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.1766
solvertime: 0.1207
info: 'Successfully solved (MOSEK)'
problem: 0
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
| ID| Constraint| Primal residual| Dual residual|
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
| #1| Matrix inequality| -1.6575e-07| -7.0352e-07|
| #2| Matrix inequality| -4.9775e-08| 2.9605e-17|
| #3| Matrix inequality| 2.4007e-05| 5.9336e-17|
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Q = value(Q);
S = value(S);
N = value(N);
L = S\N;
P = value(P);
Co = value(Co);
C1 = value(C1);
D1 = value(D1);
% value(AcP_PAc2(0.1) - AcP_PAc(0.1))
Bc0 = Bc(0);
Bo0 = Bc(0);
Ac0 = Ac(L,0);
Ao0 = Ao(L,0);
M2_val = value(M2(0));
if pcz_info(all(real(eig(M2_val)) <= 1e-5), 'M2(0) <= 0')
eig(M2_val)'
fprintf('larges eigen value: %f\n\n', max(eig(M2_val)))
return
end
M2_val = value(M2(0.1));
if pcz_info(all(real(eig(M2_val)) <= 1e-5), 'M2(0.1) <= 0')
eig(M2_val)'
fprintf('larges eigen value: %f\n\n', max(eig(M2_val)))
return
end
M2_val = value(M2(-0.1));
if pcz_info(all(real(eig(M2_val)) <= 1e-5), 'M2(-0.1) <= 0')
eig(M2_val)'
fprintf('larges eigen value: %f\n\n', max(eig(M2_val)))
return
end
if pcz_info(all(real(eig(P)) > 0), 'P > 0')
eig(P)'
return
end
if pcz_info(all(real(eig(A0 - L*C)) < 0), 'A - LC < 0')
eig(A0 - L*C)
return
end
if pcz_info(norm(D1) > 1e-3, 'norm(D1) > 1e-3')
disp(norm(D1))
return
end
% The resulting closed-loop system
sys_CLS = minreal(ss(Ac0,Bc0,Co,Do),[],0);
sys_OLS = minreal(ss(Ao0,Bo0,Co,Do),[],0);
sys_CLS = minreal(tf(sys_CLS),[],0);
sys_OLS = minreal(tf(sys_OLS),[],0);
if pcz_info(all(real(tzero(sys_OLS)) < 0), 'Zeros of the open loop system are stable')
tzero(sys_OLS)
return
end
if pcz_info(all(real(tzero(sys_CLS)) < 0), 'Zeros of the closed loop system are stable')
tzero(sys_CLS)
return
end
[POLES,ZEROS] = pzmap(sys_OLS);
[POLES,ZEROS] = pzmap(sys_CLS);
[ OK ] M2(0) <= 0 [ OK ] M2(0.1) <= 0 [ OK ] M2(-0.1) <= 0 [ OK ] P > 0 [ OK ] A - LC < 0 [ OK ] norm(D1) > 1e-3 [ OK ] Zeros of the open loop system are stable [ OK ] Zeros of the closed loop system are stable
Written on 2018. January 17.
Ebben a részben $A(\rho)$, $B(\rho)$, $C$, $D$ új értelmet nyer. Ezek adják meg az observerrel kiegészített dinamika mátrixait.
A_fh = @(rho) Ao(L,rho);
B_fh = Bo;
A0 = A_fh(0);
A1 = A_fh(1) - A0;
B0 = B_fh(0);
B1 = B_fh(1) - B0;
C0 = Co;
C1 = C0*0;
D0 = Do;
D1 = Do*0;
A_fh = @(rho) A0 + A1*rho;
B_fh = @(rho) B0 + B1*rho;
C_fh = @(rho) C0 + C1*rho;
D_fh = @(rho) D0 + D1*rho;
A = {A0, A1};
B = {B0, B1};
C = {C0, C1};
D = {D0, D1};
AA = [A{:}];
BB = [B{:}];
Nálam az $E_c$ most az $Im(B)$.
Im_B = IMA([B{1} B{2}]);
Ker_C = INTS(KER(C{1}), KER(C{2}));
We compute $\mathcal V^*$ and $\mathcal R^*$.
[R,V] = CSA([A{1} A{2}], Im_B, Ker_C);
dim_V = size(V,2);
dim_Ker_V = size(V,1) - size(V,2);
assert(rank(R) == 0 && rank([V Ker_C]) == rank(Ker_C), ...
'Strong invertibility condition is not satisfied!');
assert(rank(INTS(V, Im_B)) == 0)
We ensure that $\mathcal R^* = \{0\}$ and that $\mathcal V^* \subseteq \mathrm{Ker}(C)$. Furthermore, we checked the strong invertibility condition $\mathcal V^* \cap \mathrm{Im}\big(B(\rho)\big) = \{0\}$.
\begin{align} {\LARGE(10) \quad} \mathcal V^* = \left(\begin{array}{cccccccccc} -0.992 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ -0.0167 & 0.978 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0.0204 & -0.061 & -0.838 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0.103 & 0.0891 & 0.259 & 0.445 & 0 & 0 & 0 & 0 & 0 & 0 \\ -0.0148 & -0.0411 & 0.0728 & 0.135 & 0.968 & 0 & 0 & 0 & 0 & 0 \\ -0.048 & -0.132 & 0.232 & 0.443 & -0.209 & 0.551 & 0 & 0 & 0 & 0 \\ 0.0191 & 0.0373 & -0.033 & -0.242 & 0.0724 & 0.44 & -0.84 & 0 & 0 & 0 \\ 0.025 & 0.0121 & 0.0995 & -0.476 & 0.0693 & 0.427 & 0.39 & 0.611 & 0 & 0 \\ -0.0073 & 0.059 & -0.272 & 0.409 & 0.0232 & 0.13 & 0.0032 & 0.352 & -0.662 & 0 \\ -0.0096 & 0.0008 & -0.0595 & 0.207 & -0.0229 & -0.142 & -0.14 & 0.4 & 0.388 & -0.769 \\ 0.0147 & 0.0712 & -0.191 & -0.0019 & 0.0852 & 0.512 & 0.348 & -0.547 & 0.0989 & -0.361 \\ -0.004 & 0.05 & -0.218 & 0.301 & 0.0249 & 0.143 & 0.0345 & 0.209 & 0.634 & 0.527 \\ \end{array}\right) \end{align}1. variáns
Let $T = \begin{pmatrix} {V^*}^\perp & L \end{pmatrix}^T$, where ${V^*}^\perp$ and $L$ are orthonormal bases for ${\mathcal V^*}^\perp \not\perp \mathcal L \subseteq \mathrm{Im}\big(B(\rho)\big)$, respectively.
L = ORTCO(Im_B);
dim_L = size(L,2);
assert(dim_L >= dim_V);
T = [ ORTCO(V) L(:,1:dim_V) ]';
tol = 1e-10;
prec = -log10(tol);
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} = round(T*A{i}/T, prec);
tB{i} = round(T*B{i}, prec);
tC{i} = round(C{i}/T, prec);
tD{i} = round(D{i}, prec);
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;