% 3.3 Nonlinear LSE optimization

clear;
close all;

% Data (column vectors)
x = [1 2 3 4 5 6 7 8 9 10]';
y = [1.7321 -0.0509 -1.4885 -1.8603 -0.7881 1.5522 2.2165 0.8167 -1.0226 -1.9651]';

% Model: f = a1*sin(x+a2)
% State: s = [a1,a2];

% Initial state
s0 = [1; 0];

% Gauss-Newton Loop

% Initial state
s = s0;
loop_cond = true;
iter = 0;
while(loop_cond)
    
    iter = iter+1;
    
    % Error vector and Jacobian matrix of f
    [E,J] = LSE_sinus(s,x,y);
    % Gauss-Newton increment
    ds = inv(J'*J)*J'*E;
    s = s+ds
    if(norm(ds)<0.001)
        loop_cond = false;
    end
    s_tot1(:,iter) = s;
end

figure;
plot(s_tot1(1,:));
hold;
plot(s_tot1(2,:));

iter = iter

% Plot the results
% The estimated sinusoid is: f = s(1)*sin(x+s(2))
xx = 1:0.01:10;
y0 = s0(1)*sin(xx+s0(2));
yest = s(1)*sin(xx+s(2));

figure;
plot(xx,y0,'r');
hold;
plot(xx,yest);
plot(x,y,'b.');

% Levenberg-Marquardt Loop

% Initial LM parameter
lambda = 1;

% Initial state
s = s0;
loop_cond = true;
iter = 0;
while(loop_cond)

    iter = iter+1;
    
    % Error vector and Jacobian matrix of f
    [E,J] = LSE_sinus(s,x,y);

    % Levenberg-Marquardt increment
    ds = inv(J'*J+lambda*eye(2,2))*J'*E;
    
    [E2,J] = LSE_sinus(s+ds,x,y);
    
    % If the error is decreasing, accept the new parameters and decrease lambda
    if(norm(E2)<norm(E))
        s = s+ds;
        lambda = lambda/10
    % Else, reject it and increase lambda
    else
        lambda = lambda*10
    end
    
    if(norm(ds)<0.001)
        loop_cond = false;
    end

    s_tot2(:,iter) = s;
end

% Plot the results
% The estimated sinusoid is: f = s(1)*sin(x+s(2))
xx = 1:0.01:10;
y0 = s0(1)*sin(xx+s0(2));
yest = s(1)*sin(xx+s(2));

figure;
plot(xx,y0,'r');
hold;
plot(xx,yest);
plot(x,y,'b.');
