% ENM 4.25
% Denna kod skrevs på övningen 2017-11-17 i SF1545

% Gauss-Newtons metod för överbestämda ickelinjära ekvationssystem

clear

% Datapunkter som vi vill anpassa en funktion till
t = [0.8 1.7 2.5 3.3 4.1]';
y = [0.12 -0.09 0.06 -0.05 0.032]';

% Funktionen vi vill anpassa
f = @(x) -0.17*exp(-x(1)*t).*(cos(x(2)*t) + x(1)/x(2) * sin(x(2)*t)) - y;

% Startgissning för Gauss-Newton
w0 = 2*pi/(t(3)-t(1));
b0 = -(log(y(1))-log(y(3)))/(t(1)-t(3));
x = [b0 w0]';
% För er som var på övningen: det var startgissningen som var fel
% inskriven, därför funkade det inte. Fixat nu.

tol = 1e-12; tn = 1;
while norm(tn) >= tol
    fx = f(x);
    % Vi gör differensapproximation av Jacobimatrisen här
    h = 1e-6;
    fbh = f([x(1)-h, x(2)]);
    fwh = f([x(1), x(2)-h]);
    J = [(fx-fbh)/h, (fx-fwh)/h]; % bakåtdifferenser (t.ex.)
    tn = J\fx; % OBS! Minstakvadratmetoden
    x = x - tn
    norm(tn)
end

% Bonus: rita ut data och anpassad funktion
clf
plot(t, y, '*')
hold on
tfin = linspace(0.5, 4.5);
yfin = -0.17*exp(-x(1)*tfin).*(cos(x(2)*tfin) + x(1)/x(2) * sin(x(2)*tfin));
plot(tfin, yfin)
