Reduce simulation time in predictive control using fmincon

Hi I am designing an model predictive controller (mpc) for a differential wheeled robot in Simulink.

I have a model of the robot in the Simulink model where I want to implement my mpc. I have choosen to define the mpc without the use of the nlmpc function in matlab to make the code more accesable by the user and for learning purposes.

I have made the mpc within a matlab function block where I use the fmincon function as my optimization solver.

The problem is that when I run the code the simulation time is extremely dependend on the weights. I have the weight matrices

Q (7X7 diagonal)

R (2x2 diagonal)

and the simulation runs smoothly when I only have weights on the first 3 variables in Q. However, adding weight just one one additional variable (such that I weight the first 4 in Q) makes the simulation run extremely slow.

So my question is there a way to ease the simulation time such that it has a more consistent simulation time regardless of the weights?

The code I have within the function block is given below

function [J_val,exitflag,mv] = fcn(x_ref, x_prev, u_prev,param,lb, ub,Ts_mpc, Nc, N,Q, R,Ref)


% Define optimization problem
options = optimoptions('fmincon', 'Display', 'none','Algorithm','sqp');

% Define initial guess for control input
u_0 = u_prev;

% Define cost function
cost = @(u_0) Cost_fun(u_0,x_prev,x_ref,param,Ts_mpc,N,Q,R,Ref);

% Solve the optimization problem 
[u_sol, J_val, exitflag] = fmincon(cost, u_0, [], [], [],[], lb, ub,[], options); 

% Output the optimal control input
mv = u_sol;

end 


%%% ---------- Predictor ---------- %%% 
function x_next = predictor(u_0,x_prev,param,Ts_mpc,kappa)

    % Defining the parameters for the predictions 
    L_w     = param(1);
    L_OC    = param(2);
    L_OS_y1 = param(3);
    r_wheel = param(4);
    m_w     = param(5);
    m_body  = param(6);
    I_body  = param(7);
    I_w     = param(8);
    I_m     = param(9);
    kappa = 1; 
    % spray   = param(10);

    %Defining the current inputs 
    T_R = u_0(1); 
    T_L = u_0(2); 

    % Defining the current states  
    CTE = x_prev(1);     % Lateral error   
    ye  = x_prev(2);     % Orientation error 
    S   = x_prev(3);     % Traveled distance along path 
    vs  = x_prev(4);     % Measured velocity at spray point 
    w   = x_prev(5);     % Measured angular velocity 
    dv  = x_prev(6);     % Change in linear velocity
    dw  = x_prev(7);     % Change in angular velocity 
    v_cm = vs-L_OS_y1*w; % Linear velocity at CM

    % defining the differential equation for the states 
    CTE_dot = vs*sin(ye); 
    ye_dot  = w-(vs*cos(ye)/(1-CTE*kappa))*kappa;  
    v_dot   = (r_wheel*(T_L + T_R - L_OC*m_body*r_wheel*w^2))/(2*I_w + m_body*r_wheel^2 + 2*m_w*r_wheel^2);
    w_dot   = (r_wheel*(L_w*T_R - L_w*T_L + L_OC*m_body*r_wheel*w*v_cm))/(2*I_w*L_w^2 + I_body*r_wheel^2 + 2*I_m*r_wheel^2 + L_OC^2*m_body*r_wheel^2 + 2*L_w^2*m_w*r_wheel^2); 
    S_dot   = vs*cos(ye)/(1-CTE*kappa); 
    
    % Predict the next state based on the current state and input 
    CTE_next = CTE + Ts_mpc*CTE_dot; 
    ye_next  = ye  + Ts_mpc*ye_dot; 
    S_next   = S   + Ts_mpc*S_dot;
    v_next   = vs  + Ts_mpc*(v_dot+w_dot*L_OS_y1);
    w_next   = w   + Ts_mpc*w_dot;
    dv_next  = v_dot;
    dw_next  = w_dot;

    % Save the state prediction in the vector 
    x_next = [CTE_next;ye_next;S_next;v_next;w_next;dv_next;dw_next]; 

end 


%%% ---------- Optimization ---------- %%%
function J = Cost_fun(u_0,x_prev,x_ref,param,Ts_mpc,N,Q,R,Ref)

    %%% Optimization loop %%%
    J = 0; 
    for i =1:N-1
        
        % Defining the distance along the path 
        S_ref = Ref(:,9); 
        S     = x_prev(3); 
        % Updating reference based on robot location
        if S <= S_ref(1)
            j = 1; 
        else 
            j =  find(S>=Ref(:,9),1,'last');
        end 
        if j >= length(S_ref)-1
            j = length(S_ref)-1;
        end 
        kappa = Ref(j,7);
        x_ref = [0;0;Ref(j,9);Ref(j,10);Ref(j,11);0;0];

        % Predict the next states 
        x_next = predictor(u_0,x_prev,param,Ts_mpc,kappa);

        % Calculate the state error
        x_e = [x_next(1);x_next(2);x_ref(3) - x_next(3);x_ref(4) - x_next(4);x_ref(5) - x_next(5);x_next(6);x_next(7)];
    
        % Compute the cost function
        J = J + x_e'*Q*x_e + u_0'*R*u_0;
        % Update the current state
        x_prev = x_next;
    end 

end 

Sorry for the long post I hope you can help me.

Best regards.

Answer 1

[Rasmita]

Hi Kasper,

It is my understanding that, you are dealing with model predictive control (MPC) in Simulink and you are facing issues with simulation time while designing MPC using ‘fmincon’ function as optimization solver.

Here are a few suggestions to ease the simulation time and achieve more consistent results:

1. Experiment with different solver options and algorithms provided by ‘fmincon’. Some algorithms may have better convergence properties for specific problem characteristics. For example, the ‘interior-point’ algorithm is known for handling larger-scale problems efficiently. Refer to the below documentation link to know more about interior-point algorithm:

https://www.mathworks.com/help/mpc/ref/mpcinteriorpointsolver.html

1. The choice of simulation time step (Ts_mpc) can affect the convergence and simulation time. Try increasing or decreasing the time step to see if there has been any impact on the speed of convergence.
2. If your system supports parallel computing, consider utilizing multiple cores or distributed computing to speed up the optimization process. Refer to the below documentation link for further assistance on using parallel computing toolbox: https://www.mathworks.com/help/parallel-computing/getting-started-with-parallel-computing-toolbox.html

Remember to validate the performance of your modified MPC implementation after making any adjustments and assess whether the changes introduced affect the overall control performance and stability of the system.

For more information on ‘fmincon’ solver and configure options, please refer to the below documentation links:

https://www.mathworks.com/help/optim/ug/fmincon.html

https://www.mathworks.com/help/mpc/ug/configure-optimization-solver-for-nonlinear-mpc.html

Hope this helps you resolve the query!