From 6361cfd40cf214a5ba751136dc896b9babcc6ea4 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Sun, 12 Dec 2021 14:50:11 -0500 Subject: [PATCH] Fix Missing References & Add `clamp` Function - Change `SpecifyObjectiveGradient` to `false` - Fix missing references to `obj` - Add missing `clamp` function --- Experimentation/MPC_Class.m | 45 ++++++++++++++++++++++++------------- 1 file changed, 30 insertions(+), 15 deletions(-) diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m index b1ec44a..122940a 100644 --- a/Experimentation/MPC_Class.m +++ b/Experimentation/MPC_Class.m @@ -95,7 +95,7 @@ classdef MPC_Class optimoptions( ... 'fmincon', ... 'SpecifyConstraintGradient', false, ... - 'SpecifyObjectiveGradient', true ... + 'SpecifyObjectiveGradient', false ... ); % Constraint Parameters @@ -115,16 +115,16 @@ classdef MPC_Class obj.Y_curr = Y_curr; obj.Y_ref = Y_ref; obj.U_ref = U_ref; - obj.ref_idx = get_ref_index(); + obj.ref_idx = obj.get_ref_index(); % Calculate decision variable related quantities obj.npred = obj.T_p / obj.T_s; - obj.npredstates = npred + 1; - obj.npredinputs = npred; + obj.npredstates = obj.npred + 1; + obj.npredinputs = obj.npred; - obj.ndecstates = npredstates * nstates; - obj.ndecinputs = npredinputs * ninputs; - obj.ndec = ndecstates + ndecinputs; + obj.ndecstates = obj.npredstates * obj.nstates; + obj.ndecinputs = obj.npredinputs * obj.ninputs; + obj.ndec = obj.ndecstates + obj.ndecinputs; % Check if `Y_curr` is within prediction horizon of % the end of `Y_ref` @@ -320,10 +320,10 @@ classdef MPC_Class % Use closest boundary points +/- ntrack_boundary_pts % to construct track polygon - bl_idx_start = clamp(bl_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2)); - bl_idx_end = clamp(bl_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2)); - br_idx_start = clamp(br_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2)); - br_idx_end = clamp(br_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2)); + bl_idx_start = obj.clamp(bl_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2)); + bl_idx_end = obj.clamp(bl_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2)); + br_idx_start = obj.clamp(br_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2)); + br_idx_end = obj.clamp(br_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2)); boundary_pts = [ ... obj.TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ... @@ -431,14 +431,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(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ... + obj.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(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ... + obj.bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ... ) ... ); @@ -491,8 +491,8 @@ classdef MPC_Class end % Apply input limits (Table 1) - delta_f = clamp(delta_f, obj.delta_lims(1), obj.delta_lims(2)); - F_x = clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2)); + delta_f = obj.clamp(delta_f, obj.delta_lims(1), obj.delta_lims(2)); + F_x = obj.clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2)); end end @@ -552,5 +552,20 @@ classdef MPC_Class end end end + + %% Static Helper Functions + methods (Static) + function clamped_val = clamp(val, min, max) + %clamp Limits a value to range: min <= val <= max + + clamped_val = val; + + if clamped_val < min + clamped_val = min; + elseif clamped_val > max + clamped_val = max; + end + end + end end