Need help with figuring out the NMPC CasADi implementation in matlab

Hello,

I need some help with implementation of NMPC algorithm with CasADi optimization toolkit. I wrote a piece of code after reading the 75 page long CasADi manual. and looking at several examples. But I could not resovle the errors I am encourtering neither CHATGPT could help me fix those errors.

I am simulating clutch dynamics control using NMPC. Without CasADi everything works fine. I need to optimize the NMPC block further so I am using CasADi. However, its very difficult to get the grasp of it.

% CasADi Example for Clutch Dynamics Problem

clc; clear;close all;

import casadi.*

% Define variables

N = 20; % Horizon

Nx = 4; % Number of states

Nu = 1; % Number of controls

Ts = 0.01; % Sampling time

% Define CasADi variables

x = SX.sym('x', Nx, 1); % State vector

u = SX.sym('u', Nu, 1); % Control input

% System dynamics - Clutch Dynamics

dydt = clutchdynamics(x, u);

% Create CasADi function for dynamics

f = Function('f', {x, u}, {dydt});

M = 4; % RK4 steps per interval

DT = Ts / M; % Time step per RK4 stage

X0 = MX.sym('X0', Nx);

U = MX.sym('U', Nu);

X = X0;

for j = 1:M

k1 = f(X, U);

k2 = f(X + DT/2 * k1, U);

k3 = f(X + DT/2 * k2, U);

k4 = f(X + DT * k3, U);

X = X + DT/6 * (k1 + 2*k2 + 2*k3 + k4);

end

% Define the RK4 integrator as a CasADi function

F = Function('F', {X0, U}, {X});

% Objective weights

Q = diag([10, 10, 1, 1]); % State cost

R = 0.1; % Control cost

% Initial state and reference

x_ref = [0; 0; 0; 0]; % Desired state

u_ref = 6000; % Desired input

% Start with empty NLP

w = {}; w0 = []; lbw = []; ubw = []; % Decision variables

g = {}; lbg = []; ubg = []; % Constraints

J = 0; % Cost

% Initial state as decision variable

Xk = MX.sym('X0', Nx);

w = {w{:}, Xk};

lbw = [lbw; -inf * ones(Nx, 1)];

ubw = [ubw; inf * ones(Nx, 1)];

w0 = [w0; zeros(Nx, 1)];

% Formulate the NMPC problem

for k = 1:N

% Control input at step k

Uk = MX.sym(['U_' num2str(k)], Nu);

w = {w{:}, Uk};

lbw = [lbw; -1]; % Lower bound on control

ubw = [ubw; 1]; % Upper bound on control

w0 = [w0; 0]; % Initial guess for input

% Integrate dynamics

Xk_end = F(Xk, Uk);

% New state at next time step

Xk = MX.sym(['X_' num2str(k)], Nx);

w = {w{:}, Xk};

lbw = [lbw; -inf * ones(Nx, 1)];

ubw = [ubw; inf * ones(Nx, 1)];

w0 = [w0; zeros(Nx, 1)];

% Add system dynamics as equality constraint

g = {g{:}, Xk_end - Xk};

lbg = [lbg; zeros(Nx, 1)];

ubg = [ubg; zeros(Nx, 1)];

% Objective (quadratic cost)

J = J + (Xk - x_ref)' * Q * (Xk - x_ref) + (Uk - u_ref)' * R * (Uk - u_ref);

end

% Define terminal constraint if needed

g = {g{:}, Xk - x_ref};

lbg = [lbg; -0.01 * ones(Nx, 1)];

ubg = [ubg; 0.01 * ones(Nx, 1)];

% Define the NLP

prob = struct('f', J, 'x', vertcat(w{:}), 'g', vertcat(g{:}));

% Set solver options

opts = struct;

opts.ipopt.print_level = 0;

opts.print_time = false;

% Create solver using IPOPT

solver = nlpsol('solver', 'ipopt', prob, opts);

% Initial state

x0 = [0; 0; 0; 0]; % Initial condition

u_opt = zeros(Nu, 1); % Optimal control input storage

for t = 1:100 % Simulate for 100 steps

% Update initial condition

lbw(1:Nx) = x0;

ubw(1:Nx) = x0;

% Solve NMPC

sol = solver('x0', w0, 'lbx', lbw, 'ubx', ubw, ...

'lbg', lbg, 'ubg', ubg);

% Extract optimal control input

w_opt = full(sol.x);

u_opt = w_opt(Nx+1:Nx+Nu);

% Apply control and update state

x0 = full(F(x0, u_opt));

% Store solution for warm start

w0 = w_opt;

% Display results

fprintf('Step %d: Control = %.4f\n', t, u_opt);

end

figure;

subplot(2,1,1);

plot(1:100, u_opt);

title('Optimal Control Input');

xlabel('Time Step');

ylabel('Control u');

subplot(2,1,2);

plot(1:100, x0);

title('State Evolution');

xlabel('Time Step');

ylabel('State x');

function dydt = clutchdynamics(y,u)

% Define system parameters

je = 1.57;

jd = 0.0066;

jv = 4;

ks = 56665.5;

zeta = 0.01;

de = 2*zeta*sqrt(ks*je);

ds = 2*zeta*sqrt(ks*jd*jv/(jd+jv));

mu0 = 0.11;

muk = -0.001;

np = 14;

Rm = 0.0506;

%State Evolution Equations

dydt = zeros(4,1);

dydt(1) = (-de*y(1) - np*Rm*u*(mu0 + muk*(y(1)-y(2)))*tanh(2*(y(1)-y(2))))/je;

dydt(2) = (ks*y(4) + ds*(y(3) - y(2)) + np*Rm*u*(mu0 + muk*(y(1)-y(2)))*tanh(2*(y(1)-y(2))))/jd;

dydt(3) = (-ks*y(4) - ds*(y(3) - y(2)))/jv ;

dydt(4) = y(3) - y(2);

end

This is my first draft of code. The next step is to implement time varying reference inputs to this formulation for U-ref and X-ref.