Fitting sinusoidal data

system_identify_sinusoidal.m

input = true;

data = load('/MATLAB Drive/data/no_input_lift_to_maxdeg60_60_2.mat').IO_data;

if (input)
    data = load('/MATLAB Drive/data/04_06_08_00_stair_60_2.mat').IO_data;
end

t = data(1, :);
h = t(5) - t(4); % time step

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

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

x_1_scale = 0.792;
x_1_offset = -0.0107;

x_1 = x_1_scale * data(2, offset:end) + x_1_offset; % pitch

w_1 = data(5, offset:end);

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

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

x_1 = smoothdata(x_1);

w_2 = gradient(w_1, t); % rpm velocity
x_2 = gradient(x_1, t); % pitch velocity
x_2_dot = gradient(x_2, t); % pitch acceleration

cut_off = 100;

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);
w_2 = w_2(cut_off:end-cut_off);

c_guess = [-10.7575    0.0964    0.219];

if (input)
    c_guess = [c_guess, 1 1 1]; % 2.0506 1.4799
end

fun = @(c) objFunctions(input, c, c_guess, x_1, x_2, u_1, w_1, x_2_dot, w_2);

c = lsqnonlin(fun, c_guess);

function [rssOutput] = objFunctions(input, c, c_guess, x_1, x_2, u_1, w_1, x_2_dot, w_2) 
    f_2 = f2(input, c, c_guess, x_1, x_2, u_1, w_1);
    f_3 = f3(input, c, c_guess, x_1, x_2, u_1, w_1);
    
    rss1 = f_2 - x_2_dot; % objective function 1
    rssOutput = [rss1]; 
    if (input)
        rss2 = f_3 - w_2; % objective function 2
        rssOutput = [rss1 rss2];  
    end
  end

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

n = length(t);
x_1_e = zeros(1,n); 
x_2_e = zeros(1,n);
w_1_e = zeros(1,n);
w_2_e = zeros(1,n);
x_1_e(1) = x_1(1);
x_2_e(1) = x_2(1);
w_1_e(1) = w_1(1);
w_2_e(1) = w_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));
fprintf('w_1(1) %d\n',w_1(1));
fprintf('w_2(1) %d\n',w_2(1));

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

    w_1_e(i) = w_1_e(i-1) + f_1_w * h;
    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), w_1(i));
    f_3 = f3(input, c, c_guess, x_1_e(i), x_2_e(i-1), u_1(i), w_1_e(i));

    x_2_e(i) = x_2_e(i-1) + f_2 * h;
    w_2_e(i) = w_2_e(i-1) + f_3 * h;
end

figure(1);
plot(t, rad2deg(x_1), 'r-', t, rad2deg(x_1_e), 'b-');
%plot(t, rad2deg(x_2), 'r-', t, rad2deg(x_2_e), 'b-');
%plot(t, w_1, 'r-', t, w_1_e, 'b-');
%plot(t, w_2, 'r-', t, w_2_e, 'b-');

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

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

function f = f2(input, c, c_guess, x_1, x_2, u_1, w_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)
        f = f + c(4) .* sign(w_1) .* w_1 .^ 2;
    end
end

function f = f3(input, c, c_guess, x_1, x_2, u_1, w_1)
    f = 0;
    if (input)
        f = 0;%36 .* u_1 - c(6) .* sign(w_1) .* w_1 .^ 2; 
    end
end