From b562360f6b4118dd0fb21a9f2449e9ac608a87aa Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Fri, 10 Dec 2021 07:31:13 -0500
Subject: [PATCH] Start working on MPC Class

- Add `MPC_Class` to perform all MPC related computation
  for use with part 2's deliverable function
---
 Experimentation/MPC_Class.m | 77 +++++++++++++++++++++++++++++++++++++
 1 file changed, 77 insertions(+)
 create mode 100644 Experimentation/MPC_Class.m

diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m
new file mode 100644
index 0000000..f5226d6
--- /dev/null
+++ b/Experimentation/MPC_Class.m
@@ -0,0 +1,77 @@
+% 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
+    
+    properties
+        % Vehicle Parameters (Table 1)
+        Nw  = 2;
+        f   = 0.01;
+        Iz  = 2667;
+        a   = 1.35;
+        b   = 1.45;
+        By  = 0.27;
+        Cy  = 1.2;
+        Dy  = 0.7;
+        Ey  = -1.6;
+        Shy = 0;
+        Svy = 0;
+        m   = 1400;
+        g   = 9.806;
+
+        % Input Limits (Table 1)
+        delta_lims = [-0.5, 0.5];
+        Fx_lims    = [-5e3, 5e3];
+
+        % Position Limits (Min/Max based on track)
+        x_lims = [ 200, 1600];
+        y_lims = [-200, 1000];
+
+        % Initial Conditions (Equation 15)
+        state_init = [ ...
+            287; ...  % x [m]
+            5; ...    % u [m/s]
+            -176; ... % y [m]
+            0; ...    % v [m/s]
+            2; ...    % psi [rad]
+            0; ...    % r [rad/s]
+        ];
+
+        % Simulation Parameters
+        T_s = 0.01; % Step Size [s]
+        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;
+        ndec = (npred+1)*nstates + npred*ninputs;
+
+        % Track & Reference Trajectory Information
+        TestTrack;
+        Y_ref;
+        U_ref;
+    end
+    
+    methods
+        function obj = MPC_Class(TestTrack, Y_ref, U_ref)
+            %MPC_CLASS Construct an instance of this class
+            %   Store provided track & trajectory information
+            obj.TestTrack = TestTrack;
+            obj.Y_ref = Y_ref;
+            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
+    end
+end
+