Interface: Matlab
Goal: Generate S-function for 4-wheel vehicle MPC that works in Simulink for real-time control
Problem: M-code solver works perfectly (status=0), but S-function returns status=4 (infeasible) in Simulink with identical setup and parameters
What I tried: Used identical model, constraints, and solver settings for both M-code and S-function generation. The M-code version solves successfully, but when I use make_sfun to generate S-function for Simulink, it immediately returns status=4.
Vehicle Model
function [ocp_solver, sim_solver] = setup_fourwheel_ocp_solver(model, T_horizon, N, dt_sim)
% SETUP_FOURWHEEL_OCP_SOLVER Creates and configures OCP solver and plant simulator
% for 4-wheel drive vehicle model with individual wheel torques and front/rear steering
% Inputs:
% model: AcadosModel for 4-wheel vehicle
% T_horizon: prediction horizon time [s]
% N: prediction horizon steps
% dt_sim: simulation timestep [s]
% Outputs:
% ocp_solver: AcadosOcpSolver object
% sim_solver: AcadosSimSolver object
%% OCP SETUP - Optimized for speed
ocp = AcadosOcp();
ocp.solver_options.tf = T_horizon;
ocp.solver_options.N_horizon = N;
ocp.solver_options.nlp_solver_type = 'SQP_RTI';
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM';
% RTI-specific settings for stability
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON';
ocp.solver_options.print_level = 0;
% OCP model integrator
ocp.solver_options.sim_method_num_stages = 4;
ocp.solver_options.sim_method_num_steps = 1;
ocp.solver_options.integrator_type = 'ERK';
%% PLANT SIMULATOR SETUP
sim = AcadosSim();
% Plant integrator
sim.solver_options.num_stages = 4;
sim.solver_options.num_steps = 1;
sim.solver_options.Tsim = dt_sim;
sim.solver_options.integrator_type = 'ERK';
%% ASSIGN MODEL
ocp.model = model;
sim.model = model;
%% COST FUNCTION - External cost (defined in model)
ocp.cost.cost_type = 'EXTERNAL';
ocp.cost.cost_type_0 = 'EXTERNAL';
ocp.cost.cost_type_e = 'EXTERNAL';
%% CONSTRAINTS
% Wheel torque limits
T_max = 3000; % max wheel torque [Nm]
T_min = -3000; % min wheel torque [Nm] (negative for braking/regen)
% Steering angle limits
delta_f_max = deg2rad(30); % front steering limit [rad]
delta_r_max = deg2rad(5); % rear steering limit [rad] (smaller than front)
% Control input constraints
ocp.constraints.lbu = [T_min; T_min; T_min; T_min; -delta_f_max; -delta_r_max];
ocp.constraints.ubu = [T_max; T_max; T_max; T_max; delta_f_max; delta_r_max];
ocp.constraints.idxbu = [0; 1; 2; 3; 4; 5];
% State constraints
vx_max = 50.0; vx_min = 0.1; % longitudinal velocity limits [m/s]
vy_max = 20.0; % lateral velocity limits [m/s]
psi_dot_max = deg2rad(180); % yaw rate limits [rad/s]
% State bounds [vx, vy, psi_dot]
ocp.constraints.lbx = [vx_min; -vy_max; -psi_dot_max];
ocp.constraints.ubx = [vx_max; vy_max; psi_dot_max];
ocp.constraints.idxbx = [3; 4; 5]; % indices for vx, vy, psi_dot
% Initial state constraint setup (will be set dynamically)
ocp.constraints.x0 = zeros(6, 1); % placeholder for 6-dimensional state
ocp.simulink_opts = get_acados_simulink_opts;
%% CREATE SOLVERS
ocp_solver = AcadosOcpSolver(ocp);
sim_solver = AcadosSimSolver(sim);
end
function model = get_fourwheel_vehicle_model(varargin)
% GET_FOURWHEEL_VEHICLE_MODEL returns AcadosModel for 4-wheel drive vehicle
% States: [x, y, psi, vx, vy, psi_dot]
% - position, heading, velocities, yaw rate
% Controls: [T_fl, T_fr, T_rl, T_rr, delta_f, delta_r]
% - 4 wheel torques, front and rear steering angles
import casadi.*
%% System dimensions
nx = 6; % states: [x, y, psi, vx, vy, psi_dot]
nu = 6; % controls: [T_fl, T_fr, T_rl, T_rr, delta_f, delta_r]
%% Vehicle parameters
m = 1500; % vehicle mass [kg]
I_z = 2500; % moment of inertia around z-axis [kg⋅m²]
l_f = 1.3; % distance from CG to front axle [m]
l_r = 1.2; % distance from CG to rear axle [m]
t_f = 1.6; % front track width [m]
t_r = 1.6; % rear track width [m]
% Wheel parameters
R_w = 0.32; % wheel radius [m]
% Tire parameters
C_f = 80000; % front tire cornering stiffness [N/rad]
C_r = 80000; % rear tire cornering stiffness [N/rad]
%% Named symbolic variables
% States
x_pos = SX.sym('x'); % global x position [m]
y_pos = SX.sym('y'); % global y position [m]
psi = SX.sym('psi'); % heading angle [rad]
vx = SX.sym('vx'); % velocity in x direction [m/s]
vy = SX.sym('vy'); % velocity in y direction [m/s]
psi_dot = SX.sym('psi_dot'); % yaw rate [rad/s]
% Controls
T_fl = SX.sym('T_fl'); % front left wheel torque [Nm]
T_fr = SX.sym('T_fr'); % front right wheel torque [Nm]
T_rl = SX.sym('T_rl'); % rear left wheel torque [Nm]
T_rr = SX.sym('T_rr'); % rear right wheel torque [Nm]
delta_f = SX.sym('delta_f'); % front steering angle [rad]
delta_r = SX.sym('delta_r'); % rear steering angle [rad]
%% State and control vectors
x = vertcat(x_pos, y_pos, psi, vx, vy, psi_dot);
u = vertcat(T_fl, T_fr, T_rl, T_rr, delta_f, delta_r);
xdot = SX.sym('xdot', nx, 1);
%% Wheel velocities at contact points
% Front left wheel
vx_fl = vx - t_f/2 * psi_dot;
vy_fl = vy + l_f * psi_dot;
% Front right wheel
vx_fr = vx + t_f/2 * psi_dot;
vy_fr = vy + l_f * psi_dot;
% Rear left wheel
vx_rl = vx - t_r/2 * psi_dot;
vy_rl = vy - l_r * psi_dot;
% Rear right wheel
vx_rr = vx + t_r/2 * psi_dot;
vy_rr = vy - l_r * psi_dot;
%% Tire slip calculations
% Slip angles (considering steering)
alpha_fl = delta_f - atan2(vy_fl, abs(vx_fl));
alpha_fr = delta_f - atan2(vy_fr, abs(vx_fr));
alpha_rl = delta_r - atan2(vy_rl, abs(vx_rl));
alpha_rr = delta_r - atan2(vy_rr, abs(vx_rr));
%% Tire forces (simplified linear model)
% Longitudinal forces: simply torque divided by wheel radius
F_xfl = T_fl / R_w;
F_xfr = T_fr / R_w;
F_xrl = T_rl / R_w;
F_xrr = T_rr / R_w;
% Lateral forces
F_yfl = C_f * alpha_fl;
F_yfr = C_f * alpha_fr;
F_yrl = C_r * alpha_rl;
F_yrr = C_r * alpha_rr;
% Transform forces to vehicle coordinate system
% Front wheels (with steering)
F_xfl_body = F_xfl * cos(delta_f) - F_yfl * sin(delta_f);
F_yfl_body = F_xfl * sin(delta_f) + F_yfl * cos(delta_f);
F_xfr_body = F_xfr * cos(delta_f) - F_yfr * sin(delta_f);
F_yfr_body = F_xfr * sin(delta_f) + F_yfr * cos(delta_f);
% Rear wheels (with steering)
F_xrl_body = F_xrl * cos(delta_r) - F_yrl * sin(delta_r);
F_yrl_body = F_xrl * sin(delta_r) + F_yrl * cos(delta_r);
F_xrr_body = F_xrr * cos(delta_r) - F_yrr * sin(delta_r);
F_yrr_body = F_xrr * sin(delta_r) + F_yrr * cos(delta_r);
% Total forces
F_x_total = F_xfl_body + F_xfr_body + F_xrl_body + F_xrr_body;
F_y_total = F_yfl_body + F_yfr_body + F_yrl_body + F_yrr_body;
%% Vehicle dynamics equations
% Position dynamics (global frame)
x_dot = vx * cos(psi) - vy * sin(psi);
y_dot = vx * sin(psi) + vy * cos(psi);
psi_dot_eq = psi_dot;
% Body frame accelerations
vx_dot = F_x_total / m + vy * psi_dot;
vy_dot = F_y_total / m - vx * psi_dot;
% Yaw dynamics (moments about CG)
M_z = (F_yfl_body + F_yfr_body) * l_f - (F_yrl_body + F_yrr_body) * l_r + ...
(F_xfr_body - F_xfl_body) * t_f/2 + (F_xrr_body - F_xrl_body) * t_r/2;
psi_dot_dot = M_z / I_z;
%% System dynamics
f_expl_expr = vertcat(x_dot, ...
y_dot, ...
psi_dot_eq, ...
vx_dot, ...
vy_dot, ...
psi_dot_dot);
f_impl_expr = f_expl_expr - xdot;
% Discrete dynamics
if nargin > 0
delta_t = varargin{1};
disc_dyn_expr = x + delta_t * f_expl_expr; % explicit Euler
else
disc_dyn_expr = [];
end
%% Cost function setup (for S-function compatibility)
% Reference parameters
x_ref = SX.sym('x_ref');
y_ref = SX.sym('y_ref');
% Cost weights
Q_pos = 100; % position tracking weight
Q_vy = 2; % lateral velocity penalty (stability)
Q_psi_dot = 2; % yaw rate penalty (stability)
% Control effort weights
R_torque = 1e-6; % wheel torque penalty
R_steer_f = 10; % front steering penalty
R_steer_r = 20; % rear steering penalty (higher to prefer front steering)
% Stage cost (tracking + stability + control effort)
stage_cost = Q_pos * ((x_pos - x_ref)^2 + (y_pos - y_ref)^2) + ...
Q_vy * vy^2 + ...
Q_psi_dot * psi_dot^2 + ...
R_torque * (T_fl^2 + T_fr^2 + T_rl^2 + T_rr^2) + ...
R_steer_f * delta_f^2 + ...
R_steer_r * delta_r^2;
% Initial stage cost (stability + control effort)
initial_cost = Q_vy * vy^2 + ...
Q_psi_dot * psi_dot^2 + ...
R_torque * (T_fl^2 + T_fr^2 + T_rl^2 + T_rr^2) + ...
R_steer_f * delta_f^2 + ...
R_steer_r * delta_r^2;
% Terminal cost (tracking + stability only)
terminal_cost = Q_pos * ((x_pos - x_ref)^2 + (y_pos - y_ref)^2) + ...
Q_vy * vy^2 + ...
Q_psi_dot * psi_dot^2;
%% Populate AcadosModel
model = AcadosModel();
model.x = x;
model.xdot = xdot;
model.u = u;
model.p = vertcat(x_ref, y_ref); % parameters for reference
model.f_expl_expr = f_expl_expr;
model.f_impl_expr = f_impl_expr;
model.disc_dyn_expr = disc_dyn_expr;
model.cost_expr_ext_cost = stage_cost;
model.cost_expr_ext_cost_0 = initial_cost;
model.cost_expr_ext_cost_e = terminal_cost;
model.name = 'fourwheel_vehicle';
end
clear all; close all; clc;
% Add parent directory to path to access acados setup
addpath('..');
acados_env_variables_windows();
%% SIMULATION PARAMETERS
T_sim = 40.0; % full simulation time [s] (for reference generation)
dt_sim = 0.05; % simulation timestep [s]
N_sim = round(T_sim/dt_sim);
%% OCP SETUP - 3 second horizon optimization
dt_ocp = 0.05; % match simulation timestep
T_horizon = 3.0; % 3 second prediction horizon
N = round(T_horizon/dt_ocp); % prediction horizon steps = 60
%% GIF Recording Parameters
record_gif = false; % Disabled for single optimization
gif_filename = '4wheel_lemniscate_3sec_horizon.gif';
%% 4-WHEEL VEHICLE MODEL
model = get_fourwheel_vehicle_model();
nx = 6; % [x, y, psi, vx, vy, psi_dot]
nu = 6; % [T_fl, T_fr, T_rl, T_rr, delta_f, delta_r]
%% LEMNISCATE REFERENCE PATH - Figure-8 trajectory
t_sim = (0:N_sim) * dt_sim;
% Lemniscate parameters
scale = 200; % size of the figure-8
omega = 0.2; % frequency (slower for better tracking)
% Parametric lemniscate equations
t_param = omega * t_sim;
% Lemniscate position
denominator = 1 + sin(t_param).^2;
x_ref_sim = scale * cos(t_param) ./ denominator;
y_ref_sim = scale * sin(t_param) .* cos(t_param) ./ denominator;
% Calculate heading angle from velocity direction
dx_dt = gradient(x_ref_sim, dt_sim);
dy_dt = gradient(y_ref_sim, dt_sim);
psi_ref_sim = atan2(dy_dt, dx_dt);
% Calculate reference velocity (speed along path)
v_ref_sim = sqrt(dx_dt.^2 + dy_dt.^2);
v_ref_sim = max(v_ref_sim, 5); % minimum speed of 5 m/s
%% EXTRACT 3-SECOND REFERENCE TRAJECTORY
N_3sec = N + 1; % Number of points for 3 seconds (including t=0)
t_3sec = (0:N) * dt_sim; % Time vector for 3 seconds
% Extract first 3 seconds of reference
x_ref_3sec = x_ref_sim(1:N_3sec);
y_ref_3sec = y_ref_sim(1:N_3sec);
psi_ref_3sec = psi_ref_sim(1:N_3sec);
v_ref_3sec = v_ref_sim(1:N_3sec);
%% CREATE SOLVERS
fprintf('Creating 4-wheel vehicle solvers with 3-second horizon (N=%d)...\n', N);
tic;
[ocp_solver, sim_solver] = setup_fourwheel_ocp_solver(model, T_horizon, N, dt_sim);
solver_creation_time = toc;
fprintf('Solvers created in %.3f seconds\n', solver_creation_time);
%% Initial state
% [x, y, psi, vx, vy, psi_dot]
R_w = 0.32; % wheel radius [m]
vx0 = v_ref_3sec(1);
vy0 = 0;
x0 = [x_ref_3sec(1); y_ref_3sec(1); psi_ref_3sec(1); vx0; vy0; 0];
%% SETUP VISUALIZATION
% Get screen size and center figure
screenSize = get(0, 'ScreenSize');
screenWidth = screenSize(3);
screenHeight = screenSize(4);
figWidth = 1800;
figHeight = 1000;
xPos = (screenWidth - figWidth) / 2;
yPos = (screenHeight - figHeight) / 2;
fig = figure('Position', [xPos, yPos, figWidth, figHeight]);
% Main trajectory plot
subplot(2,3,[1,4]);
h_ref_full = plot(x_ref_sim, y_ref_sim, 'k:', 'LineWidth', 1); hold on; % Full reference (light)
h_ref_3sec = plot(x_ref_3sec, y_ref_3sec, 'k--', 'LineWidth', 2); % 3-second reference
h_optimal = plot(NaN, NaN, 'b-', 'LineWidth', 3);
h_start = plot(x0(1), x0(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
h_end_3sec = plot(x_ref_3sec(end), y_ref_3sec(end), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
xlabel('X [m]'); ylabel('Y [m]');
title('3-Second Horizon Lemniscate Trajectory Optimization');
legend('Full Reference', '3-sec Reference', 'Optimal Path', 'Start', '3-sec End', 'Location', 'best');
grid on; axis equal;
% Error plot placeholder
subplot(2,3,2);
title('Position Error (will be computed after optimization)');
xlabel('Time [s]'); ylabel('Error [m]'); grid on;
% Velocity plot placeholder
subplot(2,3,3);
title('Velocities (will be computed after optimization)');
xlabel('Time [s]'); ylabel('Velocity [m/s]'); grid on;
% Control plots placeholders
subplot(2,3,5);
title('Wheel Torques (will be computed after optimization)');
xlabel('Time [s]'); ylabel('Torque [Nm]'); grid on;
subplot(2,3,6);
title('Steering Angles (will be computed after optimization)');
xlabel('Time [s]'); ylabel('Steering Angle [deg]'); grid on;
drawnow;
%% SINGLE 3-SECOND HORIZON OPTIMIZATION
fprintf('Setting up 3-second horizon optimization...\n');
% Set initial state
ocp_solver.set('constr_x0', x0);
% Set reference parameters for 3-second trajectory
fprintf('Setting reference trajectory for %d stages (3 seconds)...\n', N+1);
for j = 0:N
ref_params = [x_ref_3sec(j+1); y_ref_3sec(j+1)];
ocp_solver.set('p', ref_params, j);
% Initialize with reference trajectory as initial guess
x_init = [x_ref_3sec(j+1); y_ref_3sec(j+1); psi_ref_3sec(j+1);
v_ref_3sec(j+1); 0; 0];
ocp_solver.set('x', x_init, j);
if j < N
u_init = [50; 50; 50; 50; 0; 0]; % small torques, zero steering
ocp_solver.set('u', u_init, j);
end
end
fprintf('Starting 3-second horizon optimization...\n');
optimization_tic = tic;
% Solve the 3-second trajectory at once
ocp_solver.solve();
total_optimization_time = toc(optimization_tic);
status = ocp_solver.get('status');
fprintf('Optimization completed!\n');
fprintf('Status: %d\n', status);
fprintf('Total optimization time: %.3f seconds\n', total_optimization_time);
if status ~= 0
fprintf('Warning: Optimization did not converge to optimal solution\n');
else
fprintf('Optimization successful!\n');
end
%% EXTRACT OPTIMAL SOLUTION
fprintf('Extracting optimal solution...\n');
% Extract optimal states and controls for 3-second horizon
x_opt = zeros(nx, N+1);
u_opt = zeros(nu, N);
for j = 0:N
x_opt(:, j+1) = ocp_solver.get('x', j);
if j < N
u_opt(:, j+1) = ocp_solver.get('u', j);
end
end
%% COMPUTE PERFORMANCE METRICS (for 3-second trajectory)
pos_error = sqrt((x_opt(1,:) - x_ref_3sec).^2 + (x_opt(2,:) - y_ref_3sec).^2);
total_force = (u_opt(1,:) + u_opt(2,:) + u_opt(3,:) + u_opt(4,:)) / R_w;
total_torque = u_opt(1,:) + u_opt(2,:) + u_opt(3,:) + u_opt(4,:);
%% UPDATE PLOTS WITH OPTIMAL SOLUTION
% Update main trajectory plot
subplot(2,3,[1,4]);
set(h_optimal, 'XData', x_opt(1,:), 'YData', x_opt(2,:));
% Error plot
subplot(2,3,2);
plot(t_3sec, pos_error, 'r-', 'LineWidth', 2);
xlabel('Time [s]'); ylabel('Position Error [m]');
title('Tracking Error (3 seconds)'); grid on;
xlim([0, 3]);
% Velocity plots
subplot(2,3,3);
plot(t_3sec, x_opt(4,:), 'b-', 'LineWidth', 2); hold on;
plot(t_3sec, x_opt(5,:), 'r-', 'LineWidth', 2);
plot(t_3sec, v_ref_3sec, 'k--', 'LineWidth', 2);
xlabel('Time [s]'); ylabel('Velocity [m/s]');
title('Velocities (3 seconds)'); legend('vx', 'vy', 'v_{ref}'); grid on;
xlim([0, 3]);
% Wheel torques
subplot(2,3,5);
plot(t_3sec(1:end-1), u_opt(1,:), 'r-', 'LineWidth', 1.5); hold on;
plot(t_3sec(1:end-1), u_opt(2,:), 'g-', 'LineWidth', 1.5);
plot(t_3sec(1:end-1), u_opt(3,:), 'b-', 'LineWidth', 1.5);
plot(t_3sec(1:end-1), u_opt(4,:), 'm-', 'LineWidth', 1.5);
xlabel('Time [s]'); ylabel('Wheel Torque [Nm]');
title('Wheel Torques (3 seconds)');
legend('FL', 'FR', 'RL', 'RR', 'Location', 'best'); grid on;
xlim([0, 3]);
% Steering angles
subplot(2,3,6);
plot(t_3sec(1:end-1), rad2deg(u_opt(5,:)), 'b-', 'LineWidth', 2); hold on;
plot(t_3sec(1:end-1), rad2deg(u_opt(6,:)), 'r-', 'LineWidth', 2);
xlabel('Time [s]'); ylabel('Steering Angle [deg]');
title('Steering Angles (3 seconds)');
legend('Front', 'Rear', 'Location', 'best'); grid on;
xlim([0, 3]);
drawnow;
%% PERFORMANCE METRICS (for 3-second trajectory)
rmse_pos = sqrt(mean(pos_error.^2));
max_pos_error = max(pos_error);
max_vy = max(abs(x_opt(5,:)));
max_yaw_rate = max(abs(rad2deg(x_opt(6,:))));
avg_speed = mean(sqrt(x_opt(4,:).^2 + x_opt(5,:).^2));
max_torque = max(max(abs(u_opt(1:4,:))));
avg_total_torque = mean(abs(total_torque));
fprintf('\n=== 3-SECOND HORIZON OPTIMIZATION RESULTS ===\n');
fprintf('Optimization time: %.3f seconds\n', total_optimization_time);
fprintf('Solver status: %d\n', status);
fprintf('Horizon length: %.1f seconds (%d steps)\n', T_horizon, N);
fprintf('\nPerformance Metrics (first 3 seconds):\n');
fprintf('RMSE Position Error: %.3f m\n', rmse_pos);
fprintf('Max Position Error: %.3f m\n', max_pos_error);
fprintf('Max Lateral Velocity: %.2f m/s\n', max_vy);
fprintf('Max Yaw Rate: %.2f deg/s\n', max_yaw_rate);
fprintf('Average Speed: %.2f m/s\n', avg_speed);
fprintf('Max Wheel Torque: %.0f Nm\n', max_torque);
fprintf('Average Total Torque: %.0f Nm\n', avg_total_torque);
Question: Why does the same NMPC problem become infeasible when compiled to S-function? Is there a difference in constraint handling, memory initialization, or parameter parsing between MATLAB interface and S-function compilation process?
Environment: Windows 11, MATLAB R2022b,2024b, MinGW-w64, ACADOS latest
