Implement compute_inputs to call fmincon

- Call fmincon with cost function and constraints in `compute_inputs`
- Add function `get_ref_decision_variable` to construct the full
  decision variable for reference trajectory from reference
  states and inputs

Experimentation/MPC_Class.m

@@ -138,7 +138,42 @@ classdef MPC_Class
             %compute_inputs Solves optimization problem to follow reference trajectory
             %   while avoiding obstacles and staying on the track
 
-            % TODO: Call fmincon here
+            % Initialize guess to follow reference trajectory perfectly (error = 0)
+            Z_err_init = zeros(obj.ndec, 1);
+
+            % Get constraint matrices
+            [A, B]     = obj.linearized_bike_model();
+            [Aeq, beq] = obj.eq_cons(A, B);
+            [Lb, Ub]   = obj.bound_cons();
+
+            Z_err = ...
+                fmincon( ...
+                    @obj.cost_fun, ... % fun:     Function to minimize
+                    Z_err_init, ...    % x0:      Initial point
+                    [], ...            % A:       Linear inequality constraints
+                    [], ...            % b:       Linear inequality constraints
+                    Aeq, ...           % Aeq:     Linear equality constraints
+                    beq, ...           % beq:     Linear equality constraints
+                    Lb, ...            % lb:      Lower bounds
+                    Ub, ...            % ub:      Upper bounds
+                    @obj.nonlcon, ...  % nonlcon: Nonlinear constraints
+                    obj.options ...    % options: Optimization options
+            );
+
+            % NOTE: Error = Actual - Reference
+            %    => Actual = Error + Reference
+            Z_ref = obj.get_ref_decision_variable();
+            Z = Z_err + Z_ref;
+
+            % Construct N-by-2 vector of control inputs with timestep 0.01 seconds
+            % to foward integrate vehicle dynamics for the next 0.5 seconds
+            Utemp = NaN(obj.npredinputs, obj.ninputs);
+            for i = 0:obj.npredinputs-1
+                idx = obj.get_input_start_idx(0);
+                Utemp(i+1,:) = Z(idx+1:idx+obj.ninputs);
+            end
+
+            FLAG_terminate = obj.FLAG_terminate;
         end
     end
 
@@ -266,24 +301,9 @@ classdef MPC_Class
             % and obstacles applied to states
             c = NaN(1, obj.npredstates);
 
-            % Construct reference decision variable from reference
-            % states and inputs
-            Z_ref = zeros(obj.ndec, 1);
-
-            for i = 0:obj.npredstates-1
-                start_idx = get_state_start_idx(i);
-                Z_ref(start_idx+1:start_idx+obj.nstates) ...
-                    = obj.Y_ref(obj.ref_idx+i, :);
-            end
-
-            for i = 0:obj.npredinputs-1
-                start_idx = get_input_start_idx(i);
-                Z_ref(start_idx+1:start_idx+obj.ninputs) ...
-                    = obj.U_ref(obj.ref_idx+i, :);
-            end
-
             % NOTE: Error = Actual - Reference
             %    => Actual = Error + Reference
+            Z_ref = obj.get_ref_decision_variable();
             Z = Z_err + Z_ref;
 
             % Construct constraint for each state
@@ -511,6 +531,26 @@ classdef MPC_Class
             % Get index of reference position closest to current position
             [~, idx] = min(dist);
         end
+
+        function Z_ref = get_ref_decision_variable(obj)
+            %get_ref_decision_variable Constructs decision variable
+            %   from reference trajectory states and inputs starting
+            %   at index closest to `Y_curr`
+
+            Z_ref = zeros(obj.ndec, 1);
+
+            for i = 0:obj.npredstates-1
+                start_idx = get_state_start_idx(i);
+                Z_ref(start_idx+1:start_idx+obj.nstates) ...
+                    = obj.Y_ref(obj.ref_idx+i, :);
+            end
+
+            for i = 0:obj.npredinputs-1
+                start_idx = get_input_start_idx(i);
+                Z_ref(start_idx+1:start_idx+obj.ninputs) ...
+                    = obj.U_ref(obj.ref_idx+i, :);
+            end
+        end
     end
 end