%%
clear; close all; clc;

%%
% load('mat/Kiff.mat', 'Kiff');
s = zpk( 's');
Kiff = -2e2*...
       (1/(s + 2*pi*20))*...    % LPF: provides integral action above x[Hz]
       (s/(s + 2*pi*20))*...    % HPF: limit low frequency gain
       (1/(1 + s/2/pi/400))*... % LPF: more robust to high frequency resonances
       eye(6);                  % Diagonal controller

%%
load('sim_data/data_sim.mat', 'Ts')
Kiff = c2d(Kiff, Ts, 'Tustin');

%%
save('sim_data/Kiff.mat', 'Kiff');