diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m index 6494d2a..f645444 100644 --- a/Experimentation/MPC_Class.m +++ b/Experimentation/MPC_Class.m @@ -2,17 +2,21 @@ % Written by: Sravan Balaji, Xenia Demenchuk, and Peter Pongsachai % Created: 10 Dec 2021 classdef MPC_Class - %MPC_CLASS TODO: Summary of this class goes here - % TODO: Detailed explanation goes here + %MPC_CLASS Provides 2 public functions: + % 1. Constructor instantiates object with track + % and reference trajectory information + % 2. `compute_inputs` uses MPC to determine inputs + % to vehicle that will track reference trajectory + % while avoiding obstacles and staying on track % Miscellaneous Notes - % - Error in States = Actual - Reference - % - Y: State [x; u; y; v; psi; r] - % - Y_err: State Error [x - x_ref; u - u_ref; y - y_ref; v - v_ref; psi - psi_ref; r - r_ref] - % - U: Input [delta_f; F_x] - % - U_err: Input Error [delta_f - delta_f_ref; F_x - F_x_ref] - % - Z: Decision Variable [Y(0);...;Y(n+1);U(0);...;U(n)] - % - Z_err: Decision Variable Error [Y_err(0);...;Y_err(n+1);U_err(0);...;U_err(n)] + % - Error = Actual - Reference + % - Y: State [x; u; y; v; psi; r] + % - Y_err: State Error [x - x_ref; u - u_ref; y - y_ref; v - v_ref; psi - psi_ref; r - r_ref] + % - U: Input [delta_f; F_x] + % - U_err: Input Error [delta_f - delta_f_ref; F_x - F_x_ref] + % - Z: Decision Variable [Y(0);...;Y(n+1);U(0);...;U(n)] + % - Z_err: Decision Variable Error [Y_err(0);...;Y_err(n+1);U_err(0);...;U_err(n)] properties % Vehicle Parameters (Table 1) @@ -31,12 +35,12 @@ classdef MPC_Class g = 9.806; % Input Limits (Table 1) - delta_lims = [-0.5, 0.5]; - F_x_lims = [-5e3, 5e3]; + delta_lims = [-0.5, 0.5]; % [rad] + F_x_lims = [-5e3, 5e3]; % [N] - % Position Limits (Min/Max based on track) - x_lims = [ 200, 1600]; - y_lims = [-200, 1000]; + % Position Limits (Min/Max based on "map") + x_lims = [ 200, 1600]; % [m] + y_lims = [-200, 1000]; % [m] % Initial Conditions (Equation 15) state_init = [ ... @@ -53,15 +57,15 @@ classdef MPC_Class T_p = 0.5; % Prediction Horizon [s] % Decision Variables - npred = T_p / T_s; - nstates = 6; - ninputs = 2; - ndec = (npred+1)*nstates + npred*ninputs; + 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 % Track & Reference Trajectory Information - TestTrack; - Y_ref; - U_ref; + TestTrack; % Information on track boundaries and centerline + Y_ref; % States of reference trajectory + U_ref; % Inputs of reference trajectory % MPC Tunable Parameters Q = [ ... % State Error Costs @@ -78,65 +82,89 @@ classdef MPC_Class ]; end + % Public Functions methods (Access = public) function obj = MPC_Class(TestTrack, Y_ref, U_ref) - %MPC_CLASS Construct an instance of this class - % Store provided track & trajectory information + %MPC_CLASS Construct an instance of this class and + % store provided track & trajectory information obj.TestTrack = TestTrack; obj.Y_ref = Y_ref; obj.U_ref = U_ref; end function [Utemp, FLAG_terminate] = compute_inputs(obj, Xobs_seen, curr_state) - %compute_inputs TODO: Summary of this method goes here - % TODO: Detailed explanation goes here + %compute_inputs Solves optimization problem to follow reference trajectory + % while avoiding obstacles and staying on the track + + % TODO: Call fmincon here end end - % TODO: Constraint Functions + % Private Constraint Functions methods (Access = private) function [Lb, Ub] = bound_cons(obj, ref_idx) - % ref_idx is the index along reference trajectory that initial condition is at - Lb = -Inf(1, obj.ndec); - Ub = Inf(1, obj.ndec); + %bound_cons Construct lower and upper bounds on states and inputs + % using stored limits and reference trajectory at the index + % closest to `curr_state` + Lb = -Inf(1, obj.ndec); % Lower Bound + Ub = Inf(1, obj.ndec); % Upper Bound + + % NOTE: Error = Actual - Reference + % Limits are defined for "Actual" states and inputs, + % but our decision variable is the "Error". We have to + % correct for this by subtracting reference states and + % inputs from the "Actual" limits. + + % State Limits for i = 0:obj.npred start_idx = get_state_start_idx(i); - % x + % x position limits Lb(start_idx+1) = obj.x_lims(1) - obj.Y_ref(ref_idx+i, 1); Ub(start_idx+1) = obj.x_lims(2) - obj.Y_ref(ref_idx+i, 1); - % y + % y position limits Lb(start_idx+3) = obj.y_lims(1) - obj.Y_ref(ref_idx+i, 3); Ub(start_idx+3) = obj.y_lims(2) - obj.Y_ref(ref_idx+i, 3); end + % Input Limits for i = 0:obj.npred-1 start_idx = get_input_start_idx(i); - % delta_f + % delta_f input limits Lb(start_idx+1) = obj.delta_lims(1) - obj.U_ref(ref_idx+i, 1); Ub(start_idx+1) = obj.delta_lims(2) - obj.U_ref(ref_idx+i, 1); - % F_x + % F_x input limits Lb(start_idx+2) = obj.F_x_lims(1) - obj.U_ref(ref_idx+i, 2); Ub(start_idx+2) = obj.F_x_lims(2) - obj.U_ref(ref_idx+i, 2); end end end - % TODO: Helper Functions + % Private Helper Functions 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 + 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 + idx = (obj.npred+1)*obj.nstates + obj.ninputs*i; end function idx = get_ref_index(obj, curr_state) + %get_ref_index Finds index of position in reference trajectory + % that is closest (based on Euclidean distance) to position + % in `curr_state` + % Get position (x,y) from current state pos = [curr_state(1), curr_state(3)]; % Get positions (x,y) from reference trajectory