Constrained SINDy as a nlp_problem

casto · January 8, 2025, 10:16am

Hi

I’m currently trying to implement a constrained version of SINDy with Acados on Matlab.
Till now i’m feeding the matrix regarding the library function used in SINDy and the derivative vector of the state directly in the function that i want to minimize.
Now i want that these two are set as parameters that then i will feed with the appropriate matrix and vector.
Is there any method to do this?
The code now is this one:

clear all

clc

import casadi.*; % Import CasADi library

% Step 1: Generate simulated data

dt = 0.01; % Time step

t = 0:dt:1; % Time vector

A_true = randi([-9,-1],8,8); % True system matrix

x0 = ones(8,1); % Initial condition

x = zeros(length(x0), length(t));

x(:, 1) = x0;

for k = 1:length(t) - 1

x(:, k + 1) = x(:, k) + dt * A_true * x(:, k); % Simple Euler integration

end

x_dot = diff(x,1,2) / dt; % Numerical derivative

% Rescale data

Theta = [x(1, :)', x(2, :)', x(3, :)', x(4, :)', x(5, :)', x(6, :)', x(7, :)', x(8, :)']; % Linear terms only

Theta=Theta(1:end-1,:);

% Step 2: Define problem variables

eps=casadi.SX.sym('eps',8);

f = abs((Theta * eps - x_dot(1, :)')' * (Theta * eps - x_dot(1, :)'));

% Constraint bounds

g = [eps(1);eps(2);eps(3);eps(4);eps(5);eps(6);eps(7);eps(8)];

glb = -10*ones(8,1);

gub = 10*ones(8,1);

% Step 3: Model setup

model = AcadosModel();

model.name = 'prova_sindy';

model.x = eps;

model.f_expl_expr = casadi.SX.zeros(length(model.x),1);

% Cost and constraints

ocp = AcadosOcp();

ocp.name = 'nlp_solver';

ocp.model = model;

ocp.cost.cost_type_e = 'EXTERNAL';

ocp.model.cost_expr_ext_cost_e = f;

ocp.model.con_h_expr_e = g;

ocp.constraints.lh_e = glb;

ocp.constraints.uh_e = gub;

ocp.cost.cost_type_0 = 'EXTERNAL';

ocp.model.cost_expr_ext_cost_0 = 0;

ocp.cost.cost_type = 'EXTERNAL';

ocp.model.cost_expr_ext_cost = 0;

% Solver options

ocp.solver_options.tf = 1;

ocp.solver_options.N_horizon = 1;

ocp.solver_options.nlp_solver_type = 'SQP_RTI';

ocp.solver_options.nlp_solver_max_iter = 1000;

ocp.solver_options.nlp_solver_tol_stat = 1e-6;

ocp.solver_options.nlp_solver_tol_eq = 1e-6;

ocp.solver_options.nlp_solver_tol_comp = 1e-6;

% Solver creation

ocp_solver = AcadosOcpSolver(ocp);

% Initial guess

init_x = ones(8,1);

ocp_solver.set('init_x', repmat(init_x,1,2));

% Solve

tic

ocp_solver.solve();

total_time = toc;

% Check status

status = ocp_solver.get('status');

if status ~= 0

warning(['Solver failed with status ', num2str(status)]);

end

% Results

x_opt = ocp_solver.get('x',1);

disp('Optimal solution:')

disp(x_opt)

disp(['Total time: ', num2str(1e3 * total_time), ' ms'])

I have seen the example in the nlp_generic but i do not have a simple value as parameters but instead a matrix and a vector.

Waiting for your reply

kaethe · January 8, 2025, 10:50am

Not sure if I understand your question correctly, but it looks like you want to use a linear discrete time model, please check this example.

Note that the discrete time system dynamics can also depend on additional parameters provided in model.p.

casto · January 8, 2025, 2:01pm

Hi kaethe, thaks to your fast reply.

I explain better what i need to do.
I’m currently trying to implement a MPC with a 3dof vehicle model to control a vehicle simulated in simulink with CarMaker.
The scope of my work is to use SINDy algorithm to discover a 3 dof vehicle model that better represent the dynamics of the simulated vehicle.
Currently i have implemented the MPC that control the vehicle and that has input as parameters also some coeeficients derived from SINDy used in the mpc model.
Now the problem is that if i want to use a constrained SINDy version using cvx, the algorithm becomes too slow so i’m trying to implement constrained SINDy in acados.
Currently i could run the script in matlab but i want that the minimization function changes in function of the new measurement taken during the simulation, so i want that the library function ‘Theta’ and that the derivative vector ‘dXdt’ are assigned as parameters that then could be changed during the simulation on simulink.
I share the script on matlab that is currently running, as i said i fed the matrix of the library function, where the columns are the different values of the functions used in SINDy a time step, and the vector of the derivative of the state; then i set up the problem and i recover the weight of the functions that better approximate ‘dXdt’.

Code

clear all

clc

import casadi.*; % Import CasADi library

load('Theta.txt')

load('dXdt.txt')

% Step 2: Define problem variables

eps=casadi.SX.sym('eps',7);

% Cost function with regularization

lambda = 1e-3; % Regularization term

f = abs((Theta * eps - dXdt)' * (Theta * eps - dXdt));

% Constraint bounds

g = [eps(1);eps(2);eps(3);eps(4);eps(5);eps(6);eps(7)];

glb = [0.001;50;-4000;70000;-600000;-0.05;-0.001];

gub = [0.002;100;-3000;90000;-500000;-0.01;0];

% Step 3: Model setup

model = AcadosModel();

model.name = 'prova_sindy';

model.x = eps;

model.f_expl_expr = casadi.SX.zeros(length(model.x),1);

% Cost and constraints

ocp = AcadosOcp();

ocp.name = 'nlp_solver';

ocp.model = model;

ocp.cost.cost_type_e = 'EXTERNAL';

ocp.model.cost_expr_ext_cost_e = f;

ocp.model.con_h_expr_e = g;

ocp.constraints.lh_e = glb;

ocp.constraints.uh_e = gub;

ocp.cost.cost_type_0 = 'EXTERNAL';

ocp.model.cost_expr_ext_cost_0 = 0;

ocp.cost.cost_type = 'EXTERNAL';

ocp.model.cost_expr_ext_cost = 0;

% Solver options

ocp.solver_options.tf = 1;

ocp.solver_options.N_horizon = 1;

ocp.solver_options.nlp_solver_type = 'SQP_RTI';

ocp.solver_options.nlp_solver_max_iter = 1000;

ocp.solver_options.nlp_solver_tol_stat = 1e-6;

ocp.solver_options.nlp_solver_tol_eq = 1e-6;

ocp.solver_options.nlp_solver_tol_comp = 1e-6;

% Solver creation

ocp_solver = AcadosOcpSolver(ocp);

% Initial guess

init_x = ones(7,1);

ocp_solver.set('init_x', repmat(init_x,1,2));

N=10;

tic;

for i=1:N

% Solve

if i>1

glb(smallinds)=0;

gub(smallinds)=0;

ocp.constraints.lh_e = glb;

ocp.constraints.uh_e = gub;

end

ocp_solver.solve();

x_opt = ocp_solver.get('x',1);

smallinds = (abs(x_opt)<lambda);

end

total_time=toc;

% Check status

status = ocp_solver.get('status');

if status ~= 0

warning(['Solver failed with status ', num2str(status)]);

end

% Results

x_opt = ocp_solver.get('x',1);

disp('Optimal solution:')

disp(x_opt)

disp(['Total time: ', num2str(1e3 * total_time), ' ms'])
...

In the end, i want that Theta and dXdt are fed as parameters in order to be able to change them in simulink without the need of computing another s-function.
Just to be sure Theta could be a matrix of 500x7 and dXdt a vector of 500x1.

kaethe · January 8, 2025, 2:11pm

I still don’t understand how your model is defined.

Could you write it down in one of the two following forms?

Continuous-time ODE: \dot{x} = f_\mathrm{expl}(x, u, p)
Discrete-time model x_{k+1} = f_{\mathrm{discrete}}(x_k, u_k, p)

with states x, controls u and parameters p

josipkh · January 8, 2025, 2:21pm

Hi,

I just wanted to add that Theta and dXdt can also be defined as parameters (similar to p here).
Later, you should be able to set them online as done here.

However, doing SINDy in every MPC step might still be too slow.
Maybe reducing the frequency of SINDy (e.g., every N samples) helps with that.

Kind regards,
Josip

casto · January 8, 2025, 2:26pm

i’m trying to solve a nonlinear program as: acados/examples/acados_matlab_octave/generic_nlp at master · acados/acados · GitHub

The dynamics is:
x_dot = 0;

and the terminal cost is:
f = (Thetax-dXdt)'(Theta*x-dXdt)

My problem is that i don’t know how to make parameters Theta and dXdt, in order to have:
f = (p(1)x-p(2))'(p(1)*x-p(2))

because Theta and dXdt are matrix or vectors and not single values as in the example.

casto · January 8, 2025, 2:30pm

I’m trying to do the same thing but it doesn’t work, it gave me no error but the result is different from the case when i do not fed Theta and dXdt as parameters, that is probably caused by the fact that i’m not doing the right thing, as Theta and dXdt are not single values as in the example.

Yeah probably i will implemnt a trigger in order to use SINDy whenever it will be usefull and not every iteration.

josipkh · January 8, 2025, 3:17pm

I understand, not sure what the exact problem is, but for me it works by modifying the code from your first post:

clear all; clc;
import casadi.*; % Import CasADi library

is_parametric = 0;

% Step 1: Generate simulated data
dt = 0.1; % Time step
t = 0:dt:1; % Time vector
A_true = [0.8 0.1;
          0.9 0.2];
nx = 2;
x0 = ones(nx,1); % Initial condition
x = zeros(length(x0), length(t));
x(:, 1) = x0;

for k = 1:length(t) - 1
    x(:, k + 1) = x(:, k) + dt * A_true * x(:, k); % Simple Euler integration
end

x_dot = diff(x,1,2) / dt; % Numerical derivative
if is_parametric
    x_dot = SX.sym('x_dot',size(x_dot));
end

% Rescale data
Theta = [x(1, :)', x(2, :)']; % Linear terms only
Theta = Theta(1:end-1,:);
if is_parametric
    Theta = SX.sym('Theta',size(Theta));
end

% Step 2: Define problem variables
eps = casadi.SX.sym('eps',nx);
f = abs((Theta * eps - x_dot(1, :)')' * (Theta * eps - x_dot(1, :)'));

% Constraint bounds
g = [eps(1);eps(2)];
glb = -10*ones(nx,1);
gub = 10*ones(nx,1);

% Step 3: Model setup
model = AcadosModel();
model.name = 'prova_sindy';
model.x = eps;
model.f_expl_expr = casadi.SX.zeros(length(model.x),1);
if is_parametric
    model.p = vertcat(Theta(:),x_dot(:));
end

% Cost and constraints
ocp = AcadosOcp();
ocp.name = 'nlp_solver';
ocp.model = model;
ocp.cost.cost_type_e = 'EXTERNAL';
ocp.model.cost_expr_ext_cost_e = f;
ocp.model.con_h_expr_e = g;
ocp.constraints.lh_e = glb;
ocp.constraints.uh_e = gub;
ocp.cost.cost_type_0 = 'EXTERNAL';
ocp.model.cost_expr_ext_cost_0 = 0;
ocp.cost.cost_type = 'EXTERNAL';
ocp.model.cost_expr_ext_cost = 0;

% Solver options
ocp.solver_options.tf = 1;
ocp.solver_options.N_horizon = 1;
ocp.solver_options.nlp_solver_type = 'SQP_RTI';
ocp.solver_options.nlp_solver_max_iter = 1000;
ocp.solver_options.nlp_solver_tol_stat = 1e-6;
ocp.solver_options.nlp_solver_tol_eq = 1e-6;
ocp.solver_options.nlp_solver_tol_comp = 1e-6;

% Solver creation
ocp_solver = AcadosOcpSolver(ocp);

% Initial guess
init_x = ones(nx,1);
ocp_solver.set('init_x', repmat(init_x,1,2));

% Solve
tic
if is_parametric
    load test_data.mat
    ocp_solver.set('p',[Theta(:);x_dot(:)]);
end
ocp_solver.solve();
total_time = toc;

% Check status
status = ocp_solver.get('status');
if status ~= 0
    warning(['Solver failed with status ', num2str(status)]);
end

% Results
x_opt = ocp_solver.get('x',1);
disp('Optimal solution:')
disp(x_opt)
disp(['Total time: ', num2str(1e3 * total_time), ' ms'])

if ~is_parametric
    save test_data.mat x_dot Theta
end

(I reduced the problem dimensions for easier debugging)

The flag is_parametric sets up the problem using Theta and dXdt as parameters.

Hope this helps!

casto · January 8, 2025, 4:02pm

Thanks josipkh, the code works, i’m setting the parameters in the wrong way, i was putting them as a single parameter while the parameters has the size of the sum of Theta and x_dot.

I have just one more question, i have verified that it works also on the other code that i have shared but it takes so much time to obtain the result, also before this modification it took much time in setting up the problem because if i take the elapsed time of only the ‘solve’ command i see around 30 ms while to run the entire script around 5 minutes.
So my question is when i will generate the s-function and put that in simulink it will be fast or there will be the same problem?

josipkh · January 9, 2025, 9:08am

Hi, maybe you’ve already tried by now, but I think that the execution time in Simulink should be close to 30 ms since the solver setup and code generation are done offline.

casto · January 9, 2025, 2:15pm

I’ve not tried yet, anyway thanks for your support.