mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-23 19:02:44 +00:00
d591bb68c9
- Use all track boundary points when checking if position is inside - Still fails to stop simulation when car goes off track
568 lines
21 KiB
Matlab
568 lines
21 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
|
|
1e-2; ... % 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', 4000, ...
|
|
'SpecifyConstraintGradient', false, ...
|
|
'SpecifyObjectiveGradient', false ...
|
|
);
|
|
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 = [];
|
|
|
|
% Nonlinear inequality constraints from track boundary
|
|
% and obstacles applied to states
|
|
c = NaN(1, obj.npredstates*(1+length(obj.Xobs_seen)));
|
|
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)];
|
|
|
|
% Construct track polygon from all boundary points
|
|
bl_idx_start = 1;
|
|
bl_idx_end = size(obj.TestTrack.bl,2);
|
|
br_idx_start = 1;
|
|
br_idx_end = 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(cons_idx) = obj.NL(1); % c(Z_err) > 0, nonlinear inequality constraint violated
|
|
else
|
|
c(cons_idx) = -obj.NL(1); % c(Z_err) <= 0, nonlinear inequality constraint satisfied
|
|
end
|
|
cons_idx = cons_idx + 1;
|
|
|
|
for j = 1:length(obj.Xobs_seen)
|
|
% 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(cons_idx) = obj.NL(2); % c(Z_err) > 0, nonlinear inequality constraint violated
|
|
else
|
|
c(cons_idx) = -obj.NL(2); % c(Z_err) <= 0, nonlinear inequality constraint satisfied
|
|
end
|
|
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
|
|
end
|
|
end
|
|
|