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

Boundary Constraints & Index Helper Functions

Browse Source 
- Add boundary constraints for staying on the "map"
  and staying within operating limits of vehicle inputs
- Add helper functions to get indices of things
This commit is contained in:
Sravan Balaji
2021-12-10 10:20:53 -05:00
parent d16e25ca85
commit 28fc4d8b47
1 changed files with 56 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
Experimentation/MPC_Class.m
View File

@@ -32,7 +32,7 @@ classdef MPC_Class
% Input Limits (Table 1)
delta_lims = [-0.5, 0.5];
Fx_lims = [-5e3, 5e3];
F_x_lims = [-5e3, 5e3];
% Position Limits (Min/Max based on track)
x_lims = [ 200, 1600];
@@ -93,8 +93,62 @@ classdef MPC_Class
end
end
% TODO: Constraint Functions
methods (Access = private)
% TODO: Add MPC related private functions
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);
for i = 0:obj.npred
start_idx = get_state_start_idx(i);
% x
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
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
for i = 0:obj.npred-1
start_idx = get_input_start_idx(i);
% delta_f
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
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
methods (Access = private)
function idx = get_state_start_idx(obj, i)
idx = obj.nstates*i;
end
function idx = get_input_start_idx(obj, i)
idx = (obj.npred+1)*obj.nstates + obj.ninputs*i;
end
function idx = get_ref_index(obj, curr_state)
% Get position (x,y) from current state
pos = [curr_state(1), curr_state(3)];
% Get positions (x,y) from reference trajectory
pos_ref = obj.Y_ref(:,[1,3]);
% Calculate Euclidean distance between current position and
% reference trajectory positions
dist = vecnorm(pos - pos_ref, 2, 2);
% Get index of reference position closest to current position
[~, idx] = min(dist);
end
end
end