-
Notifications
You must be signed in to change notification settings - Fork 1
/
auto_final.m
161 lines (144 loc) · 5.33 KB
/
auto_final.m
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
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
n = 5;
m = 1;
L = m+n;
N = 250;
inputdata = load('input_cute.mat');
statedata = load('state_cute.mat');
X = statedata.all_x_hist;
U = inputdata.all_F_hist;
%% check rank condition
for k = 1:N
% Extract the state and input data for time k from all experiments
X_k = permute(X(:, k, :), [1, 3, 2]); % State data at time k
U_k = permute(U(:, k, :), [1, 3, 2]); % Input data at time k
% Combine state and input data into a single matrix
combined_data = [X_k; U_k];
% Check if the combined data matrix has full rank (should be n + m)
if rank(combined_data) ~= n + m
error('Rank condition failed at step %d', k);
end
end
%% LQR Parameters
Q_f = 50 * eye(n);
for k = 1:N
Qk = Q(k, n);
Rk = R(k, m);
end
%% Optimization Variables
S = sdpvar(n, n, N+1, 'symmetric');
H = sdpvar(L, n, N, 'full');
O = sdpvar(m, m, N, 'symmetric');
%% Optimization Constraints
Constraints = [S(:,:,1) >= eye(n)]; % Initial constraint on S(0)
% Adding SDP constraints for each time step
for k = 1:N
X_k = permute(X(:, k, :), [1, 3, 2]); % State data at time k, reshaped correctly
X_k_1 = permute(X(:, k+1, :), [1, 3, 2]);
U_k = permute(U(:, k, :), [1, 3, 2]); % Input data at time k, reshaped correctly
% Construct block matrix constraints
Constraints = [Constraints, ...
[S(:,:,k+1) - eye(n), X_k_1 * H(:,:,k); ...
H(:,:,k)' * X_k_1', S(:,:,k)] >= 0, ...
[O(:,:,k), sqrtm(R(k,m)) * U_k * H(:,:,k); ...
H(:,:,k)' * U_k' * sqrtm(R(k,m)), S(:,:,k)] >= 0, ...
S(:,:,k) == X_k * H(:,:,k)];
end
%% Optimization Objective
% Initialize the objective
objective = 0;
% Add terms for each time step
for k = 1:N
% Assuming Q(k) returns a matrix for the cost at time step k
objective = objective + trace(Q(k,n) * S(:,:,k)) + trace(O(:,:,k));
end
% Add the final term involving Q_f and S(N)
objective = objective + trace(Q_f * S(:,:,N+1)); % Index N+1 for S(N) due to MATLAB indexing
% Setup the optimization problem
% Note: You need to define the constraints array before this step
options = sdpsettings('solver', 'mosek'); % Using MOSEK as the solver
sol = optimize(Constraints, objective, options);
% Check the result
if sol.problem == 0
disp('Optimization solved successfully');
else
disp(['Problem not solved, status: ' yalmiperror(sol.problem)]);
end
%% Calculate State Feedback Gain (K)
K_star = cell(1, N); % Initialize cell array to store feedback gains
% Calculate the optimal feedback gains K*(k)
for k = 1:N
% Ensure to correctly extract and use matrix dimensions
% Assume U_k comes from some defined operational matrix, for example
U_k = permute(U(:, k, :), [1, 3, 2]);
% Extract values and compute gain
Hk_value = value(H(:,:,k));
Sk_value = value(S(:,:,k));
K_star{k} = U_k * Hk_value * inv(Sk_value); % Compute feedback gain using matrix inverse
end
% Print the optimal state feedback gains
disp('Optimal state feedback control gains K*(k):');
for k = 1:N
fprintf('K*(%d) =\n', k-1); % Adjusted index for display to match 0-based indexing
disp(K_star{k});
end
%% Closed Loop Simulation for DD
X0 = [0.4411; 0.2711];
X_closed_loop = simulate_closed_loop(K_star, X0, N);
figure;
plot(0:N, X_closed_loop);
xlabel('Time step');
ylabel('State values');
title('Closed-Loop State Trajectories');
legend('State 1', 'State 2');
grid on;
5:13
%% Optimization using CVX
cvx_begin sdp
% Define optimization variables
variable S(n, n, N+1) symmetric
variable H(L, n, N)
variable O(m, m, N) symmetric
% Define constraints
Constraints = [S(:,:,1) >= eye(n)]; % Initial constraint on S(0)
for k = 1:N
X_k = permute(X(:, k, :), [1, 3, 2]); % State data at time k, reshaped correctly
X_k_1 = permute(X(:, k+1, :), [1, 3, 2]);
U_k = permute(U(:, k, :), [1, 3, 2]); % Input data at time k, reshaped correctly
% Construct block matrix constraints
Constraints = [Constraints, ...
[S(:,:,k+1) - eye(n), X_k_1 * H(:,:,k); ...
H(:,:,k)' * X_k_1', S(:,:,k)] >= 0, ...
[O(:,:,k), sqrtm(R(k,m)) * U_k * H(:,:,k); ...
H(:,:,k)' * U_k' * sqrtm(R(k,m)), S(:,:,k)] >= 0, ...
S(:,:,k) == X_k * H(:,:,k)];
end
% Add constraints to CVX
subject to
Constraints;
% Define the objective function
objective = 0;
for k = 1:N
objective = objective + trace(Q(k,n) * S(:,:,k)) + trace(O(:,:,k));
end
objective = objective + trace(Q_f * S(:,:,N+1));
% Minimize the objective
minimize(objective)
cvx_end
% Check the result
if strcmp(cvx_status, 'Solved')
disp('Optimization solved successfully');
else
disp(['Problem not solved, status: ' cvx_status]);
end
%% Calculate State Feedback Gain (K)
K_star = cell(1, N); % Initialize cell array to store feedback gains
% Calculate the optimal feedback gains K*(k)
for k = 1:N
% Ensure to correctly extract and use matrix dimensions
% Assume U_k comes from some defined operational matrix, for example
U_k = permute(U(:, k, :), [1, 3, 2]);
% Extract values and compute gain
Hk_value = H(:,:,k);
Sk_value = S(:,:,k);
K_star{k} = U_k * Hk_value * inv(Sk_value); % Compute feedback gain using matrix inverse
end