summaryrefslogtreecommitdiffstats
path: root/templates/traj_cost.m
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