Ts = 1e-4; % [s]
load('../mat/K_diag.mat', 'Kd');