sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-15 07:18:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Bunch of new properties, remove unnecessary function arguments

Browse Source 
- Add properties for prediction horizon of states & inputs
- Add properties for obstacles, current state, reference index,
  and termination flag
- Update constructor to take in and set new properties
- Move properties with calculated values to constructor
- Remove function inputs for new properties
- Rename `curr_state` to `Y_curr` in `MPC_Class.m`
- Use object properties instead of function arguments (e.g., obj.ref_idx)
This commit is contained in:
Sravan Balaji
2021-12-12 13:26:15 -05:00
parent fde6b62ac4
commit 96aa438587
2 changed files with 66 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Deliverables/ROB535_ControlProject_part2_Team3.m
View File

@@ -38,6 +38,6 @@ load('ROB535_ControlProject_part1_Team3.mat');
Y_ref = ROB535_ControlProject_part1_state; Y_ref = ROB535_ControlProject_part1_state;
U_ref = ROB535_ControlProject_part1_input; U_ref = ROB535_ControlProject_part1_input;
obj = MPC_Class(TestTrack, Y_ref, U_ref); obj = MPC_Class(TestTrack, Xobs_seen, curr_state, Y_ref, U_ref);
[Utemp, FLAG_terminate] = obj.compute_inputs(Xobs_seen, curr_state); [Utemp, FLAG_terminate] = obj.compute_inputs();
end end

101
Experimentation/MPC_Class.m
View File

@@ -58,17 +58,25 @@ classdef MPC_Class
T_p = 0.5; % Prediction Horizon [s] T_p = 0.5; % Prediction Horizon [s]
% Decision Variables % Decision Variables
npred = T_p / T_s; % Number of predictions nstates = 6; % Number of states per prediction
nstates = 6; % Number of states per prediction ninputs = 2; % Number of inputs 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 npred; % Number of predictions
TestTrack; % Information on track boundaries and centerline npredstates; % State prediction horizon
Y_ref; % States of reference trajectory npredinputs; % Input prediction horizon
U_ref; % Inputs of reference trajectory
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 % MPC Tunable Parameters
Q = [ ... % State Error Costs Q = [ ... % State Error Costs
@@ -87,15 +95,34 @@ classdef MPC_Class
%% Public Functions %% Public Functions
methods (Access = public) 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 %MPC_CLASS Construct an instance of this class and
% store provided track & trajectory information % store provided track, obstacle, & trajectory information
obj.TestTrack = TestTrack; obj.TestTrack = TestTrack;
obj.Y_ref = Y_ref; obj.Xobs_seen = Xobs_seen;
obj.U_ref = U_ref; 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 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 %compute_inputs Solves optimization problem to follow reference trajectory
% while avoiding obstacles and staying on the track % while avoiding obstacles and staying on the track
@@ -105,10 +132,10 @@ classdef MPC_Class
%% Private Constraint Functions %% Private Constraint Functions
methods (Access = private) 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 %bound_cons Construct lower and upper bounds on states and inputs
% using stored limits and reference trajectory at the index % using stored limits and reference trajectory at the index
% closest to `curr_state` % closest to `Y_curr`
Lb = -Inf(1, obj.ndec); % Lower Bound Lb = -Inf(1, obj.ndec); % Lower Bound
Ub = Inf(1, obj.ndec); % Upper Bound Ub = Inf(1, obj.ndec); % Upper Bound
@@ -124,12 +151,12 @@ classdef MPC_Class
start_idx = get_state_start_idx(i); start_idx = get_state_start_idx(i);
% x position limits % x position limits
Lb(start_idx+1) = obj.x_lims(1) - 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(ref_idx+i, 1); Ub(start_idx+1) = obj.x_lims(2) - obj.Y_ref(obj.ref_idx+i, 1);
% y position limits % y position limits
Lb(start_idx+3) = obj.y_lims(1) - 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(ref_idx+i, 3); Ub(start_idx+3) = obj.y_lims(2) - obj.Y_ref(obj.ref_idx+i, 3);
end end
% Input Limits % Input Limits
@@ -137,27 +164,27 @@ classdef MPC_Class
start_idx = get_input_start_idx(i); start_idx = get_input_start_idx(i);
% delta_f input limits % delta_f input limits
Lb(start_idx+1) = obj.delta_lims(1) - 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(ref_idx+i, 1); Ub(start_idx+1) = obj.delta_lims(2) - obj.U_ref(obj.ref_idx+i, 1);
% F_x input limits % F_x input limits
Lb(start_idx+2) = obj.F_x_lims(1) - 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(ref_idx+i, 2); Ub(start_idx+2) = obj.F_x_lims(2) - obj.U_ref(obj.ref_idx+i, 2);
end end
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 %eq_cons Construct equality constraint matrices for
% Euler discretization of linearized system % Euler discretization of linearized system
% using reference trajectory at the index closest % 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 % Build matrix for A_i*Y_i + B_i*U_i - Y_{i+1} = 0
% in the form Aeq*Z = beq % in the form Aeq*Z = beq
% Initial Condition % Initial Condition
% NOTE: Error = Actual - Reference % 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); Aeq = zeros(obj.ndecstates, obj.ndec);
beq = zeros(obj.ndecstates, 1); beq = zeros(obj.ndecstates, 1);
@@ -177,12 +204,12 @@ classdef MPC_Class
% A matrix for i % A matrix for i
Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ... Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ...
state_idxs(i)-obj.nstates:state_idxs(i)-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 % B matrix for i
Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ... Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ...
input_idxs(i):input_idxs(i)+obj.ninputs-1) ... input_idxs(i):input_idxs(i)+obj.ninputs-1) ...
= B(ref_idx+i-1); = B(obj.ref_idx+i-1);
end end
end end
end end
@@ -208,11 +235,11 @@ classdef MPC_Class
dYdt = [dx; du; dy; dv; dpsi; dr]; dYdt = [dx; du; dy; dv; dpsi; dr];
end 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 %linearized_bike_model Computes the discrete-time LTV
% system matrices of the nonlinear bike model linearized % system matrices of the nonlinear bike model linearized
% around the reference trajectory starting at the index % 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 syms x_var u_var y_var v_var psi_var r_var ... % states
delta_f_var F_x_var ... % inputs delta_f_var F_x_var ... % inputs
@@ -252,14 +279,14 @@ classdef MPC_Class
subs( ... subs( ...
A_c_symb, ... 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], ... [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( ... B_c = @(i) double( ...
subs( ... subs( ...
B_c_symb, ... 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], ... [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; idx = (obj.npred+1)*obj.nstates + obj.ninputs*i;
end 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 %get_ref_index Finds index of position in reference trajectory
% that is closest (based on Euclidean distance) to position % that is closest (based on Euclidean distance) to position
% in `curr_state` % in `Y_curr`
% Get position (x,y) from current state % 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 % Get positions (x,y) from reference trajectory
pos_ref = obj.Y_ref(:,[1,3]); pos_ref = obj.Y_ref(:,[1,3]);