I am developing an MPC controller for vehicle trajectory tracking. To achieve this, I use the bicycle model of the vehicle. When I include the longitudinal velocity Vx as a state variable (along with its time derivative), the optimization does not proceed, even if it is constant and properly initialized. On the other hand, if I remove Vx from the state vector and declare it separately as a variable, the optimization runs successfully. Additionally, I define the buses in Simulink and assign an initial value to enable the simulation. I am attaching the implementation code.
clc;
clear all;
close all;
clear mex;
% Controller settings
Ts = 0.01; % Sampling time
% Vehicle parameters of bicycle model (dynamic)
Vehicle_parameter;
%% ACADO set up
% Controller states
DifferentialState Vx doty dotpsi x y psi;
Control delta; % Definition of control variable
%OnlineData Vx; % Vx is now a parameter instead of a constant
%% Calculation of forces from the nonlinear tire model
s_f = 0.1;
s_r = 0.1;
Cf_non = par.Ccf;
Cr_non = par.Ccr;
Fxf = s_f * par.Csf;
Fxr = s_r * par.Csr;
% Now Vx will be passed from Simulink, we no longer define it here
%Vx = 10; % Constant velocity
f_ctrl = [
dot(Vx) == doty*dotpsi;
dot(doty) == (Fxf*sin(delta) + Cf_non*(delta-(doty + par.lf*dotpsi)/(Vx))*cos(delta))/par.m - (Cr_non*(doty - par.lf*dotpsi)/(Vx))/par.m - Vx*dotpsi;...
dot(dotpsi) == (Fxf*sin(delta) + Cf_non*(delta-(doty + par.lf*dotpsi)/(Vx))*cos(delta))*par.lf/par.Iz + (Cr_non*(doty - par.lf*dotpsi)/(Vx))*par.lr/par.Iz;...
dot(x) == Vx*cos(psi) - doty*sin(psi);...
dot(y) == Vx*sin(psi) + doty*cos(psi);...
dot(psi) == dotpsi];
%% ACADO: Controller formulation
acadoSet('problemname', 'PF_problem');
Np = 20;
ocp = acado.OCP(0.0, Np*Ts, Np);
% Residual function definition based on ACADO
h = [diffStates ; controls];
hN = [diffStates]; % terminal
% Initialization weights
W = acado.BMatrix(eye(length(h)));
WN = acado.BMatrix(eye(length(hN)));
% Cost definition
ocp.minimizeLSQ(W, h);
ocp.minimizeLSQEndTerm(WN, hN); % terminal
% Constraints definition
beta_thd = 100 / 180 * pi; % absolute sideslip
delta_thd = 360 / 180 * pi / par.i_steer;
y_thd = 100;
psi_thd = 2*pi;
% Constraints in ACADO
ocp.subjectTo(-y_thd <= y <= y_thd);
ocp.subjectTo(-psi_thd <= psi <= psi_thd);
%ocp.subjectTo(-delta_thd <= delta <= delta_thd);
% Definition of ACADO prediction model
ocp.setModel(f_ctrl); % Set the model dynamics
% ACADO settings
mpc = acado.OCPexport(ocp);
mpc.set('HESSIAN_APPROXIMATION', 'GAUSS_NEWTON');
mpc.set('DISCRETIZATION_TYPE', 'MULTIPLE_SHOOTING');
mpc.set('INTEGRATOR_TYPE', 'INT_IRK_GL2');
mpc.set('NUM_INTEGRATOR_STEPS', 3 * Np);
mpc.set('LEVENBERG_MARQUARDT', 1e-4);
mpc.set('SPARSE_QP_SOLUTION', 'FULL_CONDENSING_N2');
mpc.set('QP_SOLVER', 'QP_QPOASES3');
mpc.set('MAX_NUM_QP_ITERATIONS', 200);
mpc.set('HOTSTART_QP', 'YES');
mpc.set('GENERATE_SIMULINK_INTERFACE', 'YES'); % Interface with Simulink
%% Export and Compile flags
EXPORT = 1;
COMPILE = 1;
% Export code to the defined folder
if EXPORT
mpc.exportCode('export_MPC');
end
% Compilation of the S-function using autogenerated make script
if COMPILE
global ACADO_;
copyfile([ACADO_.pwd '/../../external_packages/qpoases3'], 'export_MPC/qpoases3')
cd export_MPC
make_acado_solver_sfunction
copyfile('acado_solver_sfun.mex*', '../')
cd ..
end
%% Initial MPC settings
disp('Initialization')
Vini = 10;
X0 = [Vini 0 0 0 0 0]; % Initial states conditions
% Initialize controller bus
input.x = repmat(X0, Np + 1, 1).'; % Size Np + 1
%input.od = zeros(Np+1,1); % Size Np + 1
input.od = repmat(Vini, Np + 1, 1)';
Uref = zeros(Np, 1);
input.u = Uref.'; % Control inputs
input.y = [repmat(X0, Np, 1) Uref].'; % Reference trajectory
input.yN = X0.'; % Terminal reference
input.W = diag([0 0 0 0 0 5 1]); % Weight tuning
input.WN = diag([0 0 0 0 5 1]); % Terminal weight tuning
input.x0 = X0.'; % Initial state
%input.od = ones(1,Np + 1) * Vx_ini; % Online data (Vx as an external parameter)
% Inicializar la estructura 'init' que pasará datos al bloque de Simulink
init.x = input.x(:).';
init.u = input.u(:).';
init.y = input.y(:).';
init.yN = input.yN(:).';
init.W = input.W(:).';
init.WN = input.WN(:).';
init.x0 = input.x0(:).';
%init.od = input.od(:).'; % Asegurar que se inicializa la entrada od (Vx)