From fde6b62ac4e416a4bbae03910fd474ddde1b0396 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Sun, 12 Dec 2021 12:27:25 -0500 Subject: [PATCH] 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 | 50 ++++++++++++++++++++++++++++++++++--- 1 file changed, 46 insertions(+), 4 deletions(-) diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m index 3e48aa8..3fa3f52 100644 --- a/Experimentation/MPC_Class.m +++ b/Experimentation/MPC_Class.m @@ -58,10 +58,12 @@ classdef MPC_Class T_p = 0.5; % Prediction Horizon [s] % Decision Variables - 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 + npred = T_p / T_s; % Number of predictions + nstates = 6; % Number of states per prediction + ninputs = 2; % Number of inputs per prediction + 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