From 0105f2746c6dd8c3617f93fd3281f7e2708cae76 Mon Sep 17 00:00:00 2001
From: Nao Pross <np@0hm.ch>
Date: Wed, 24 May 2023 16:29:40 +0200
Subject: Take deliverables for from LQR to MPC from yuanxu

According to table 7 that is

  - lqr_maxPI
  - traj_cost
  - MPC
  - MPC/eval (contained in MPC.m)
---
 templates/traj_cost.m | 6 ++++++
 1 file changed, 6 insertions(+)

(limited to 'templates/traj_cost.m')

diff --git a/templates/traj_cost.m b/templates/traj_cost.m
index 7e0b443..57566c9 100644
--- a/templates/traj_cost.m
+++ b/templates/traj_cost.m
@@ -8,4 +8,10 @@
 
 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
-- 
cgit v1.2.1