Hello, I was trying to run this code script in Matlab Function in Simulink, however I encountered the Model error and it showed that error in the figure and didn't tell me where it caused. After I preliminary debugging , I think the problems will be in the parts of "path", but the code script runs well in the Matlab Workspace, so I don't know where caused the error. May you help me to check please. Thank you a lot!
Below is my code script in Matlab Function1
function path_adjusted = fcn(waypoints, velocity, window_length, P_N, P_E)
persistent current_index last_index path;
if isempty(current_index)
current_index = 1;
end
if isempty(last_index)
last_index = 0;
end
if isempty(path)
path = zeros(2*window_length, 2);
end
distance_threshold = 30;
poly_degree = 3;
regulator_weight = 0.5;
path_raw = zeros(length(waypoints), 2);
path_adjusted = zeros(2*window_length, 2);
if last_index > 0
last_window_point = waypoints(last_index, 1:2);
distance_to_last_point = norm([P_N, P_E] - last_window_point);
if distance_to_last_point >= distance_threshold
path_adjusted = path;
return;
end
end
start_index = max(current_index, last_index + 1);
end_index = min(last_index + window_length, length(waypoints));
% Check if all waypoints are already processed
if start_index > length(waypoints)
path_adjusted = path;
return;
end
waypoints_window = waypoints(start_index:end_index, :);
velocity_window = velocity(start_index:end_index, :);
time_window = linspace(0, 1, size(waypoints_window, 1));
P_ = zeros((poly_degree+1), (poly_degree+1));
qx = zeros((poly_degree+1), 1);
qy = zeros((poly_degree+1), 1);
for j = 1:size(waypoints_window, 1)
t = time_window(j);
Ti0 = [1; t; t^2; t^3];
Tie0 = Ti0 * Ti0';
Ti1 = [0; 1; 2*t; 3*t^2];
Tie1 = Ti1 * Ti1';
P_ = P_ + Tie0 + Tie1;
qx = qx + -2*waypoints_window(j, 1)*Ti0 + -2*velocity_window(j, 1)*Ti1;
qy = qy + -2*waypoints_window(j, 2)*Ti0 + -2*velocity_window(j, 2)*Ti1;
end
t_last = time_window(end);
t_init = time_window(1);
% Combining the Regular Term
Tir_last = [0 0 0 0; 0 0 0 0; 0 0 4*t_last 6*t_last^2; 0 0 6*t_last^2 12*t_last^3];
Tir_init = [0 0 0 0; 0 0 0 0; 0 0 4*t_init 6*t_init^2; 0 0 6*t_init^2 12*t_init^3];
Tir = Tir_last - Tir_init;
P_ = P_ + window_length*regulator_weight*Tir;
P = 2*P_;
P = 0.5*(P+P');
initial_guess_x = [waypoints(1, 1); 0; 0; 0];
initial_guess_y = [waypoints(1, 2); 0; 0; 0];
% Solve QP
options = optimoptions('quadprog', ...
'Algorithm', 'active-set', ... % Use 'active-set' explicitly
'Display', 'off'); % Suppress output for real-time compatibility
coeff_x = quadprog(P, qx, [], [], [], [], [], [], initial_guess_x, options);
coeff_y = quadprog(P, qy, [], [], [], [], [], [], initial_guess_y, options);
target_pos_poly_x = coeff_x(1) + coeff_x(2)*time_window + coeff_x(3)*time_window.^2 + coeff_x(4)*time_window.^3;
target_pos_poly_y = coeff_y(1) + coeff_y(2)*time_window + coeff_y(3)*time_window.^2 + coeff_y(4)*time_window.^3;
path_raw = [target_pos_poly_x(:), target_pos_poly_y(:)];
if last_index > 0
path = circshift(path,-window_length);
path(window_length+1:end, :) = path_raw;
else
path(window_length+1:end, :) = path_raw;
end
path_adjusted = path;
last_index = end_index;
if end_index < length(waypoints)
current_index = current_index + 1;
end
end