Skip to content

Commit eac270d

Browse files
MRGilakMRGilak
committed
changed controllers to classes instead of functions, better architecutre for the entire project
1 parent eedac6a commit eac270d

61 files changed

Lines changed: 1263 additions & 788 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎+controllers/+dmc/DMCController.m‎

Lines changed: 141 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
classdef DMCController < handle
2+
properties
3+
p
4+
m
5+
a
6+
Q
7+
R
8+
sr
9+
is_programmed
10+
is_open_loop
11+
mode
12+
Ts
13+
model_step_fn
14+
15+
F
16+
G
17+
K
18+
19+
upast
20+
u
21+
u_prev
22+
y_prev
23+
end
24+
25+
methods
26+
function obj = DMCController(config)
27+
obj.sr = config.sr;
28+
obj.p = config.p;
29+
obj.m = config.m;
30+
obj.a = config.a;
31+
obj.Q = config.Q(:);
32+
obj.R = config.R(:);
33+
obj.is_programmed = config.is_programmed;
34+
obj.is_open_loop = config.is_open_loop;
35+
36+
if isfield(config, 'mode')
37+
obj.mode = config.mode;
38+
else
39+
obj.mode = 'linear';
40+
end
41+
42+
if isfield(config, 'Ts')
43+
obj.Ts = config.Ts;
44+
else
45+
obj.Ts = [];
46+
end
47+
48+
if isfield(config, 'model_step_fn')
49+
obj.model_step_fn = config.model_step_fn;
50+
else
51+
obj.model_step_fn = @systems.model.dmc.update_nonlinear_state;
52+
end
53+
54+
n = numel(obj.sr) - obj.p;
55+
obj.upast = zeros(n, 1);
56+
57+
step_response = obj.sr(1:n);
58+
obj.F = hankel(obj.sr(2:obj.p+1), obj.sr(obj.p+1:end)) - repmat(step_response(:)', obj.p, 1);
59+
obj.G = toeplitz(obj.sr(1:obj.p), obj.sr(1) * eye(1, obj.m));
60+
61+
Qmat = diag(obj.Q);
62+
Rmat = diag(obj.R);
63+
obj.K = (obj.G' * Qmat * obj.G + Rmat) \ (obj.G' * Qmat);
64+
65+
obj.u = config.u0;
66+
obj.u_prev = config.u0;
67+
68+
if isfield(config, 'y0')
69+
obj.y_prev = config.y0;
70+
else
71+
obj.y_prev = 0;
72+
end
73+
end
74+
75+
function [u_next, info] = step(obj, y_measured, ref_window, x_model)
76+
if nargin < 4
77+
x_model = [];
78+
end
79+
80+
if obj.is_open_loop
81+
d = 0;
82+
else
83+
d = y_measured - obj.y_prev;
84+
end
85+
D = ones(obj.p, 1) * d;
86+
87+
if strcmp(obj.mode, 'linear')
88+
Ypast = obj.F * obj.upast + y_measured;
89+
else
90+
if isempty(obj.Ts)
91+
error('Ts must be provided for nonlinear DMC mode.');
92+
end
93+
if isempty(x_model)
94+
error('x_model must be provided for nonlinear DMC mode.');
95+
end
96+
97+
Ypast = zeros(obj.p, 1);
98+
x_tmp = x_model;
99+
u_current = obj.u_prev;
100+
for k = 1:obj.p
101+
x_tmp = obj.model_step_fn(x_tmp, u_current, obj.Ts);
102+
Ypast(k) = x_tmp(1);
103+
end
104+
end
105+
106+
ref = obj.expand_reference(ref_window);
107+
w = filter([0 (1 - obj.a)], [1 -obj.a], ref, y_measured);
108+
109+
Ebar = w - Ypast - D;
110+
control = obj.K * Ebar;
111+
112+
du = control(1);
113+
obj.upast = [du; obj.upast(1:end-1)];
114+
obj.u = obj.u_prev + du;
115+
obj.y_prev = Ypast(1) + obj.G(1, 1) * du;
116+
obj.u_prev = obj.u;
117+
118+
u_next = obj.u;
119+
info = struct('du', du, 'd', d, 'Ebar', Ebar, 'w', w, 'Ypast', Ypast);
120+
end
121+
122+
function data = internals(obj)
123+
data = struct('F', obj.F, 'G', obj.G, 'gainMatrix', obj.K(1, :));
124+
end
125+
end
126+
127+
methods (Access = private)
128+
function ref = expand_reference(obj, ref_window)
129+
if obj.is_programmed
130+
number_reference = numel(ref_window);
131+
if number_reference >= obj.p
132+
ref = ref_window(1:obj.p);
133+
else
134+
ref = [ref_window(:); ref_window(end) * ones(obj.p - number_reference, 1)];
135+
end
136+
else
137+
ref = ref_window(1) * ones(obj.p, 1);
138+
end
139+
end
140+
end
141+
end

‎+controllers/+epfc/EPFCController.m‎

Lines changed: 199 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,199 @@
1+
classdef EPFCController < handle
2+
properties
3+
linearization_method
4+
Ts
5+
N_model
6+
mu
7+
m
8+
q
9+
r
10+
psi
11+
u_min
12+
u_max
13+
beta
14+
Ni
15+
TOL
16+
is_programmed_ref
17+
18+
linearize_fn
19+
step_response_fn
20+
model_step_fn
21+
22+
v_prev
23+
y_pred
24+
u_prev
25+
end
26+
27+
methods
28+
function obj = EPFCController(config)
29+
obj.linearization_method = config.linearization_method;
30+
obj.Ts = config.Ts;
31+
obj.N_model = config.N_model;
32+
obj.mu = config.mu(:)';
33+
obj.m = config.m(:)';
34+
obj.q = config.q(:)';
35+
obj.r = config.r(:)';
36+
obj.psi = config.psi(:)';
37+
obj.u_min = config.u_min;
38+
obj.u_max = config.u_max;
39+
obj.beta = config.beta;
40+
obj.Ni = config.Ni;
41+
obj.TOL = config.TOL;
42+
43+
if isfield(config, 'is_programmed_ref')
44+
obj.is_programmed_ref = config.is_programmed_ref;
45+
else
46+
obj.is_programmed_ref = false;
47+
end
48+
49+
if isfield(config, 'linearize_fn')
50+
obj.linearize_fn = config.linearize_fn;
51+
else
52+
obj.linearize_fn = @systems.model.epfc.linearize_dynamics;
53+
end
54+
55+
if isfield(config, 'step_response_fn')
56+
obj.step_response_fn = config.step_response_fn;
57+
else
58+
obj.step_response_fn = @systems.model.epfc.get_step_response_nonlinear;
59+
end
60+
61+
if isfield(config, 'model_step_fn')
62+
obj.model_step_fn = config.model_step_fn;
63+
else
64+
obj.model_step_fn = @systems.model.epfc.update_nonlinear_state;
65+
end
66+
67+
obj.v_prev = 0;
68+
obj.u_prev = config.u0;
69+
obj.y_pred = config.x0(1);
70+
end
71+
72+
function [u_next, info] = step(obj, x_model, y_current, ref_now, ref_future)
73+
x1_prev = x_model(1);
74+
obj.u_prev = -x1_prev + obj.v_prev;
75+
76+
if strcmp(obj.linearization_method, 'jacobian')
77+
C = [1, 0];
78+
[A, B] = obj.linearize_fn(x_model, obj.Ts);
79+
sys_d = ss(A, B, C, 0, obj.Ts);
80+
sr_all = step(sys_d, (0:obj.N_model) * obj.Ts);
81+
step_response = sr_all(1:end);
82+
elseif strcmp(obj.linearization_method, 'perturbation')
83+
step_response = obj.step_response_fn(x_model, obj.u_prev, obj.Ts, obj.N_model);
84+
else
85+
error('Unknown linearization method');
86+
end
87+
88+
G = obj.build_dynamic_matrix(step_response);
89+
Tmat = obj.build_dynamic_matrix(step_response);
90+
91+
Ypast = zeros(max(obj.mu), 1);
92+
x_tmp = x_model;
93+
for t = 1:max(obj.mu)
94+
u_sim = -x_tmp(1) + obj.v_prev;
95+
x_tmp = obj.model_step_fn(x_tmp, u_sim, obj.Ts);
96+
Ypast(t) = x_tmp(1);
97+
end
98+
99+
if obj.is_programmed_ref
100+
Yd = zeros(1, numel(obj.mu));
101+
for i = 1:numel(obj.mu)
102+
idx = obj.mu(i);
103+
if idx <= numel(ref_future)
104+
ref_val = ref_future(idx);
105+
else
106+
ref_val = ref_future(end);
107+
end
108+
Yd(i) = obj.psi(i) * y_current + (1 - obj.psi(i)) * ref_val;
109+
end
110+
else
111+
Yd = obj.psi .* y_current + (1 - obj.psi) .* ref_now;
112+
end
113+
114+
Ym = Ypast(obj.mu);
115+
d_ext = y_current - obj.y_pred;
116+
D_ext = d_ext * ones(numel(Ym), 1);
117+
118+
D_nl = zeros(numel(Ym), 1);
119+
dv = zeros(numel(obj.m), 1);
120+
121+
for iter = 1:obj.Ni
122+
D_total = D_ext + D_nl;
123+
e = Yd(:) - Ym - D_total;
124+
125+
Qmat = diag(obj.q);
126+
Rmat = diag(obj.r);
127+
H = G' * Qmat * G + Rmat;
128+
f = -G' * Qmat * e;
129+
130+
T = tril(ones(numel(obj.m), numel(obj.m)));
131+
use_constraints = isfinite(obj.u_min) && isfinite(obj.u_max);
132+
133+
options = optimoptions('quadprog', 'Display', 'off');
134+
135+
if use_constraints
136+
Aineq = [T; -T];
137+
bineq = [(obj.u_max + x1_prev - obj.v_prev) * ones(numel(obj.m), 1); ...
138+
-(obj.u_min + x1_prev - obj.v_prev) * ones(numel(obj.m), 1)];
139+
dv = quadprog(H, f, Aineq, bineq, [], [], [], [], [], options);
140+
else
141+
dv = quadprog(H, f, [], [], [], [], [], [], [], options);
142+
end
143+
144+
x_nl_tmp = x_model;
145+
Y_nl = zeros(numel(obj.mu), 1);
146+
for i = 1:max(obj.mu)
147+
if any(obj.m == (i - 1))
148+
delta_idx = find(obj.m == (i - 1));
149+
delta_u = dv(delta_idx);
150+
else
151+
delta_u = 0;
152+
end
153+
u_sim = -x_nl_tmp(1) + obj.v_prev + delta_u;
154+
x_nl_tmp = obj.model_step_fn(x_nl_tmp, u_sim, obj.Ts);
155+
if any(obj.mu == i)
156+
Y_nl(obj.mu == i) = x_nl_tmp(1);
157+
end
158+
end
159+
160+
Y_li = Tmat * dv + Ym;
161+
h = Y_nl - (Y_li + D_nl);
162+
D_nl_candidate = D_nl + obj.beta * h;
163+
164+
if norm(D_nl_candidate - D_nl) < obj.TOL
165+
D_nl = D_nl_candidate;
166+
break;
167+
end
168+
169+
D_nl = D_nl_candidate;
170+
end
171+
172+
v = obj.v_prev + dv(1);
173+
u_next = -x_model(1) + v;
174+
175+
x_pred = obj.model_step_fn(x_model, u_next, obj.Ts);
176+
obj.y_pred = x_pred(1);
177+
obj.v_prev = v;
178+
obj.u_prev = u_next;
179+
180+
info = struct('dv', dv, 'du', dv(1), 'v', v, 'd_ext', d_ext, 'D_nl', D_nl);
181+
end
182+
end
183+
184+
methods (Access = private)
185+
function G = build_dynamic_matrix(obj, step_response)
186+
G = zeros(numel(obj.mu), numel(obj.m));
187+
for i = 1:numel(obj.mu)
188+
for j = 1:numel(obj.m)
189+
idx = obj.mu(i) - obj.m(j);
190+
if idx > 0 && idx <= numel(step_response)
191+
G(i, j) = step_response(idx);
192+
else
193+
G(i, j) = 0;
194+
end
195+
end
196+
end
197+
end
198+
end
199+
end

‎+systems/+actual/+dmc/update_nonlinear_state.m‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
function x_next = update_nonlinear_state(x, u, Ts, disturbance)
2+
if nargin < 4
3+
disturbance = 0;
4+
end
5+
x_next = systems.model.dmc.update_nonlinear_state(x, u, Ts) + [disturbance; 0];
6+
end

‎+systems/+actual/+epfc/update_nonlinear_state.m‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
function x_next = update_nonlinear_state(x, u, Ts, disturbance)
2+
if nargin < 4
3+
disturbance = 0;
4+
end
5+
substeps = 20;
6+
dt = Ts / substeps;
7+
x1 = x(1); x2 = x(2);
8+
9+
true_c1 = -0.33;
10+
true_c2 = -1.1;
11+
12+
for j = 1:substeps
13+
dx1 = x2;
14+
dx2 = true_c1 * exp(-x1) * x1 + true_c2 * x2 + u + disturbance;
15+
x1 = x1 + dt * dx1;
16+
x2 = x2 + dt * dx2;
17+
end
18+
19+
x_next = [x1; x2];
20+
end

DMC/update_linear_state.m renamed to +systems/+model/+dmc/update_linear_state.m

File renamed without changes.

DMC/update_nonlinear_state.m renamed to +systems/+model/+dmc/update_nonlinear_state.m

File renamed without changes.

EPFC/get_step_response_nonlinear.m renamed to +systems/+model/+epfc/get_step_response_nonlinear.m

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,23 +1,19 @@
11
function step_response = get_step_response_nonlinear(x0, u0, Ts, N_model)
2-
% Small control perturbation
32
du = 1e-2;
43

5-
% Simulate system with baseline control (u0)
64
x = x0;
75
y0 = zeros(N_model, 1);
86
for i = 1:N_model
9-
x = update_nonlinear_state(x, u0, Ts);
7+
x = systems.model.epfc.update_nonlinear_state(x, u0, Ts);
108
y0(i) = x(1);
119
end
1210

13-
% Simulate system with perturbed control (u0 + du)
1411
x = x0;
1512
y1 = zeros(N_model, 1);
1613
for i = 1:N_model
17-
x = update_nonlinear_state(x, u0 + du, Ts);
14+
x = systems.model.epfc.update_nonlinear_state(x, u0 + du, Ts);
1815
y1(i) = x(1);
1916
end
2017

21-
% Difference approximates step response
2218
step_response = (y1 - y0) / du;
2319
end

EPFC/linearize_dynamics.m renamed to +systems/+model/+epfc/linearize_dynamics.m

File renamed without changes.

EPFC/update_nonlinear_state.m renamed to +systems/+model/+epfc/update_nonlinear_state.m

File renamed without changes.

‎.gitignore‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
docs/*.docx

0 commit comments

Comments
 (0)