Cost Function for fmincon

- Add quadratic cost function using state costs (Q)
  and input costs (R)
- Use `npredstates` and `npredinputs` instead of `npred`
  when looping through states and inputs
- Update `get_state_start_idx` and `get_input_start_idx`
  descriptions to include assumption of `i` starting at 0

Experimentation/MPC_Class.m

@@ -142,6 +142,33 @@ classdef MPC_Class
         end
     end
 
+    %% Private Cost Function
+    methods (Access = private)
+        function J = cost_fun(obj, Z_err)
+            %cost_fun Compute cost of current Z_err based
+            %   on specified state cost (Q) and input cost (R).
+
+            % Create vector to hold cost values
+            H = NaN(obj.ndec, 1);
+
+            for i = 0:obj.npredstates-1
+                start_idx = get_state_start_idx(i);
+                H(start_idx+1:start_idx+obj.nstates) = obj.Q;
+            end
+
+            for i = 0:obj.npredinputs-1
+                start_idx = get_input_start_idx(i);
+                H(start_idx+1:start_idx+obj.ninputs) = obj.R;
+            end
+
+            % Create diagonal matrix from cost value vector
+            H = diag(H);
+
+            % Evaluate cost function (quadratic function)
+            J = Z_err.' * H * Z_err;
+        end
+    end
+
     %% Private Constraint Functions
     methods (Access = private)
         function [Lb, Ub] = bound_cons(obj)
@@ -159,7 +186,7 @@ classdef MPC_Class
             % inputs from the "Actual" limits.
 
             % State Limits
-            for i = 0:obj.npred
+            for i = 0:obj.npredstates-1
                 start_idx = get_state_start_idx(i);
 
                 % x position limits
@@ -172,7 +199,7 @@ classdef MPC_Class
             end
 
             % Input Limits
-            for i = 0:obj.npred-1
+            for i = 0:obj.npredinputs-1
                 start_idx = get_input_start_idx(i);
 
                 % delta_f input limits
@@ -453,14 +480,16 @@ classdef MPC_Class
     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
+            %   the full decision variable. Assumes `i` starts
+            %   at 0.
 
             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
+            %   the full decision variable. Assumes `i` starts
+            %   at 0.
 
             idx = (obj.npred+1)*obj.nstates + obj.ninputs*i;
         end