1 files changed, 17 insertions, 0 deletions
diff --git a/Submission files/traj_cost.m b/Submission files/traj_cost.m
new file mode 100644
index 0000000..57566c9
--- /dev/null
+++ b/Submission files/traj_cost.m
@@ -0,0 +1,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
+\ No newline at end of file