From d16e25ca85d029e8e7d183ccc555c855635e5704 Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Fri, 10 Dec 2021 08:05:47 -0500
Subject: [PATCH] Update part2 function to use `MPC_Class`

- Update part2 deliverable function to use `MPC_Class`
- Clean-up some comments in `MPC_Class`
- Add MPC tunable parameters to `MPC_Class`
- Add TODO comments
---
 .../ROB535_ControlProject_part2_Team3.m       | 12 ++--
 Experimentation/MPC_Class.m                   | 57 +++++++++++++------
 2 files changed, 47 insertions(+), 22 deletions(-)

diff --git a/Deliverables/ROB535_ControlProject_part2_Team3.m b/Deliverables/ROB535_ControlProject_part2_Team3.m
index 68d1c25..d2142e8 100644
--- a/Deliverables/ROB535_ControlProject_part2_Team3.m
+++ b/Deliverables/ROB535_ControlProject_part2_Team3.m
@@ -34,8 +34,10 @@ function [Utemp, FLAG_terminate] = ROB535_ControlProject_part2_Team3(TestTrack,X
 %             
 % Written by: Sravan Balaji, Xenia Demenchuk, and Peter Pongsachai
 % Created: 30 Nov 2021
-    Utemp = NaN(51,2);
-    Utemp(:,1) = 0 .* ones(51,1); % delta
-    Utemp(:,2) = 5000 .* ones(51,1); % F_x
-    FLAG_terminate = rand(1,1) > 0.9;
-end
\ No newline at end of file
+load('ROB535_ControlProject_part1_Team3.mat');
+Y_ref = ROB535_ControlProject_part1_state;
+U_ref = ROB535_ControlProject_part1_input;
+
+obj = MPC_Class(TestTrack, Y_ref, U_ref);
+[Utemp, FLAG_terminate] = obj.compute_inputs(Xobs_seen, curr_state);
+end
diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m
index f5226d6..fb45e1d 100644
--- a/Experimentation/MPC_Class.m
+++ b/Experimentation/MPC_Class.m
@@ -1,9 +1,18 @@
+% Filename: MPC_Class.m
 % Written by: Sravan Balaji, Xenia Demenchuk, and Peter Pongsachai
 % Created: 10 Dec 2021
-
 classdef MPC_Class
-    %MPC_CLASS Summary of this class goes here
-    %   Detailed explanation goes here
+    %MPC_CLASS TODO: Summary of this class goes here
+    %   TODO: Detailed explanation goes here
+
+    % Miscellaneous Notes
+    % - Error in States = Actual - Reference
+    % - Y: State [x; u; y; v; psi; r]
+    % - Y_err: State Error [x - x_ref; u - u_ref; y - y_ref; v - v_ref; psi - psi_ref; r - r_ref]
+    % - U: Input [delta_f; F_x]
+    % - U_err: Input Error [delta_f - delta_f_ref; F_x - F_x_ref]
+    % - Z: Decision Variable [Y(0);...;Y(n+1);U(0);...;U(n)]
+    % - Z_err: Decision Variable Error [Y_err(0);...;Y_err(n+1);U_err(0);...;U_err(n)]
     
     properties
         % Vehicle Parameters (Table 1)
@@ -31,12 +40,12 @@ classdef MPC_Class
 
         % Initial Conditions (Equation 15)
         state_init = [ ...
-            287; ...  % x [m]
-            5; ...    % u [m/s]
-            -176; ... % y [m]
-            0; ...    % v [m/s]
+            287; ...  % x   [m]
+            5; ...    % u   [m/s]
+            -176; ... % y   [m]
+            0; ...    % v   [m/s]
             2; ...    % psi [rad]
-            0; ...    % r [rad/s]
+            0; ...    % r   [rad/s]
         ];
 
         % Simulation Parameters
@@ -44,9 +53,6 @@ classdef MPC_Class
         T_p = 0.5;  % Prediction Horizon [s]
 
         % Decision Variables
-        % Y: State [x; u; y; v; psi; r]
-        % U: Input [delta_f; F_x]
-        % Z: Decision Variable [Y(0);...;Y(n+1);U(0);...;U(n)]
         npred = T_p / T_s;
         nstates = 6;
         ninputs = 2;
@@ -56,9 +62,23 @@ classdef MPC_Class
         TestTrack;
         Y_ref;
         U_ref;
+
+        % MPC Tunable Parameters
+        Q = [ ...  % State Error Costs
+            1; ... % x_err   [m]
+            1; ... % u_err   [m/s]
+            1; ... % y_err   [m]
+            1; ... % v_err   [m/s]
+            1; ... % psi_err [rad]
+            1; ... % r_err   [rad/s]
+        ];
+        R = [ ...     % Input Error Costs
+            0.1; ...  % delta_f_err [rad]
+            0.01; ... % F_x_err     [N]
+        ];
     end
     
-    methods
+    methods (Access = public)
         function obj = MPC_Class(TestTrack, Y_ref, U_ref)
             %MPC_CLASS Construct an instance of this class
             %   Store provided track & trajectory information
@@ -67,11 +87,14 @@ classdef MPC_Class
             obj.U_ref = U_ref;
         end
         
-        % function outputArg = method1(obj,inputArg)
-        %     %METHOD1 Summary of this method goes here
-        %     %   Detailed explanation goes here
-        %     outputArg = obj.Property1 + inputArg;
-        % end
+        function [Utemp, FLAG_terminate] = compute_inputs(obj, Xobs_seen, curr_state)
+            %compute_inputs TODO: Summary of this method goes here
+            %   TODO: Detailed explanation goes here
+        end
+    end
+
+    methods (Access = private)
+        % TODO: Add MPC related private functions
     end
 end