diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m
index 6fd4678..589570a 100644
--- a/Experimentation/MPC_Class.m
+++ b/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