sysid_multipleshooting.m

import casadi.*
load SilverboxSimulated
u_data = u;
y_data = y;

%%

% simulation horizon
N = size(u_data, 1);

% declare symbols for states and controls
y  = MX.sym('y');
dy = MX.sym('dy');
u = MX.sym('u');

% all states
states = [y;dy];
controls = u;

M = optivar();
c = optivar();
k = optivar();
k_NL = optivar();

params = [M;c;k;k_NL];

% Construct the right hand side
rhs = [dy ; (u-k_NL*y^3-k*y-c*dy)/M];

% Form an ode function
ode = MXFunction('ode',{states,controls,params},{rhs});

N_steps_per_sample = 10;
dt = 1/fs/N_steps_per_sample;

% Build an integrator for this system: Runge Kutta 4 integrator
k1 = ode({states,controls,params});
k2 = ode({states+dt/2.0*k1{1},controls,params});
k3 = ode({states+dt/2.0*k2{1},controls,params});
k4 = ode({states+dt*k3{1},controls,params});

states_final = states+dt/6.0*(k1{1}+2*k2{1}+2*k3{1}+k4{1});

% Create a function that simulates one step propagation in a sample
one_step = MXFunction('one_step',{states, controls, params},{states_final});

X = states;

for i=1:N_steps_per_sample
  Xn = one_step({X, controls, params});
  X = Xn{1};
end

% Create a function that simulates all step propagation on a sample
one_sample = MXFunction('one_sample',{states, controls, params}, {X});

% speedup trick: expand into scalar operations
one_sample = one_sample.expand();

% all states at every sample are decision variables
X = optivar(2, N);

params_scale = [1e-6*M;c*1e-4;k;k_NL];

Xn = one_sample.map({X, u_data', params_scale});
Xn = Xn{1};

% gap-closing constraints
gaps = Xn(:,1:end-1)-X(:,2:end);
g = gaps == 0;

e = (y_data-Xn(1,:)');

M.setInit(5);
c.setInit(2.3);
k.setInit(1);
k_NL.setInit(4);
X.setInit([ y_data [diff(y_data)*fs;0]]');

options = struct;
options.codegen = true;

% Hand in a vector objective -> interpreted as 2-norm 
% such that Gauss-Newton can be performed
optisolve(e,{g},options);

optival(M)*1e-6
optival(c)*1e-4
optival(k)
optival(k_NL)