Identification including coriolis force

coriolis_identify_2_state.m

input = true;

h = 0.01;

data = load('no_input_test.mat').IO_data;

if (input)
    data = load('stair_test.mat').IO_data;
    %data = load('/MATLAB Drive/data/n_04_06_08_stair_45_2.mat').IO_data;
    %data = load('/MATLAB Drive/data/0_step_20_2.mat').IO_data;
end

disp('h:');
disp(h);

offset = int32(2.3 / h);
% offset = 200 works best for ramp_1_60
t = data(1, offset:end);

x_1 = data(2, offset:end); % pitch

w_1 = data(4, offset:end);

u_1 = zeros(5, length(t));

if (input)
    u_1 = data(5, offset:end); % pitch input
end

x_1 = lowpass(x_1, 0.00001);
w_1 = lowpass(w_1, 0.00001);

x_2 = gradient(x_1, t); % pitch velocity
x_2 = lowpass(x_2, 0.00001);

x_2_dot = gradient(x_2, t); % pitch acceleration

cut_off = 20;

x_1 = x_1(cut_off:end-cut_off);
x_2 = x_2(cut_off:end-cut_off);
x_2_dot = x_2_dot(cut_off:end-cut_off);
t = t(cut_off:end-cut_off);
u_1 = u_1(cut_off:end-cut_off);
w_1 = w_1(cut_off:end-cut_off);

c_guess = [-10.5888    0.0054    0.0748   -6.9064   -0.2750   -6.1290    0.1000    0.1000    0.0100    0.0100];

fun = @(c) f2(input, c, c_guess, x_1, x_2, u_1) - x_2_dot;

c = lsqnonlin(fun, c_guess);

disp('c:');
disp(c);

n = length(t);
x_1_e = zeros(1,n); 
x_2_e = zeros(1,n);
x_1_e(1) = x_1(1);
x_2_e(1) = x_2(1);

fprintf('x_1(1) %d\n',x_1(1));
fprintf('x_2(1) %d\n',x_2(1));
fprintf('x_2_dot(1) %d\n',x_2_dot(1));

for i=2:n  
    f_1_x = x_2_e(i-1);

    x_1_e(i) = x_1_e(i-1) + f_1_x * h;

    f_2 = f2(input, c, c_guess, x_1_e(i), x_2_e(i-1), u_1(i));
  
    x_2_e(i) = x_2_e(i-1) + f_2 * h;
end

figure(1);
plot(t, rad2deg(x_1), 'r-', t, rad2deg(x_1_e), 'b-');
%figure(2);
%plot(t, rad2deg(x_2), 'r-', t, rad2deg(x_2_e), 'b-');

%plot(t, u_1, 'r-');
%plot(t, w_1, 'r-');

xlabel('time (s)');
ylabel('pitch (deg)');

legend({'Data','Estimate'},'Location','north')

function f = f2(input, c, c_guess, x_1, x_2, u_1)
    if (input)
        c(1) = c_guess(1);
        c(2) = c_guess(2);
        c(3) = c_guess(3);
    end

    f = c(1) .* sin(x_1 + c(2)) ...
      - c(3) .* x_2;
    if (input)
      w = abs(u_1) .^ 0.6602;
      f = f + c(4) .* sign(u_1) .* w .^ 2 ...
      + c(5) .* w .* x_2;
    end
end