Add Euler Discretization Equality Constraint

- Use problem 4 solution `eq_cons` function as baseline
- Add number of decision states/inputs as properties

Experimentation/MPC_Class.m

@@ -61,7 +61,9 @@ classdef MPC_Class
         npred      = T_p / T_s;               % Number of predictions
         nstates    = 6;                       % Number of states per prediction
         ninputs    = 2;                       % Number of inputs per prediction
-        ndec = (npred+1)*nstates + npred*ninputs; % Total number of decision variables
+        ndecstates = (npred+1)*nstates;       % Total number of decision states
+        ndecinputs = npred*ninputs;           % Total number of decision inputs
+        ndec       = ndecstates + ndecinputs; % Total number of decision variables
 
         % Track & Reference Trajectory Information
         TestTrack; % Information on track boundaries and centerline
@@ -143,6 +145,46 @@ classdef MPC_Class
                 Ub(start_idx+2) = obj.F_x_lims(2) - obj.U_ref(ref_idx+i, 2);
             end
         end
+
+        function [Aeq, beq] = eq_cons(obj, ref_idx, curr_state, A, B)
+            %eq_cons Construct equality constraint matrices for
+            %   Euler discretization of linearized system
+            %   using reference trajectory at the index closest
+            %   to `curr_state`
+
+            % Build matrix for A_i*Y_i + B_i*U_i - Y_{i+1} = 0
+            % in the form Aeq*Z = beq
+
+            % Initial Condition
+            % NOTE: Error = Actual - Reference
+            Y_err_init = curr_state - obj.Y_ref(ref_idx, :);
+
+            Aeq = zeros(obj.ndecstates, obj.ndec);
+            beq = zeros(obj.ndecstates, 1);
+
+            Aeq(1:obj.nstates, 1:obj.nstates) = eye(obj.nstates);
+            beq(1:obj.nstates) = Y_err_init;
+
+            state_idxs = obj.nstates+1:obj.nstates:obj.ndecstates;
+            input_idxs = obj.ndecstates+1:obj.ninputs:obj.ndec;
+
+            for i = 1:obj.npred
+                % Negative identity for i+1
+                Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ...
+                    state_idxs(i):state_idxs(i)+obj.nstates-1) ...
+                    = -eye(obj.nstates);
+
+                % A matrix for i
+                Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ...
+                    state_idxs(i)-obj.nstates:state_idxs(i)-1) ...
+                    = A(ref_idx+i-1);
+
+                % B matrix for i
+                Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ...
+                    input_idxs(i):input_idxs(i)+obj.ninputs-1) ...
+                    = B(ref_idx+i-1);
+            end
+        end
     end
 
     %% Private Kinematic Models