blob: 610043452c0660800059cb4c77b6f18daffd6059 (
plain)
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
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich.
%
% All rights reserved.
%
% Please see the LICENSE file that has been included as part of this package.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params)
% YOUR CODE HERE
% Hint: you can access the control command with ctrl.eval(x(:,i))
Xt = [x0];
Ut = [];
x = x0
ctrl_info = [];
% obj = LQR();
for i = 1:params.model.HorizonLength
[u, ctrl_info_] = ctrl.eval(x);
Ut = [Ut u];
ctrl_info = [ctrl_info ctrl_info_];
x = params.model.A*x + params.model.B*u;
Xt = [Xt x];
end
plot_trajectory(Xt, Ut, ctrl_info, params);
% Ut = u;
% Xt = x;
end
|
