-
Notifications
You must be signed in to change notification settings - Fork 16
/
BrysonDenham_Solution_States.m
59 lines (56 loc) · 2.25 KB
/
BrysonDenham_Solution_States.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
%--------------------------------------------------------------------------
% BrysonDenham_Solution_Control.m
% Calculates the optimal states for the Bryson-Denham problem
%--------------------------------------------------------------------------
% Valid when 0 < l <= 1/6
% See pages 120–123 of A. E. Bryson and Y.-C. Ho, Applied Optimal Control,
% revised printing ed. Taylor & Francis, 1975.
%--------------------------------------------------------------------------
% Primary Contributor: Daniel R. Herber, Graduate Student, University of
% Illinois at Urbana-Champaign
% https://github.com/danielrherber/optimal-control-direct-method-examples
%--------------------------------------------------------------------------
function X = BrysonDenham_Solution_States(t,l)
X = zeros(length(t),2);
%----------------------------------------------------------------------
if (l > 0) && (l <= 1/6)
for i = 1:length(t)
if (t(i) <= 3*l)
v = (1 - t(i)/(3*l)).^2;
x = l*(1 - (1 - t(i)/(3*(l))).^3);
elseif (t(i) >= 1-3*l)
v = -(1 - (1-t(i))/(3*l)).^2;
x = l*(1 - (1 - (1-t(i))/(3*(l))).^3);
else
v = 0;
x = l;
end
% create the state matrix
X(i,:) = [x,v];
end
%----------------------------------------------------------------------
elseif (l >= 1/6) && (l <= 1/4)
for i = 1:length(t)
if (t(i) <= 1/2)
v = 1 - 8*(1-3*l)*t(i) + 12*(1-4*l)*(t(i)).^2;
x = t(i) - 4*(1-3*l)*(t(i)).^2 + 4*(1-4*l)*(t(i)).^3;
else
v = -1 + 8*(1-3*l)*(1-t(i)) - 12*(1-4*l)*(1-t(i)).^2;
x = 1 - t(i) - 4*(1-3*l)*(1-t(i)).^2 + 4*(1-4*l)*(1-t(i)).^3;
end
% create the state matrix
X(i,:) = [x,v];
end
%----------------------------------------------------------------------
elseif (l >= 1/4)
for i = 1:length(t)
v = 1-2*t(i);
x = t(i)*(1-t(i));
% create the state matrix
X(i,:) = [x,v];
end
%----------------------------------------------------------------------
else
X = nan(length(t),2);
end
end