-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathsimulate_linear_system.m
More file actions
83 lines (64 loc) · 2.38 KB
/
simulate_linear_system.m
File metadata and controls
83 lines (64 loc) · 2.38 KB
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
function [results, sr] = simulate_linear_system(params)
A_lin = [0, 1; -1.7941, -1.1];
B_lin = [0; 1];
C_lin = [1, 0];
N = params.N;
Ts = params.Ts;
sys_c = ss(A_lin, B_lin, C_lin, 0);
sys_d = c2d(sys_c, Ts);
N_model = params.N_model;
Ref = padarray(params.Ref(:), max(0, N - numel(params.Ref)), 'replicate', 'post');
[sr_all, ~] = step(sys_d, (0:N_model)*Ts);
sr = sr_all(2:end);
% Initialize p with y set to x0(1)
Struct = struct('sr', sr, 'p', params.P, 'm', params.M, ...
'a', params.alpha, 'Q', params.Q, 'R', params.R, ...
'y', params.x0(1), 'upast', [], 'u', 0, ...
'u0', params.u0_linear, ...
'is_programmed', params.is_programmed, ...
'is_open_loop', params.is_open_loop);
x0 = params.x0;
Struct.y0 = x0(1);
Struct.x0 = x0;
Struct.u0 = params.u0_linear;
Y = zeros(N, 1);
Y(1) = x0(1);
results = struct('Y', Y, 'U', zeros(N,1), 'X', zeros(N,2), ...
'DU', zeros(N,1), 'Ref_filtered', zeros(N,1), ...
'time', (0:N-1)'*Ts, 'F', [], 'G', [], 'sr_all', []);
x = x0(:);
disturbance_start_step = round(params.disturbance_start_time / Ts);
for k = 1:N
ref_window = Ref(k:min(N, k+params.P-1));
Struct.r = ref_window;
% Compute control
Struct = dmc_linear(Struct);
% Store control inputs
results.U(k) = Struct.u;
results.DU(k) = Struct.du;
% Disturbances and noise
w3 = params.disturbance_amp * (k >= disturbance_start_step & k <= (disturbance_start_step + 1));
w4 = wgn(1, 1, params.noise_power);
% Apply full absolute input
u_actual = results.U(k) + w3;
% Linear system
x = update_linear_state(sys_d, x, u_actual) + [w3; 0];
y = sys_d.C * x + w4;
results.Y(k) = y;
results.X(k,:) = x';
% Feedback
Struct.y = results.Y(k);
% Reference filtering
if k == 1
results.Ref_filtered(k) = Ref(k);
else
results.Ref_filtered(k) = params.alpha * results.Ref_filtered(k - 1) + (1 - params.alpha) * Ref(k);
end
results.d(k) = Struct.d;
results.Ebar(k, :) = Struct.Ebar;
end
results.F = Struct.F;
results.G = Struct.G;
results.gainMatrix = Struct.k;
results.sr_all = sr_all;
end