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

Implement compute_inputs to call fmincon

Browse Source 
- Call fmincon with cost function and constraints in `compute_inputs`
- Add function `get_ref_decision_variable` to construct the full
  decision variable for reference trajectory from reference
  states and inputs
This commit is contained in:
Sravan Balaji
2021-12-12 14:36:39 -05:00
parent 637112e60c
commit 2d3304ace1
1 changed files with 57 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
Experimentation/MPC_Class.m
View File

@@ -138,7 +138,42 @@ classdef MPC_Class
%compute_inputs Solves optimization problem to follow reference trajectory
% while avoiding obstacles and staying on the track
% TODO: Call fmincon here
% Initialize guess to follow reference trajectory perfectly (error = 0)
Z_err_init = zeros(obj.ndec, 1);
% Get constraint matrices
[A, B] = obj.linearized_bike_model();
[Aeq, beq] = obj.eq_cons(A, B);
[Lb, Ub] = obj.bound_cons();
Z_err = ...
fmincon( ...
@obj.cost_fun, ... % fun: Function to minimize
Z_err_init, ... % x0: Initial point
[], ... % A: Linear inequality constraints
[], ... % b: Linear inequality constraints
Aeq, ... % Aeq: Linear equality constraints
beq, ... % beq: Linear equality constraints
Lb, ... % lb: Lower bounds
Ub, ... % ub: Upper bounds
@obj.nonlcon, ... % nonlcon: Nonlinear constraints
obj.options ... % options: Optimization options
);
% NOTE: Error = Actual - Reference
% => Actual = Error + Reference
Z_ref = obj.get_ref_decision_variable();
Z = Z_err + Z_ref;
% Construct N-by-2 vector of control inputs with timestep 0.01 seconds
% to foward integrate vehicle dynamics for the next 0.5 seconds
Utemp = NaN(obj.npredinputs, obj.ninputs);
for i = 0:obj.npredinputs-1
idx = obj.get_input_start_idx(0);
Utemp(i+1,:) = Z(idx+1:idx+obj.ninputs);
end
FLAG_terminate = obj.FLAG_terminate;
end
end
@@ -266,24 +301,9 @@ classdef MPC_Class
% and obstacles applied to states
c = NaN(1, obj.npredstates);
% Construct reference decision variable from reference
% states and inputs
Z_ref = zeros(obj.ndec, 1);
for i = 0:obj.npredstates-1
start_idx = get_state_start_idx(i);
Z_ref(start_idx+1:start_idx+obj.nstates) ...
= obj.Y_ref(obj.ref_idx+i, :);
end
for i = 0:obj.npredinputs-1
start_idx = get_input_start_idx(i);
Z_ref(start_idx+1:start_idx+obj.ninputs) ...
= obj.U_ref(obj.ref_idx+i, :);
end
% NOTE: Error = Actual - Reference
% => Actual = Error + Reference
Z_ref = obj.get_ref_decision_variable();
Z = Z_err + Z_ref;
% Construct constraint for each state
@@ -511,6 +531,26 @@ classdef MPC_Class
% Get index of reference position closest to current position
[~, idx] = min(dist);
end
function Z_ref = get_ref_decision_variable(obj)
%get_ref_decision_variable Constructs decision variable
% from reference trajectory states and inputs starting
% at index closest to `Y_curr`
Z_ref = zeros(obj.ndec, 1);
for i = 0:obj.npredstates-1
start_idx = get_state_start_idx(i);
Z_ref(start_idx+1:start_idx+obj.nstates) ...
= obj.Y_ref(obj.ref_idx+i, :);
end
for i = 0:obj.npredinputs-1
start_idx = get_input_start_idx(i);
Z_ref(start_idx+1:start_idx+obj.ninputs) ...
= obj.U_ref(obj.ref_idx+i, :);
end
end
end
end