%Finn Aakre Haugen (finn@techteach.no)
%240218
%----------------------------------------------------------
disp('Newtons method to solve an')
disp('optimization problem:')
disp('min_x f(x)')
disp('s.t.')
disp('model: f=(x1-p1)^2+(x2-p2)^2+p3')
disp('params: p1 = 1, p2 = 2, p3 = 3')
%----------------------------------------------------------
format compact
clear all
close all

p1=1;
p2=2;
p3=3;
params_model.p1=p1;%params_model is a struct. p1 is a field.
params_model.p2=p2;
params_model.p3=p3;

x_k=[4,5]';%Init
k=0;
N=100;%Preset max number of iterations
abs_df=1e-4;%Stopping criterion
for K=1:N-1
    %Gradient:
    G_k=[2*(x_k(1)-p1);
        2*(x_k(2)-p2)];
    %Hessian:
    H_k=[2,0;
        0,2];
    %Calculation of increment of x (used in x_kp1=x_k+dx_k):
    %dx_k=-inv(H_k)*G_k;
    %A numerically better alternative to calc dx_k:
    %dx_k is solution of following system of lin eqs: H_k*dx_k = -G_k
    %which can be effectively solved using '\' operator:
    dx_k=-H_k\G_k;
    
    x_kp1=x_k+dx_k;
    f_k=f(x_k,params_model);
    f_kp1=f(x_kp1,params_model);
    df=f_kp1-f_k;
    x_k=x_kp1;
    if abs(df) < abs_df
        break
    end
    k=k+1;
end
disp('-------------------')
disp('Results:')
k
disp('Optimal solution:')
x_opt=x_kp1
f_min=f(x_opt,params_model)

%------------------------------------------
%Definition of f() as local function:
function [f_value]=f(x,params_model);
p1=params_model.p1;
p2=params_model.p2;
p3=params_model.p3;
f_value=(x(1)-p1)^2+(x(2)-p2)^2+p3;
end