Comments & Clean-Up

- Add more comments describing class, functions, variables, etc.
- Remove some unnecessary TODO comments

Experimentation/MPC_Class.m

@@ -2,17 +2,21 @@
 % Written by: Sravan Balaji, Xenia Demenchuk, and Peter Pongsachai
 % Created: 10 Dec 2021
 classdef MPC_Class
-    %MPC_CLASS TODO: Summary of this class goes here
+    %MPC_CLASS Provides 2 public functions:
-    %   TODO: Detailed explanation goes here
+    %   1. Constructor instantiates object with track
+    %      and reference trajectory information
+    %   2. `compute_inputs` uses MPC to determine inputs
+    %      to vehicle that will track reference trajectory
+    %      while avoiding obstacles and staying on track
 
     % Miscellaneous Notes
-    % - Error in States = Actual - Reference
+    %   - Error = Actual - Reference
-    % - Y: State [x; u; y; v; psi; r]
+    %   - 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]
+    %   - 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: Input [delta_f; F_x]
-    % - U_err: Input Error [delta_f - delta_f_ref; F_x - F_x_ref]
+    %   - 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: 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)]
+    %   - 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 +35,12 @@ classdef MPC_Class
         g   = 9.806;
 
         % Input Limits (Table 1)
-        delta_lims = [-0.5, 0.5];
+        delta_lims = [-0.5, 0.5]; % [rad]
-        F_x_lims   = [-5e3, 5e3];
+        F_x_lims   = [-5e3, 5e3]; % [N]
 
-        % Position Limits (Min/Max based on track)
+        % Position Limits (Min/Max based on "map")
-        x_lims = [ 200, 1600];
+        x_lims = [ 200, 1600]; % [m]
-        y_lims = [-200, 1000];
+        y_lims = [-200, 1000]; % [m]
 
         % Initial Conditions (Equation 15)
         state_init = [ ...
@@ -53,15 +57,15 @@ classdef MPC_Class
         T_p = 0.5;  % Prediction Horizon [s]
 
         % Decision Variables
-        npred = T_p / T_s;
+        npred = T_p / T_s;                        % Number of predictions
-        nstates = 6;
+        nstates = 6;                              % Number of states per prediction
-        ninputs = 2;
+        ninputs = 2;                              % Number of inputs per prediction
-        ndec = (npred+1)*nstates + npred*ninputs;
+        ndec = (npred+1)*nstates + npred*ninputs; % Total number of decision variables
 
         % Track & Reference Trajectory Information
-        TestTrack;
+        TestTrack; % Information on track boundaries and centerline
-        Y_ref;
+        Y_ref;     % States of reference trajectory
-        U_ref;
+        U_ref;     % Inputs of reference trajectory
 
         % MPC Tunable Parameters
         Q = [ ...  % State Error Costs
@@ -78,65 +82,89 @@ classdef MPC_Class
         ];
     end
     
+    % Public Functions
     methods (Access = public)
         function obj = MPC_Class(TestTrack, Y_ref, U_ref)
-            %MPC_CLASS Construct an instance of this class
+            %MPC_CLASS Construct an instance of this class and
-            %   Store provided track & trajectory information
+            %   store provided track & trajectory information
             obj.TestTrack = TestTrack;
             obj.Y_ref = Y_ref;
             obj.U_ref = U_ref;
         end
         
         function [Utemp, FLAG_terminate] = compute_inputs(obj, Xobs_seen, curr_state)
-            %compute_inputs TODO: Summary of this method goes here
+            %compute_inputs Solves optimization problem to follow reference trajectory
-            %   TODO: Detailed explanation goes here
+            %   while avoiding obstacles and staying on the track
+
+            % TODO: Call fmincon here
         end
     end
 
-    % TODO: Constraint Functions
+    % Private Constraint Functions
     methods (Access = private)
         function [Lb, Ub] = bound_cons(obj, ref_idx)
-            % ref_idx is the index along reference trajectory that initial condition is at
+            %bound_cons Construct lower and upper bounds on states and inputs
-            Lb = -Inf(1, obj.ndec);
+            %   using stored limits and reference trajectory at the index
-            Ub =  Inf(1, obj.ndec);
+            %   closest to `curr_state`
 
+            Lb = -Inf(1, obj.ndec); % Lower Bound
+            Ub =  Inf(1, obj.ndec); % Upper Bound
+
+            % NOTE: Error = Actual - Reference
+            % Limits are defined for "Actual" states and inputs,
+            % but our decision variable is the "Error". We have to
+            % correct for this by subtracting reference states and
+            % inputs from the "Actual" limits.
+
+            % State Limits
             for i = 0:obj.npred
                 start_idx = get_state_start_idx(i);
 
-                % x
+                % x position limits
                 Lb(start_idx+1) = obj.x_lims(1) - obj.Y_ref(ref_idx+i, 1);
                 Ub(start_idx+1) = obj.x_lims(2) - obj.Y_ref(ref_idx+i, 1);
 
-                % y
+                % y position limits
                 Lb(start_idx+3) = obj.y_lims(1) - obj.Y_ref(ref_idx+i, 3);
                 Ub(start_idx+3) = obj.y_lims(2) - obj.Y_ref(ref_idx+i, 3);
             end
 
+            % Input Limits
             for i = 0:obj.npred-1
                 start_idx = get_input_start_idx(i);
 
-                % delta_f
+                % delta_f input limits
                 Lb(start_idx+1) = obj.delta_lims(1) - obj.U_ref(ref_idx+i, 1);
                 Ub(start_idx+1) = obj.delta_lims(2) - obj.U_ref(ref_idx+i, 1);
 
-                % F_x
+                % F_x input limits
                 Lb(start_idx+2) = obj.F_x_lims(1) - obj.U_ref(ref_idx+i, 2);
                 Ub(start_idx+2) = obj.F_x_lims(2) - obj.U_ref(ref_idx+i, 2);
             end
         end
     end
 
-    % TODO: Helper Functions
+    % Private Helper Functions
     methods (Access = private)
         function idx = get_state_start_idx(obj, i)
+            %get_state_start_idx Calculates starting index of state i in
+            %   the full decision variable
+
             idx = obj.nstates*i;
         end
 
         function idx = get_input_start_idx(obj, i)
+            %get_input_start_idx Calculates starting index of input i in
+            %   the full decision variable
+
             idx = (obj.npred+1)*obj.nstates + obj.ninputs*i;
         end
 
         function idx = get_ref_index(obj, curr_state)
+            %get_ref_index Finds index of position in reference trajectory
+            %   that is closest (based on Euclidean distance) to position
+            %   in `curr_state`
+
             % Get position (x,y) from current state
             pos = [curr_state(1), curr_state(3)];
             % Get positions (x,y) from reference trajectory