mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-24 03:02:45 +00:00
3d3c53fcd1
- Adjust input error cost for steering angle - Increase `fmincon` maximum function evaluations - Add buffer factors for track boundary distance and obstacle radius - Add intermediate variables for calculating the number of constraints - Change discrete `inpolygon` constraints to continuous function based constraints where obstacles are modeled as circles and distance/direction from track boundary is computed - Add `point_to_line` function for computing track boundary quantities
611 lines
23 KiB
Matlab
611 lines
23 KiB
Matlab
% 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]
|
|
sim_stop_idx = 2.0e3; % Index in reference trajectory to stop sim
|
|
|
|
% 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
|
|
1e-2; ... % x_err [m]
|
|
1e-2; ... % u_err [m/s]
|
|
1e-2; ... % y_err [m]
|
|
1e-2; ... % v_err [m/s]
|
|
1e-2; ... % psi_err [rad]
|
|
1e-2; ... % r_err [rad/s]
|
|
];
|
|
R = [ ... % Input Error Costs
|
|
1e0; ... % delta_f_err [rad]
|
|
1e-2; ... % F_x_err [N]
|
|
];
|
|
NL = [ ... % Nonlinear Constraint Cost
|
|
1e6; ... % within track bounds
|
|
1e6; ... % outside obstacles
|
|
];
|
|
options = ...
|
|
optimoptions( ...
|
|
'fmincon', ...
|
|
'MaxFunctionEvaluations', 5000, ...
|
|
'SpecifyConstraintGradient', false, ...
|
|
'SpecifyObjectiveGradient', false ...
|
|
);
|
|
|
|
% Constraint Parameters
|
|
track_dist_fact = 1.00; % Buffer factor on distance from track boundary
|
|
obs_rad_fact = 0.90; % Buffer factor on obstacle radius
|
|
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;
|
|
|
|
% Stop simulation when `sim_stop_idx` is reached
|
|
obj.FLAG_terminate = 0;
|
|
if obj.ref_idx > obj.sim_stop_idx
|
|
disp('Reached simulation stop index')
|
|
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();
|
|
|
|
[ ... % `fmincon` outputs
|
|
Z_err, ... % x: Solution
|
|
~, ... % fval: Objective function value at solution
|
|
exitflag, ... % exitflag: Reason `fmincon` stopped
|
|
~, ... % output: Information about the optimization process
|
|
] = ...
|
|
fmincon( ... % `fmincon` inputs
|
|
@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
|
|
);
|
|
|
|
% Stop sim if `fmincon` failed to find feasible point
|
|
if exitflag == -2
|
|
disp('No feasible point was found')
|
|
obj.FLAG_terminate = 1;
|
|
end
|
|
|
|
% Stop sim if nonlinear constraint was violated
|
|
[c, ~] = obj.nonlcon(Z_err);
|
|
if any(c > 0)
|
|
disp('Found point violates nonlinear inequality constraint')
|
|
obj.FLAG_terminate = 1;
|
|
end
|
|
|
|
% 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 = [];
|
|
|
|
% Calculate number of constraints
|
|
ntrack_cons = 2; % left and right track boundary
|
|
nobstacle_cons = length(obj.Xobs_seen); % outside of each obstacle
|
|
ncons_per_state = ntrack_cons + nobstacle_cons;
|
|
ntotal_cons = obj.npredstates * ncons_per_state;
|
|
|
|
% Nonlinear inequality constraints from track boundary
|
|
% and obstacles applied to states
|
|
c = NaN(1, ntotal_cons);
|
|
cons_idx = 1;
|
|
|
|
% 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)];
|
|
|
|
% Get indexes of closest two track border points
|
|
[~,left_idxs] = mink(vecnorm(p - obj.TestTrack.bl), 2);
|
|
[~,right_idxs] = mink(vecnorm(p - obj.TestTrack.br), 2);
|
|
|
|
% Compute distance and direction from left and right boundaries
|
|
[left_dist, left_direction] = ...
|
|
obj.point_to_line( ...
|
|
p, ...
|
|
obj.TestTrack.bl(:,min(left_idxs)), ...
|
|
obj.TestTrack.bl(:,max(left_idxs)) ...
|
|
);
|
|
[right_dist, right_direction] = ...
|
|
obj.point_to_line( ...
|
|
p, ...
|
|
obj.TestTrack.br(:,min(right_idxs)), ...
|
|
obj.TestTrack.br(:,max(right_idxs)) ...
|
|
);
|
|
|
|
% Construct Track Boundary Constraints
|
|
|
|
% Direction from left boundary should be negative,
|
|
% so <= 0 constraint is satisfied when left_direction
|
|
% is negative
|
|
c(cons_idx) = obj.track_dist_fact * left_dist * left_direction;
|
|
cons_idx = cons_idx + 1;
|
|
|
|
% Direction from right boundary should be positive,
|
|
% so <= 0 constraint is satisfied when right_direction
|
|
% is positive
|
|
c(cons_idx) = obj.track_dist_fact * right_dist * -right_direction;
|
|
cons_idx = cons_idx + 1;
|
|
|
|
% Construct Obstacle Constraints
|
|
for j = 1:length(obj.Xobs_seen)
|
|
cen_obstacle = [ ... % Obstacle centroid
|
|
mean(obj.Xobs_seen{j}(:,1)), ...
|
|
mean(obj.Xobs_seen{j}(:,2)) ...
|
|
];
|
|
rad_obstacle = ... % Obstacle radius
|
|
max(vecnorm(cen_obstacle - obj.Xobs_seen{j})) ...
|
|
* obj.obs_rad_fact;
|
|
|
|
% Model obstacle as a circle
|
|
c(cons_idx) = ...
|
|
(rad_obstacle)^2 ...
|
|
- (p(1) - cen_obstacle(1))^2 ...
|
|
- (p(2) - cen_obstacle(2))^2;
|
|
cons_idx = cons_idx + 1;
|
|
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
|
|
|
|
function [dist, direction] = point_to_line(pt, v1, v2)
|
|
%point_to_line Computes shortest distance from
|
|
% a point pt to a line defined by v1 & v2
|
|
% and direction (+/- 1) based on axis orthogonal to xy
|
|
% plane being pointed upwards such that CCW rotation
|
|
% is positive
|
|
|
|
% Convert 2D column vectors to 3D column vectors
|
|
pt = [pt; 0];
|
|
v1 = [v1; 0];
|
|
v2 = [v2; 0];
|
|
|
|
% Compute distance
|
|
a = v2 - v1; % vector from v1 to v2
|
|
b = pt - v1; % vector from v1 to pt
|
|
dist = norm(cross(a,b)) / norm(a);
|
|
|
|
% Compute direction as sign of cross product
|
|
cross_p = cross(a,b);
|
|
direction = sign(cross_p(3));
|
|
end
|
|
end
|
|
end
|
|
|