blob: 57566c927577dbf553a287e4f9fed7328c606dd0 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 J_Nt = traj_cost(Xt,Ut,Q,R)
% YOUR CODE HERE
J_Nt=0;
for i=1:length(Xt(1,:))-1
a= Xt(:,i)'*Q*Xt(:,i);
b= Ut(:,i)'*R*Ut(:,i);
J_Nt=J_Nt+a+b;
end
end
|
