diff --git a/Deliverables/ROB535_ControlProject_part2_Team3.m b/Deliverables/ROB535_ControlProject_part2_Team3.m index d2142e8..e94b570 100644 --- a/Deliverables/ROB535_ControlProject_part2_Team3.m +++ b/Deliverables/ROB535_ControlProject_part2_Team3.m @@ -38,6 +38,6 @@ load('ROB535_ControlProject_part1_Team3.mat'); Y_ref = ROB535_ControlProject_part1_state; U_ref = ROB535_ControlProject_part1_input; -obj = MPC_Class(TestTrack, Y_ref, U_ref); -[Utemp, FLAG_terminate] = obj.compute_inputs(Xobs_seen, curr_state); +obj = MPC_Class(TestTrack, Xobs_seen, curr_state, Y_ref, U_ref); +[Utemp, FLAG_terminate] = obj.compute_inputs(); end diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m index 3fa3f52..73616b1 100644 --- a/Experimentation/MPC_Class.m +++ b/Experimentation/MPC_Class.m @@ -58,17 +58,25 @@ 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 - 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 + nstates = 6; % Number of states per prediction + ninputs = 2; % Number of inputs per prediction - % Track & Reference Trajectory Information - TestTrack; % Information on track boundaries and centerline - Y_ref; % States of reference trajectory - U_ref; % Inputs of reference trajectory + npred; % Number of predictions + npredstates; % State prediction horizon + npredinputs; % Input prediction horizon + + ndecstates; % Total number of decision states + ndecinputs; % Total number of decision inputs + ndec; % Total number of decision variables + + % Track, Obstacle, & Reference Trajectory Information + TestTrack; % Information on track boundaries and centerline + Xobs_seen; % Information on seen obstacles + Y_curr; % Current state + Y_ref; % States of reference trajectory (Size = [steps, nstates]) + U_ref; % Inputs of reference trajectory (Size = [steps, ninputs]) + ref_idx; % Index of reference trajectory closest to `Y_curr` + FLAG_terminate; % Binary flag indicating simulation termination % MPC Tunable Parameters Q = [ ... % State Error Costs @@ -87,15 +95,34 @@ classdef MPC_Class %% Public Functions methods (Access = public) - function obj = MPC_Class(TestTrack, Y_ref, U_ref) + function obj = MPC_Class(TestTrack, Xobs_seen, Y_curr, Y_ref, U_ref) %MPC_CLASS Construct an instance of this class and - % store provided track & trajectory information + % store provided track, obstacle, & trajectory information obj.TestTrack = TestTrack; - obj.Y_ref = Y_ref; - obj.U_ref = U_ref; + obj.Xobs_seen = Xobs_seen; + obj.Y_curr = Y_curr; + obj.Y_ref = Y_ref; + obj.U_ref = U_ref; + obj.ref_idx = get_ref_index(); + + % Calculate decision variable related quantities + obj.npred = obj.T_p / obj.T_s; + obj.npredstates = npred + 1; + obj.npredinputs = npred; + + obj.ndecstates = npredstates * nstates; + obj.ndecinputs = npredinputs * ninputs; + obj.ndec = ndecstates + ndecinputs; + + % Check if `Y_curr` is within prediction horizon of + % the end of `Y_ref` + obj.FLAG_terminate = 0; + if size(obj.Y_ref, 1) - obj.ref_idx < obj.npred + obj.FLAG_terminate = 1; + end end - function [Utemp, FLAG_terminate] = compute_inputs(obj, Xobs_seen, curr_state) + function [Utemp, FLAG_terminate] = compute_inputs(obj) %compute_inputs Solves optimization problem to follow reference trajectory % while avoiding obstacles and staying on the track @@ -105,10 +132,10 @@ classdef MPC_Class %% Private Constraint Functions methods (Access = private) - function [Lb, Ub] = bound_cons(obj, ref_idx) + function [Lb, Ub] = bound_cons(obj) %bound_cons Construct lower and upper bounds on states and inputs % using stored limits and reference trajectory at the index - % closest to `curr_state` + % closest to `Y_curr` Lb = -Inf(1, obj.ndec); % Lower Bound Ub = Inf(1, obj.ndec); % Upper Bound @@ -124,12 +151,12 @@ classdef MPC_Class start_idx = get_state_start_idx(i); % 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); + Lb(start_idx+1) = obj.x_lims(1) - obj.Y_ref(obj.ref_idx+i, 1); + Ub(start_idx+1) = obj.x_lims(2) - obj.Y_ref(obj.ref_idx+i, 1); % 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); + Lb(start_idx+3) = obj.y_lims(1) - obj.Y_ref(obj.ref_idx+i, 3); + Ub(start_idx+3) = obj.y_lims(2) - obj.Y_ref(obj.ref_idx+i, 3); end % Input Limits @@ -137,27 +164,27 @@ classdef MPC_Class start_idx = get_input_start_idx(i); % 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); + Lb(start_idx+1) = obj.delta_lims(1) - obj.U_ref(obj.ref_idx+i, 1); + Ub(start_idx+1) = obj.delta_lims(2) - obj.U_ref(obj.ref_idx+i, 1); % 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); + Lb(start_idx+2) = obj.F_x_lims(1) - obj.U_ref(obj.ref_idx+i, 2); + Ub(start_idx+2) = obj.F_x_lims(2) - obj.U_ref(obj.ref_idx+i, 2); end end - function [Aeq, beq] = eq_cons(obj, ref_idx, curr_state, A, B) + function [Aeq, beq] = eq_cons(obj, A, B) %eq_cons Construct equality constraint matrices for % Euler discretization of linearized system % using reference trajectory at the index closest - % to `curr_state` + % to `Y_curr` % 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, :); + Y_err_init = obj.Y_curr - obj.Y_ref(obj.ref_idx, :); Aeq = zeros(obj.ndecstates, obj.ndec); beq = zeros(obj.ndecstates, 1); @@ -177,12 +204,12 @@ classdef MPC_Class % 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); + = A(obj.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); + = B(obj.ref_idx+i-1); end end end @@ -208,11 +235,11 @@ classdef MPC_Class dYdt = [dx; du; dy; dv; dpsi; dr]; end - function [A, B] = linearized_bike_model(obj, ref_idx) + function [A, B] = linearized_bike_model(obj) %linearized_bike_model Computes the discrete-time LTV % system matrices of the nonlinear bike model linearized % around the reference trajectory starting at the index - % closest to `curr_state` + % closest to `Y_curr` syms x_var u_var y_var v_var psi_var r_var ... % states delta_f_var F_x_var ... % inputs @@ -252,14 +279,14 @@ classdef MPC_Class subs( ... A_c_symb, ... [x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ... - bike_model_helper(obj.Y_ref(ref_idx+i,:), obj.U_ref(ref_idx+i,:)) ... + bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ... ) ... ); B_c = @(i) double( ... subs( ... B_c_symb, ... [x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ... - bike_model_helper(obj.Y_ref(ref_idx+i,:), obj.U_ref(ref_idx+i,:)) ... + bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ... ) ... ); @@ -333,13 +360,13 @@ classdef MPC_Class idx = (obj.npred+1)*obj.nstates + obj.ninputs*i; end - function idx = get_ref_index(obj, curr_state) + function idx = get_ref_index(obj) %get_ref_index Finds index of position in reference trajectory % that is closest (based on Euclidean distance) to position - % in `curr_state` + % in `Y_curr` % Get position (x,y) from current state - pos = [curr_state(1), curr_state(3)]; + pos = [obj.Y_curr(1), obj.Y_curr(3)]; % Get positions (x,y) from reference trajectory pos_ref = obj.Y_ref(:,[1,3]);