/
forwardSimulation.m
143 lines (119 loc) · 3.79 KB
/
forwardSimulation.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
function [X,t,h] = forwardSimulation(f,x0,U,tf,N,intMethod,varargin)
%FORWARDSIMULATION - forward simulate an IVP with given dynamic system and
%specified method at equal time step
% [X,T] = FORWARDSIMULATION(F,X0,U,TF,N,INT) simulates a controlled
% dynamic system F forward in time with given initial states X0,
% a (m X N) control input trajectory, a final time TF, number of iterations N
% , and an integration method INT. The function return state trajectory X
% and time vector T.
%
% [X,T] = FORWARDSIMULATION(F,X0,U,TF,N,'given',MET) simulates a controlled
% dynamic system with the given integration method MET.
%
% [X,T,H] = FORWARDSIMULATION(F,X0,U,TF,N,...,'plot') simulates the
% controlled dynamic system, plot state and control input
% trajectories, and return state trajectory X, time vector T, and plot
% handle H.
%% validate attributes
validateattributes(f,{'function_handle'},{});
validateattributes(x0,{'numeric'},{'column','finite'});
validateattributes(U,{'numeric'},{'finite'});
validateattributes(tf,{'numeric'},{'scalar','finite'});
validateattributes(N,{'numeric'},{'integer','finite'});
% verify method's validity
listNotMATLABode = {'euler','runge-kutta','rk4','given'};
isMATLABode = true;
for i = 1:numel(listNotMATLABode)
isMATLABode = isMATLABode && ~strcmpi(intMethod,listNotMATLABode{i});
end
if strcmpi(intMethod,'euler')
update = @(f,x,u,t,dt)eulerMethod(f,x,u,t,dt);
elseif strcmpi(intMethod,'runge-kutta')||strcmpi(intMethod,'rk4')
update = @(f,x,u,t,dt)rungeKuttaMethod(f,x,u,t,dt);
elseif strcmpi(intMethod,'given')
if numel(varargin) == 2
update = varargin{1};
elseif numel(varargin) == 1
if strcmpi(varargin{:},'plot')
error('method is not given');
else
validateattributes(varargin{:},{'function_handle'},{});
update = varargin{:};
end
elseif numel(varargin) == 0
error('method is not given');
else
error('too many parameters');
end
else
error('invalid update method');
end
if size(U,2)~=N
error('Given control input trajectory must have length of N');
end
t = linspace(0,tf,N+1);
dt = t(2)-t(1);
if (size(x0,1)~=size(update(f,x0,U(:,1),t(1),dt),1))||(size(x0,2)~=size(update(f,x0,U(:,1),t(1),dt),2))
error('Update method must return vector f the same size of x0');
end
%% iterations
n = size(x0,1);
m = size(U,1);
X = zeros(n,N+1);
X(:,1) = x0;
for k = 1:N,
X(:,k+1) = update(f,X(:,k),U(:,k),t(:,k),dt);
end
%% Visualization
% obatin plotting command
if numel(varargin) == 2
if strcmpi(varargin{2},'plot')
doPlot = true;
else
error('invalid plot command')
end
elseif numel(varargin) == 1
if strcmpi(intMethod,'given')
doPlot = false;
else
if strcmpi(varargin{1},'plot')
doPlot = true;
elseif isempty(varargin{1})
doPlot = false;
else
error('invalid plot command');
end
end
elseif numel(varargin) == 0
doPlot = false;
else
error('too many input parameters');
end
if doPlot
h = plotXU(t,X,U);
else
h = {};
end
end
function h = plotXU(t,X,U)
%PLOTXU - internal function : plots state and control input trajectories
n = size(X,1);
m = size(U,1);
scrsz = get(groot,'ScreenSize');
% State Trajectory
figure('Position',[50 scrsz(4)/5 scrsz(3)/2-50 3*scrsz(4)/5],'Name','State Trajectory');
labelY = cell(1,n);
for i = 1:n,
labelY{i} = sprintf('x_{%d}',i);
end
h1 = plotVector(t,X,'State Trajectory : x(t)','t : time',labelY);
% Control Input Trajectory
figure('Position',[scrsz(3)/2 scrsz(4)/5 scrsz(3)/2-50 3*scrsz(4)/5],'Name','Control Input Trajectory');
labelY = cell(1,m);
for i = 1:m,
labelY{i} = sprintf('u_{%d}',i);
end
h2 = plotVector(t(1:end-1),U,'Control Input Trajectory','t : time',labelY);
h(1) = h1;
h(2) = h2;
end