sysid_singleshooting.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();

% Create a function that evaluates a single shooting sequence of 
sim = one_sample.mapaccum('sim', N);


y0 = zeros(2,1);

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

% simulate through all samples with numerical y0, u
% and symbolic parameters
ysim = sim({y0,u_data, repmat(params_scale,1,N) });
ysim = ysim{1};

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

M.setInit(5);
c.setInit(2.3);
k.setInit(1);
k_NL.setInit(4);

options = struct;
options.codegen = true;

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

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