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

Add Euler Discretization Equality Constraint

Browse Source 
- Use problem 4 solution `eq_cons` function as baseline
- Add number of decision states/inputs as properties
This commit is contained in:
Sravan Balaji
2021-12-12 12:27:25 -05:00
parent 04d0734b48
commit fde6b62ac4
1 changed files with 46 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
Experimentation/MPC_Class.m
View File

@@ -58,10 +58,12 @@ 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 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
ndec = (npred+1)*nstates + npred*ninputs; % Total number of decision variables 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 % Track & Reference Trajectory Information
TestTrack; % Information on track boundaries and centerline TestTrack; % Information on track boundaries and centerline
@@ -143,6 +145,46 @@ classdef MPC_Class
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(ref_idx+i, 2);
end end
end end
function [Aeq, beq] = eq_cons(obj, ref_idx, curr_state, A, B)
%eq_cons Construct equality constraint matrices for
% Euler discretization of linearized system
% using reference trajectory at the index closest
% to `curr_state`
% 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, :);
Aeq = zeros(obj.ndecstates, obj.ndec);
beq = zeros(obj.ndecstates, 1);
Aeq(1:obj.nstates, 1:obj.nstates) = eye(obj.nstates);
beq(1:obj.nstates) = Y_err_init;
state_idxs = obj.nstates+1:obj.nstates:obj.ndecstates;
input_idxs = obj.ndecstates+1:obj.ninputs:obj.ndec;
for i = 1:obj.npred
% Negative identity for i+1
Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ...
state_idxs(i):state_idxs(i)+obj.nstates-1) ...
= -eye(obj.nstates);
% 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);
% 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);
end
end
end end
%% Private Kinematic Models %% Private Kinematic Models