1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95

n = 3;  
l = 2;  
m = 2;  
p = 1;  
r = 2;  


A = [2.8982, -3.1606,  0.6816;
     6.3595, -4.2055,  4.5423;
     3.2046, -3.1761, -3.8142];
if ~isequal(size(A), [n, n])
    fprintf('Dimension of A is wrong!!\n');
end

B1 = [2.0310,  1.2164;
      0.4084,  0.2794;
     -0.7775, -0.3307];
if ~isequal(size(B1), [n, r])
    fprintf('Dimension of B1 is wrong!!\n');
end

B2 = [-0.0785;
      -0.0853;
      -0.0986];
if ~isequal(size(B2), [n, p])
    fprintf('Dimension of B2 is wrong!!\n');
end

C1 = [-0.8778, -4.9442, -4.5084;
       4.0161, -2.0259,  1.9318];
if ~isequal(size(C1), [l, n])
    fprintf('Dimension of C1 is wrong!!\n');
end

D1 = [0.6004,  0.2107;
      1.9320, -0.3997];
if ~isequal(size(D1), [l, r])
    fprintf('Dimension of D1 is wrong!!\n');
end

D2 = [0.0330;
     -0.0414];
if ~isequal(size(D2), [l, p])
    fprintf('Dimension of D2 is wrong!!\n');
end

C2 = [0.9607, 1.5600, 2.8558;
     -2.4371, 1.3634, 0.0095];
if ~isequal(size(C2), [m, n])
    fprintf('Dimension of C2 is wrong!!\n');
end


yalmip('clear')

gamma = 1e-8;
P = sdpvar(n, n);
W = sdpvar(n, l, 'full');
M = [A'*P+C1'*W'+P*A+W*C1, P*B2+W*D2, C2';
     (P*B2+W*D2)', -gamma*eye(p), zeros(p, m);
     C2, zeros(m, p), -gamma*eye(m)];

Constraints = [P >= 0; M <= 0];
Objective = [];
options = sdpsettings('verbose', 0);  
sol = optimize(Constraints, Objective, options);

if sol.problem == 0
    L = value(P)\value(W);
else
    disp('Hmm, something went wrong!');
    sol.info
    yalmiperror(sol.problem)
end



poles = [-5; -7+7j; -7-7j];
K = place(A, -B1, poles);

Aco = [A+B1*K, B1*K;
       zeros(size(A)), A+L*C1];
Bco = [B2; B2+L*D2];
Cco = [zeros(size(C2)), C2]; Dco = 0;
sys_co = ss(Aco, Bco, Cco, Dco);

t = 0:0.01:4;
xe0 = zeros(6, 1);
w = wgn(1, length(t), 100, 'real');     
[zt, t, xe] = lsim(sys_co, w, t, xe0);  

subplot(311); plot(t, w, 'LineWidth', 1); grid on
subplot(312); plot(t, xe(:, 4:6), 'LineWidth', 1); grid on
subplot(313); plot(t, zt(:, 1:2), 'LineWidth', 1); grid on