mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-30 13:23:14 +00:00
File Renaming / Reorganization & Minor Changes
- Add 1/2 coefficient to cost function - Move `MPC_Class.m` to "Deliverables" folder - Modify `part2_test_controller.m` plotting style a bit - Rename `part2_MPC_controller.m` to `sravan_MPC_controller.m`
This commit is contained in:
Sravan Balaji
557
Deliverables/MPC_Class.m
Normal file
557
Deliverables/MPC_Class.m
Normal file
@@ -0,0 +1,557 @@
|
||||
% Filename: MPC_Class.m
|
||||
% Written by: Sravan Balaji, Xenia Demenchuk, and Peter Pongsachai
|
||||
% Created: 10 Dec 2021
|
||||
classdef MPC_Class
|
||||
%MPC_CLASS Provides 2 public functions:
|
||||
% 1. Constructor instantiates object with track
|
||||
% and reference trajectory information
|
||||
% 2. `compute_inputs` uses MPC to determine inputs
|
||||
% to vehicle that will track reference trajectory
|
||||
% while avoiding obstacles and staying on track
|
||||
|
||||
% Miscellaneous Notes
|
||||
% - Error = Actual - Reference
|
||||
% - Y: State [x; u; y; v; psi; r]
|
||||
% - Y_err: State Error [x - x_ref; u - u_ref; y - y_ref; v - v_ref; psi - psi_ref; r - r_ref]
|
||||
% - U: Input [delta_f; F_x]
|
||||
% - U_err: Input Error [delta_f - delta_f_ref; F_x - F_x_ref]
|
||||
% - Z: Decision Variable [Y(0);...;Y(n+1);U(0);...;U(n)]
|
||||
% - Z_err: Decision Variable Error [Y_err(0);...;Y_err(n+1);U_err(0);...;U_err(n)]
|
||||
|
||||
%% Internal Variables
|
||||
properties
|
||||
% Vehicle Parameters (Table 1)
|
||||
Nw = 2;
|
||||
f = 0.01;
|
||||
Iz = 2667;
|
||||
a = 1.35;
|
||||
b = 1.45;
|
||||
By = 0.27;
|
||||
Cy = 1.2;
|
||||
Dy = 0.7;
|
||||
Ey = -1.6;
|
||||
Shy = 0;
|
||||
Svy = 0;
|
||||
m = 1400;
|
||||
g = 9.806;
|
||||
|
||||
% Input Limits (Table 1)
|
||||
delta_lims = [-0.5, 0.5]; % [rad]
|
||||
F_x_lims = [-5e3, 5e3]; % [N]
|
||||
|
||||
% Position Limits (Min/Max based on "map")
|
||||
x_lims = [ 200, 1600]; % [m]
|
||||
y_lims = [-200, 1000]; % [m]
|
||||
|
||||
% Initial Conditions (Equation 15)
|
||||
state_init = [ ...
|
||||
287; ... % x [m]
|
||||
5; ... % u [m/s]
|
||||
-176; ... % y [m]
|
||||
0; ... % v [m/s]
|
||||
2; ... % psi [rad]
|
||||
0; ... % r [rad/s]
|
||||
];
|
||||
|
||||
% Simulation Parameters
|
||||
T_s = 0.01; % Step Size [s]
|
||||
T_p = 0.5; % Prediction Horizon [s]
|
||||
|
||||
% Decision Variables
|
||||
nstates = 6; % Number of states per prediction
|
||||
ninputs = 2; % Number of inputs per prediction
|
||||
|
||||
npred; % Number of predictions
|
||||
npredstates; % State prediction horizon
|
||||
npredinputs; % Input prediction horizon
|
||||
|
||||
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 & Optimization Parameters
|
||||
Q = [ ... % State Error Costs
|
||||
1; ... % x_err [m]
|
||||
1; ... % u_err [m/s]
|
||||
1; ... % y_err [m]
|
||||
1; ... % v_err [m/s]
|
||||
1; ... % psi_err [rad]
|
||||
1; ... % r_err [rad/s]
|
||||
];
|
||||
R = [ ... % Input Error Costs
|
||||
0.1; ... % delta_f_err [rad]
|
||||
0.01; ... % F_x_err [N]
|
||||
];
|
||||
options = ...
|
||||
optimoptions( ...
|
||||
'fmincon', ...
|
||||
'SpecifyConstraintGradient', false, ...
|
||||
'SpecifyObjectiveGradient', false ...
|
||||
);
|
||||
|
||||
% Constraint Parameters
|
||||
ntrack_boundary_pts = 1; % Number of track boundary points
|
||||
% around closest boundary point
|
||||
% to use when checking if position
|
||||
% is within track polygon
|
||||
end
|
||||
|
||||
%% Public Functions
|
||||
methods (Access = public)
|
||||
function obj = MPC_Class(TestTrack, Xobs_seen, Y_curr, Y_ref, U_ref)
|
||||
%MPC_CLASS Construct an instance of this class and
|
||||
% store provided track, obstacle, & trajectory information
|
||||
obj.TestTrack = TestTrack;
|
||||
obj.Xobs_seen = Xobs_seen;
|
||||
obj.Y_curr = Y_curr;
|
||||
obj.Y_ref = Y_ref;
|
||||
obj.U_ref = U_ref;
|
||||
obj.ref_idx = obj.get_ref_index();
|
||||
|
||||
% Calculate decision variable related quantities
|
||||
obj.npred = obj.T_p / obj.T_s;
|
||||
obj.npredstates = obj.npred + 1;
|
||||
obj.npredinputs = obj.npred + 1;
|
||||
|
||||
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`
|
||||
obj.FLAG_terminate = 0;
|
||||
if size(obj.Y_ref, 1) - obj.ref_idx < obj.npred
|
||||
obj.FLAG_terminate = 1;
|
||||
end
|
||||
end
|
||||
|
||||
function [Utemp, FLAG_terminate] = compute_inputs(obj)
|
||||
%compute_inputs Solves optimization problem to follow reference trajectory
|
||||
% while avoiding obstacles and staying on the track
|
||||
|
||||
% 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
|
||||
|
||||
%% Private Cost Function
|
||||
methods (Access = private)
|
||||
function J = cost_fun(obj, Z_err)
|
||||
%cost_fun Compute cost of current Z_err based
|
||||
% on specified state cost (Q) and input cost (R).
|
||||
|
||||
% Create vector to hold cost values
|
||||
H = NaN(obj.ndec, 1);
|
||||
|
||||
for i = 0:obj.npredstates-1
|
||||
start_idx = obj.get_state_start_idx(i);
|
||||
H(start_idx+1:start_idx+obj.nstates) = obj.Q;
|
||||
end
|
||||
|
||||
for i = 0:obj.npredinputs-1
|
||||
start_idx = obj.get_input_start_idx(i);
|
||||
H(start_idx+1:start_idx+obj.ninputs) = obj.R;
|
||||
end
|
||||
|
||||
% Create diagonal matrix from cost value vector
|
||||
H = diag(H);
|
||||
|
||||
% Evaluate cost function (quadratic function)
|
||||
J = 0.5 * Z_err.' * H * Z_err;
|
||||
end
|
||||
end
|
||||
|
||||
%% Private Constraint Functions
|
||||
methods (Access = private)
|
||||
function [Lb, Ub] = bound_cons(obj)
|
||||
%bound_cons Construct lower and upper bounds on states and inputs
|
||||
% using stored limits and reference trajectory at the index
|
||||
% closest to `Y_curr`
|
||||
|
||||
Lb = -Inf(1, obj.ndec); % Lower Bound
|
||||
Ub = Inf(1, obj.ndec); % Upper Bound
|
||||
|
||||
% NOTE: Error = Actual - Reference
|
||||
% Limits are defined for "Actual" states and inputs,
|
||||
% but our decision variable is the "Error". We have to
|
||||
% correct for this by subtracting reference states and
|
||||
% inputs from the "Actual" limits.
|
||||
|
||||
% State Limits
|
||||
for i = 0:obj.npredstates-1
|
||||
start_idx = obj.get_state_start_idx(i);
|
||||
|
||||
% x position limits
|
||||
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(obj.ref_idx+i, 1);
|
||||
|
||||
% y position limits
|
||||
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(obj.ref_idx+i, 3);
|
||||
end
|
||||
|
||||
% Input Limits
|
||||
for i = 0:obj.npredinputs-1
|
||||
start_idx = obj.get_input_start_idx(i);
|
||||
|
||||
% delta_f input limits
|
||||
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(obj.ref_idx+i, 1);
|
||||
|
||||
% F_x input limits
|
||||
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(obj.ref_idx+i, 2);
|
||||
end
|
||||
end
|
||||
|
||||
function [Aeq, beq] = eq_cons(obj, A, B)
|
||||
%eq_cons Construct equality constraint matrices for
|
||||
% Euler discretization of linearized system
|
||||
% using reference trajectory at the index closest
|
||||
% to `Y_curr`
|
||||
|
||||
% 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 = obj.Y_curr - obj.Y_ref(obj.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(obj.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(obj.ref_idx+i-1);
|
||||
end
|
||||
end
|
||||
|
||||
function [c, ceq] = nonlcon(obj, Z_err)
|
||||
%nonlcon Construct nonlinear constraints for
|
||||
% vehicle to stay within track bounds and
|
||||
% outside of obstacles using reference
|
||||
% trajectory at the index closest to
|
||||
% `Y_curr`
|
||||
|
||||
% No equality constraints, so leave `ceq` empty
|
||||
ceq = [];
|
||||
|
||||
% Nonlinear inequality constraints from track boundary
|
||||
% and obstacles applied to states
|
||||
c = NaN(1, obj.npredstates);
|
||||
|
||||
% NOTE: Error = Actual - Reference
|
||||
% => Actual = Error + Reference
|
||||
Z_ref = obj.get_ref_decision_variable();
|
||||
Z = Z_err + Z_ref;
|
||||
|
||||
% Construct constraint for each state
|
||||
for i = 0:obj.npredstates-1
|
||||
% Get index of current state in decision variable
|
||||
idx = obj.get_state_start_idx(i);
|
||||
Y = Z(idx+1:idx+obj.nstates);
|
||||
|
||||
% Get xy position from state vector
|
||||
p = [Y(1); Y(3)];
|
||||
% Find closest boundary points to position
|
||||
[~,bl_idx] = min(vecnorm(obj.TestTrack.bl - p));
|
||||
[~,br_idx] = min(vecnorm(obj.TestTrack.br - p));
|
||||
|
||||
% Use closest boundary points +/- ntrack_boundary_pts
|
||||
% to construct track polygon
|
||||
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), ...
|
||||
obj.TestTrack.br(:,br_idx_end:-1:br_idx_start) ...
|
||||
];
|
||||
xv_track = boundary_pts(1,:);
|
||||
yv_track = boundary_pts(2,:);
|
||||
|
||||
% Check if p is within track polygon
|
||||
in_track = inpolygon(p(1), p(2), xv_track, yv_track);
|
||||
|
||||
if ~in_track
|
||||
% Position not inside track
|
||||
c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
|
||||
|
||||
% Skip to next constraint
|
||||
continue;
|
||||
end
|
||||
|
||||
for j = 1:size(obj.Xobs_seen,2)
|
||||
% Check if position is in or on each obstacle
|
||||
xv_obstacle = obj.Xobs_seen{j}(:,1);
|
||||
yv_obstacle = obj.Xobs_seen{j}(:,2);
|
||||
[in_obstacle, on_obstacle] = inpolygon(p(1), p(2), xv_obstacle, yv_obstacle);
|
||||
|
||||
if in_obstacle || on_obstacle
|
||||
% Point in or on obstacle
|
||||
c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
|
||||
|
||||
% Skip remaining obstacle checking
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
if isnan(c(i+1))
|
||||
% If value not set, no constraints violated
|
||||
c(i+1) = -1; % c(Z_err) <= 0, nonlinear inequality constraint satisfied
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
%% Private Kinematic Models
|
||||
methods (Access = private)
|
||||
function [A, B] = linearized_bike_model(obj)
|
||||
%linearized_bike_model Computes the discrete-time LTV
|
||||
% system matrices of the nonlinear bike model linearized
|
||||
% around the reference trajectory starting at the index
|
||||
% closest to `Y_curr`
|
||||
|
||||
% Continuous-time system
|
||||
A_c = @(i) obj.A_c_func(i);
|
||||
B_c = @(i) obj.B_c_func(i);
|
||||
|
||||
% Discrete-time LTV system
|
||||
A = @(i) eye(obj.nstates) + obj.T_s*A_c(i);
|
||||
B = @(i) obj.T_s * B_c(i);
|
||||
end
|
||||
|
||||
function [x,u,y,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(obj, Y, U)
|
||||
%bike_model_helper Computes the intermediate values
|
||||
% and applies limits used by the kinematic bike
|
||||
% model before the final derivative
|
||||
|
||||
% Get state & input variables
|
||||
x = Y(1);
|
||||
u = Y(2);
|
||||
y = Y(3);
|
||||
v = Y(4);
|
||||
psi = Y(5);
|
||||
r = Y(6);
|
||||
delta_f = U(1);
|
||||
F_x = U(2);
|
||||
|
||||
% Front and rear lateral slip angles in radians (Equations 8 & 9)
|
||||
alpha_f_rad = delta_f - atan2(v + obj.a*r, u);
|
||||
alpha_r_rad = -atan2(v - obj.b*r, u);
|
||||
|
||||
% Convert radians to degrees for other equations
|
||||
alpha_f = rad2deg(alpha_f_rad);
|
||||
alpha_r = rad2deg(alpha_r_rad);
|
||||
|
||||
% Nonlinear Tire Dynamics (Equations 6 & 7)
|
||||
phi_yf = (1-obj.Ey)*(alpha_f + obj.Shy) + (obj.Ey/obj.By)*atan(obj.By*(alpha_f + obj.Shy));
|
||||
phi_yr = (1-obj.Ey)*(alpha_r + obj.Shy) + (obj.Ey/obj.By)*atan(obj.By*(alpha_r + obj.Shy));
|
||||
|
||||
% Lateral forces using Pacejka "Magic Formula" (Equations 2 - 5)
|
||||
F_zf = (obj.b/(obj.a+obj.b))*(obj.m*obj.g);
|
||||
F_yf = F_zf*obj.Dy*sin(obj.Cy*atan(obj.By*phi_yf)) + obj.Svy;
|
||||
F_zr = (obj.a/(obj.a+obj.b))*(obj.m*obj.g);
|
||||
F_yr = F_zr*obj.Dy*sin(obj.Cy*atan(obj.By*phi_yr)) + obj.Svy;
|
||||
|
||||
% Limits on combined longitudinal and lateral loading of tires
|
||||
% (Equations 10 - 14)
|
||||
F_total = sqrt((obj.Nw*F_x)^2 + (F_yr^2));
|
||||
F_max = 0.7*(obj.m*obj.g);
|
||||
|
||||
if F_total > F_max
|
||||
F_x = (F_max/F_total)*F_x;
|
||||
F_yr = (F_max/F_total)*F_yr;
|
||||
end
|
||||
|
||||
% Apply input limits (Table 1)
|
||||
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
|
||||
|
||||
function A_c = A_c_func(obj, i)
|
||||
%A_c Computes the continuous time `A` matrix
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||
% 12-Dec-2021 15:05:59
|
||||
|
||||
[~,u_var,~,v_var,psi_var,r_var,~,~,~,~] ...
|
||||
= obj.bike_model_helper( ...
|
||||
obj.Y_ref(obj.ref_idx+i,:), ...
|
||||
obj.U_ref(obj.ref_idx+i,:) ...
|
||||
);
|
||||
|
||||
t2 = cos(psi_var);
|
||||
t3 = sin(psi_var);
|
||||
A_c = reshape([ ...
|
||||
0.0,0.0,0.0,0.0,0.0,0.0, ...
|
||||
t2,0.0,t3,-r_var,0.0,0.0, ...
|
||||
0.0,0.0,0.0,0.0,0.0,0.0, ...
|
||||
-t3,r_var,t2,0.0,0.0,0.0, ...
|
||||
-t3.*u_var-t2.*v_var,0.0,t2.*u_var-t3.*v_var,0.0,0.0,0.0, ...
|
||||
0.0,v_var,0.0,-u_var,1.0,0.0 ...
|
||||
], ...
|
||||
[6,6] ...
|
||||
);
|
||||
end
|
||||
|
||||
function B_c = B_c_func(obj, i)
|
||||
%B_c_func Computes the continuous time `B` matrix
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||
% 12-Dec-2021 15:05:59
|
||||
[~,~,~,~,~,~,delta_f_var,~,F_yf_var,~] ...
|
||||
= obj.bike_model_helper( ...
|
||||
obj.Y_ref(obj.ref_idx+i,:), ...
|
||||
obj.U_ref(obj.ref_idx+i,:) ...
|
||||
);
|
||||
|
||||
t2 = sin(delta_f_var);
|
||||
B_c = reshape([ ...
|
||||
0.0,F_yf_var.*cos(delta_f_var).*(-7.142857142857143e-4), ...
|
||||
0.0,F_yf_var.*t2.*(-7.142857142857143e-4), ...
|
||||
0.0,F_yf_var.*t2.*(-5.061867266591676e-4), ...
|
||||
0.0,1.0./7.0e+2, ...
|
||||
0.0,0.0, ...
|
||||
0.0,0.0 ...
|
||||
], ...
|
||||
[6,2] ...
|
||||
);
|
||||
end
|
||||
end
|
||||
|
||||
%% Private Helper Functions
|
||||
methods (Access = private)
|
||||
function idx = get_state_start_idx(obj, i)
|
||||
%get_state_start_idx Calculates starting index of state i in
|
||||
% the full decision variable. Assumes `i` starts
|
||||
% at 0.
|
||||
|
||||
idx = obj.nstates*i;
|
||||
end
|
||||
|
||||
function idx = get_input_start_idx(obj, i)
|
||||
%get_input_start_idx Calculates starting index of input i in
|
||||
% the full decision variable. Assumes `i` starts
|
||||
% at 0.
|
||||
|
||||
idx = (obj.npred+1)*obj.nstates + obj.ninputs*i;
|
||||
end
|
||||
|
||||
function idx = get_ref_index(obj)
|
||||
%get_ref_index Finds index of position in reference trajectory
|
||||
% that is closest (based on Euclidean distance) to position
|
||||
% in `Y_curr`
|
||||
|
||||
% Get position (x,y) from current state
|
||||
pos = [obj.Y_curr(1), obj.Y_curr(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
|
||||
|
||||
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 = obj.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 = obj.get_input_start_idx(i);
|
||||
Z_ref(start_idx+1:start_idx+obj.ninputs) ...
|
||||
= obj.U_ref(obj.ref_idx+i, :);
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user