function [X,F,Cp,PP,Hist,params] = AO(funopts)

% A (Bayesian) gradient/curvature descent optimisation routine, designed primarily

% for nonlinear model fitting / system identification / parameter estimation.

%

% The algorithm combines a Newton-like gradient routine (& optionally

% MAP and ML) with line search and an optimisation on the composition of update

% parameters from a combination of the gradient flow and memories of previous updates.

% It further implements an exponential cost function hyperparameter which

% is tuned independently, and an iteratively updating precision operator.

%

% The objective function minimises a smooth Gaussian process eror term, or

% alternatively the free energy (~ ELBO - evidence lower bound), SSE or RMSE.

% Or a precision-weighted RMSE, 'qrmse'.

%

% General idea:

% Y0 = f(x) + e

%

% Y0 = empirical data (to fit)

% f = model (function)

% x = model parameters/inputs to be optimised

% (treated as Gaussians with variance V)

%

% For a multivariate function f(x) where x = [p1 .. pn]' the ascent scheme

% is:

%

% x[p,t+1] = x[p,t] + a[p] *-dFdx[p]

%

% Note the use of different step sizes, a[p], for each parameter.

% Optionally, low dimensional hyperparameter tuning can be used to find

% the best step size a[p], by setting step_method = 6;

%

% x[p,t+1] = x[p,t] + (b*V) *-dFdx[p] ... where b is optimised

% independently

%

% where V maps between a (reduced) parameter subspace and the full vector.

% dFdx[p] are the partial derivatives of F, w.r.t parameters, p. (Note F =

% the objective function and not 'f' - your function). See jaco.m for options,

% although by default these are computed using a finite difference

% approximation of the curvature, which retains the sign of the gradient:

%

% f0 = F(x[p]+h)

% fx = F(x[p] )

% f1 = F(x[p]-h)

%

% j(p,:) = (f0 - f1) / 2h

% ----------------------------

% (f0 - 2 * fx + f1) / h^2

%

% nb. - depending on the specified step method, a[] is computed from j & V

% using variations on: (where V is the variance of each param)

%

% J = -j ;

% dFdpp = approx -(J'*J); -- however, we normalise out the full magnitude

% of the gradient using a LDL decomposition,

% such that;

% dFdpp = -L*(D./sum(D))*L'

% a = (V)./(1-dFdpp);

%

% If step_method = 3, dFdpp is a small number and this reduces to a simple

% scale on the stepsize - i.e. a = V./scale. However, if step_method = 1, J

% is transposed, such that dFdpp is np*np. This leads to much bigger

% parameter steps & can be useful when your start positions are a long way

% from the solution. In either case, note that the variance term V[p] on the

% parameter distribution is a scaled version of the step size.

%

% step methods:

% -- step_method = 1 invokes steepest descent

% -- step_method = 3 or 4 invokes a vanilla dx = x + a*-J descent

% -- step_method = 6 invokes hyperparameter tuning of the step size.

% -- step_method = 7 invokes an eigen decomp of the Jacobian matrix

% -- step_method = 8 converts the routine to a mirror descent with

% Bregman proximity term.

%

% By default momentum is included (opts.im=1). The idea is that we can

% have more confidence in parameters that are repeatedly updated in the

% same direction, so we can take bigger steps for those parameters as the

% optimisation progresses.

%

% For each iteration of the ascent:

%

% dx[p] = x[p] + a[p]*-dFdx

%

% With the MLE setting, the probability of each predicted dx[p] coming

% from x[p] is computed (Pp) and incorporated into a NL-WLS implementation of

% MLE:

%

% b(s+1) = b(s) - (J'*Pp*J)^-1*J'*Pp*r(s)

% dx = x - (a*b)

%

% Similarly, setting DoMAP = 1 or DoMAP_Bayes = 1 will result in maximum

% aposteriori estimates for parameters using a hyperparameter-tuned GaussNewton/

% Levenberg-Marquardt scheme:

%

% dx = x - ( [lambda*eye(D)] + j*j' )\(j*r0);

%

% where the regularisation parameter lambda is optimised. r0 is the redisual.

% (Note that inv(lambda*eye(D)+j*j') is the Fisher-information matrix).

%

% By convention you can onvert from an MLE scheme to MAP by switching

% either op.EnforcePriorProb=1 or op.WeightByProbability=1, or both. This

% invoke a scheme which minimises error while maximising the pdf of the

% param

%

%

% NOTE - the default option [2022] is now to use a regularised Newton routine:

%---------------------------------------------------------------------------

%

% dx = x + inv(H*L*H)*-J

%

% whewre H is the Hessian and J the jacobian (dFdx). L is a regularisation

% term that is optimised independently.

%

%

%

% Other stuff:

%

% With the Gaussian Process Regression option (do_gpr), a GP is fitted over

% (gradient) predictions and best errors to re-estimate the parameter step

% on each iteration. This is applied as a "refinement" step on the gradient

% predicted dx.

%

% Set opts.hypertune = 1 to append an exponential cost function to the chosen

% objective function. This is defined as:

%

% c = t * exp(1/t * data - pred)

%

% where t is a (temperature) hyperparameter controlled through a separate gradient

% descent routine.

%

% ALSO SET:

%

% opts.memory_optimise = 1; to optimise the weighting of dx on the gradient flow and recent memories

% opts.opts.rungekutta = 1; to invoke a runge-kutta optimisation locally

% around the gradient predicted dx

% opts.updateQ = 1; to update the error weighting on the precision matrix

%

%

% The {free energy} objective function minimised is

%==========================================================================

% L(1) = spm_logdet(iS)*nq/2 - real(e'*iS*e)/2 - ny*log(8*atan(1))/2; % complexity minus accuracy of states

% L(2) = spm_logdet(ipC*Cp)/2 - p'*ipC*p/2; % complexity minus accuracy of parameters

% L(3) = spm_logdet(ihC*Ch)/2 - d'*ihC*d/2; % complexity minus accuracy of precision (hyperparameter)

% F = -sum(L);

%

% *Alternatively, set opts.objective to 'rmse' 'sse' 'euclidean' 'mvgkl'...

%

% INPUTS:

%-------------------------------------------------------------------------

% To call this function using an options structure:

%-------------------------------------------------------------------------

% opts = AO('options'); % get the options struct

% opts.fun = @myfun % fill in what you want...

% opts.x0 = [0 1]; % start param value

% opts.V = [1 1]/8; % variances for each param

% opts.y = [...]; % data to fit - .e.g y = f(p) + e

% opts.maxit = 125; % max num iterations

% opts.step_method = 1; % step method - 1 (big), 3 (small) and 4 (vanilla)**.

% opts.im = 1; % flag to include momentum of parameters

% opts.order = 2; % partial derivate fun option (see jaco.m): 1 or 2

% opts.hyperparameters = 0; % flag to do a grad ascent on the precision (3rd term in FE)

% opts.criterion = -300 % convergence threshold

% opts.mleselect = 0; % maximum likelihood param selection

% opts.objective = 'fe'; % objective fun: 'free energy', 'logevidence','sse', ...

% opts.writelog = 0; % flag to write logbook instead of to console

% opts.Q = []; % square matrix precision operator ( size=length(y) )

% opts.inner_loop = 10; % limit on number of repeats on same decent (between grad comp)

% opts.DoMLE = 0; % flag to perform a sort of MLE via WLS

% opts.force_ls = 0; % flag to force a line search every time

% opts.ismimo = 0; % compute dFdx of a multi-output function, not just objective value

% opts.gradmemory = 0; % remember previous gradients

% opts.doparallel = 0; % compute gradients using parfor

% opts.fsd = 1; % use fixed step for derivative computation

% opts.allow_worsen = 0; % allow objective to get worse sometimes

% opts.doimagesc = 0; % if data/model outputs are matrix (not vector), plot as such

% opts.EnforcePriorProb = 0;% force the parameter updates to strictly adhere to prior distribution

% opts.FS = [] % feat selection function: FS(y)

% opts.userplotfun = []; % inject a user plot function into the main display

% opts.corrweight = 1; % weight error term by correlation

% opts.WeightByProbability = 0; % weight parameter step update by prob

% opts.faster = 0; % make faster by limiting loops

% opts.factorise_gradients = 1; % factorise gradients

% opts.sample_mvn = 0; % when stuck sample from a multivariate gauss

% opts.do_gpr = 0; % try to refine estimates using a Gauss process

% opts.normalise_gradients=0;% normalise gradients to a pdf

% opts.isGaussNewton = 0; % explicitly make the update a Gauss-Newton

% opts.do_poly=0; % same as do_gpr but using polynomials

% opts.steps_choice = []; % use multiple step methods and pick best on-line

% opts.hypertune = 1; % tune a hyperparameter using exponential cost

% opts.memory_optimise = 1; % switch on memory (recommend)

% opts.rungekutta = 1; % RK line search (recommend)

% opts.updateQ = 1; % update precision on each iteration(recommend)

% opts.DoMAP = 0; % maximum a posteriori

% opts.DoMAP_Bayes = 0; % Bayesian MAP (recommend, make sure mimo=1)

% opts.crit = [0 0 0 0 0 0 0 0];

% opts.save_constant = 0; % save .mat on each iteration

% opts.nocheck = 0; % skip some checks to make faster

% opts.isQR = 0; % use QR decomp to solve for dx

% opts.NatGrad = 0; % work in natural gradients

%

% [X,F,Cp,PP,Hist] = AO(opts); % call the optimser, passing the options struct

%

% OUTPUTS:

%-------------------------------------------------------------------------

% X = posterior parameters

% F = fit value (depending on objective function specified)

% CP = parameter covariance

% Pp = posterior probabilites

% H = history

%

% *If the optimiser isn't working well, try making V smaller!

%

% References

%-------------------------------------------------------------------------

%

% "Computing the objective function in DCM" Stephan, Friston & Penny

% https://www.fil.ion.ucl.ac.uk/spm/doc/papers/stephan_DCM_ObjFcn_tr05.pdf

%

% "The free energy principal: a rough guide to the brain?" Friston

% https://www.fil.ion.ucl.ac.uk/~karl/The%20free-energy%20principle%20-%20a%20rough%20guide%20to%20the%20brain.pdf

%

% "Likelihood and Bayesian Inference And Computation" Gelman & Hill

% http://www.stat.columbia.edu/~gelman/arm/chap18.pdf

%

% For the nonlinear least squares MLE / Gauss Newton:

% https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm

%

% For an explanation of momentum in gradient methods:

% https://distill.pub/2017/momentum/

%

% Approximation of derivaives by finite difference methods:

% https://www.ljll.math.upmc.fr/frey/cours/UdC/ma691/ma691_ch6.pdf

%

% For an explanation of normalised gradients in gradient descent

% https://jermwatt.github.io/machine_learning_refined/notes/3_First_order_methods/3_9_Normalized.html

%

% AS2019/2020/2021

% alexandershaw4@gmail.com

% Print the description of steps and exit

%--------------------------------------------------------------------------

if nargin == 1 && strcmp(lower(funopts),'help')

PrintHelp(); return;

end

if nargin == 1 && strcmp(lower(funopts),'options');

X = DefOpts; return;

end

% Inputs & Defaults...

%--------------------------------------------------------------------------

if isstruct(funopts)

parseinputstruct(funopts);

else

fprintf('You have to supply a funopts input struct now...\nTry AO(''options'')\n');

return;

end

% Set up log if requested

persistent loc ;

if writelog

name = datestr(now); name(name==' ') = '_';

name = [char(fun) '_' name '.txt'];

loc = fopen(name,'w');

else

loc = 1;

end

% If a feature selection function was passed, append it to the user fun

if ~isempty(FS) && isa(FS,'function_handle')

params.FS = FS;

end

% check functions, inputs, options... note: many of these are set/returned

% by the subfunctions parseinputstruct and DefOpts()

%--------------------------------------------------------------------------

%aopt = []; % reset

aopt.x0x0 = x0;

aopt.order = order; % first or second order derivatives [-1,0,1,2]

aopt.fun = fun; % (objective?) function handle

aopt.yshape = y;

aopt.y = y(:); % truth / data to fit

aopt.pp = x0(:); % starting parameters

aopt.Q = Q; % precision matrix

aopt.history = []; % error history when y=e & arg min y = f(x)

aopt.memory = gradmemory;% incorporate previous gradients when recomputing

aopt.fixedstepderiv = fsd;% fixed or adjusted step for derivative calculation

aopt.ObjectiveMethod = objective; % 'sse' 'fe' 'mse' 'rmse' (def sse)

aopt.hyperparameters = hyperparams;

aopt.forcels = force_ls; % force line search

aopt.mimo = ismimo; % derivatives w.r.t multiple output fun

aopt.parallel = doparallel; % compute dpdy in parfor

aopt.doimagesc = doimagesc; % change plot to a surface

aopt.rankappropriate = rankappropriate; % ensure facorised rank aprop cov

aopt.do_ssa = ssa;

aopt.corrweight = corrweight;

aopt.factorise_gradients = factorise_gradients;

aopt.hypertune = hypertune;

BayesAdjust = mleselect; % Select params to update based in probability

IncMomentum = im; % Observe and use momentum data

givetol = allow_worsen; % Allow bad updates within a tolerance

EnforcePriorProb = EnforcePriorProb; % Force updates to comply with prior distribution

WeightByProbability = WeightByProbability; % weight parameter updates by probability

ExLineSearch = ext_linesearch;

params.aopt = aopt; % Need to move form global aopt to a structure

params.userplotfun = userplotfun;

if save_constant

name = ['optim_' date];

end

% parameter and step / distribution vectors

x0 = full(x0(:));

XX0 = x0;

V = full(V(:));

v = V;

pC = diag(V);

%crit = [0 0 0 0 0 0 0 0];

% variance (in reduced space)

%--------------------------------------------------------------------------

V = eye(length(x0)); %turn off svd

pC = V'*(pC)*V;

ipC = spm_inv(spm_cat(spm_diag({pC})));

red = (diag(pC));

aopt.updateh = true; % update hyperpriors

aopt.pC = red; % store for derivative & objective function access

aopt.ipC = ipC; % store ^

red_x0 = red;

% initial probs

aopt.pt = zeros(length(x0),1) + (1/length(x0));

params.aopt = aopt;

% initial objective value (approx at this point as missing covariance data)

[e0] = obj(x0,params);

n = 0;

iterate = true;

Vb = V;

% initial error plot(s)

%--------------------------------------------------------------------------

if doplot

f = setfig(); params = makeplot(x0,x0,params); aopt.oerror = params.aopt.oerror;

pl_init(x0,params)

end

% initialise counters

%--------------------------------------------------------------------------

n_reject_consec = 0;

search = 0;

% Initial probability threshold for inclusion (i.e. update) of a parameter

Initial_JPDtol = 1e-10;

JPDtol = Initial_JPDtol;

etol = 0;

% parameters (in reduced space)

%--------------------------------------------------------------------------

np = size(V,2);

p = [V'*x0];

ip = (1:np)';

Ep = V*p(ip);

dff = []; % tracks changes in error over iterations

localminflag = 0; % triggers when stuck in local minima

% print options before we start printing updates

if DoMLE; fprintf('Using GaussNewton/MLE option\n'); end

if BayesAdjust; fprintf('Using Probability-Based Parameter Selection option\n'); end

if IncMomentum; fprintf('Using Momentum option\n'); end

fprintf('Using step-method: %d\n',step_method);

fprintf('Using Jaco (gradient) option: %d\n',order);

fprintf('User fun has %d varying parameters\n',length(find(red)));

% print start point - to console or logbook (loc)

refdate(loc);pupdate(loc,n,0,e0,e0,'start: ');

if step_method == 0

% step method can switch between 1 (big) and 3 (small) automatcically

autostep = 1;

else autostep = 0;

end

all_dx = [];

all_ex = [];

Hist.e = [];

%etol = 0;

% start optimisation loop

%==========================================================================

while iterate

% counter

%----------------------------------------------------------------------

n = n + 1; tic;

% Save each step if requested

if save_constant

save(name,'x0','Hist');

end

if WeightByProbability

aopt.pp = x0(:);

end

% update Gaussian precision hyperparameters matrix

% integrate error-weighted Q over iterations: Q(t+1) = dt * Q(t) + dQ/de

%----------------------------------------------------------------------

if ~isempty(Q) && updateQ

fprintf('| Updating Q...\n');

[~,~,erx,g0] = obj( V*x0(ip),params );

% extract residuals & convert to Gaussian mixture [gp]

RSE = erx(1:length(Q));%rescale(real(erx(1:length(Q))));

gerr = AGenQ(RSE);

% fit gauss residuals to data using lsq

b=AGenQ(RSE)\RSE-g0(1:length(Q));

aopt.Q = Q + diag(b'*gerr);

%aopt.Q = (Q + AGenQ(RSE) );

Hist.QQ(n,:) = rescale(real(erx(1:length(Q))));

%s=subplot(5,3,13);plot(real(diag(aopt.Q)),'color',[1 .7 .7],'linewidth',3);

s=subplot(5,3,12);imagesc(real(Q));

title('Gaussian Hyperparameter','color','w','fontsize',18);

ax = gca;

ax.XGrid = 'off';ax.YGrid = 'on';

s.YColor = [1 1 1];s.XColor = [1 1 1];s.Color = [.3 .3 .3];

drawnow;

end

% compute gradients & search directions

%----------------------------------------------------------------------

aopt.updatej = true; aopt.updateh = true; params.aopt = aopt;

pupdate(loc,n,0,e0,e0,'gradnts',toc);

% Second order partial derivates of F w.r.t x0 using Jaco.m

[e0,df0,~,~,~,~,params] = obj( V*x0(ip),params );

[e0,~,er] = obj( V*x0(ip),params );

df0 = real(df0);

if normalise_gradients

df0 = df0./sum(df0);

end

% catch instabilities in the gradient - ie explosive parameters

df0(isinf(df0)) = 0;

% Update aopt structure and place in params

aopt = params.aopt;

aopt.er = er;

aopt.updateh = false;

params.aopt = aopt;

% print end of gradient computation (just so we know it's finished)

pupdate(loc,n,0,e0,e0,'grd-fin',toc);

% update hyperparameter tuning plot

if hypertune; plot_hyper(params.hyper_tau,[Hist.e e0]); end

% update h_opt plot

if hyperparams; plot_h_opt(params.h_opt); drawnow; end

% Switching for different methods for calculating 'a' / step size

if autostep; search_method = autostepswitch(n,e0,Hist);

else; search_method = step_method;

end

% initial search direction (steepest) and slope

%----------------------------------------------------------------------

% Compute step, a, in scheme: dx = x0 + a*-J

if n == 1

a = red*0;

end

% Setting step size to 6 invokes low-dimensional hyperparameter tuning

if search_method ~= 6

pupdate(loc,n,0,e0,e0,'stepsiz',toc);

else

pupdate(loc,n,0,e0,e0,'hyprprm',toc);

end

% Feature Scoring for MIMOs

%----------------------------------------------------------------------

if orthogradient && ismimo

fprintf('Orthogonalising Jacobian\n');

params.aopt.J = symmetric_orthogonalise(params.aopt.J);

end

% i.e. where J(np,nf) & nf > 1

if ismimo

pupdate(loc,n,0,e0,e0,'scoring',toc);

JJ = params.aopt.J;

Q0 = aopt.Q;

padQ = size(JJ,2) - length(Q0);

Q0(end+1:end+padQ,end+1:end+padQ)=mean(Q0(:))/10;

for i = 1:np

% information score

score(i) = JJ(i,:)*Q0*JJ(i,:)';

end

red = red + rescale(real(score(:)),.125,1)/8;

red = real(red);

red(isnan(red)) = 1e-3;

end

% Select step size method

%----------------------------------------------------------------------

if ~ismimo

[a,J,nJ,L,D] = compute_step(df0,red,e0,search_method,params,x0,a,df0);

else

[a,J,nJ,L,D] = compute_step(params.aopt.J,red,e0,search_method,params,x0,a,df0);

J = -df0(:);

end

if simplelinesearch

pupdate(loc,n,0,e0,e0,'linesrc',toc);

fls = @(r) obj(x0 - (red.*r).*df0,params);

a = hypertuner(fls,ones(size(red))/2);

end

if search_method ~= 6

pupdate(loc,n,0,e0,e0,'stp-fin',toc);

else

pupdate(loc,n,0,e0,e0,'hyp-fin',toc);

end

% Log start of iteration (these are returned)

Hist.e(n) = e0;

Hist.p{n} = x0;

Hist.J{n} = df0;

Hist.a{n} = a;

if ismimo

Hist.Jfull{n} = aopt.J;

end

% Make copies of error and param set for inner while loops

x1 = x0;

e1 = e0;

% Start counters

improve = true;

nfun = 0;

% check norm of gradients (def gradtol = 1e-4)

if norm(J) < gradtol

fprintf('Gradient step below tolerance (%d)\n',norm(J));

[X,F,Cp,PP] = finishup(V,x0,ip,e0,doparallel,params,J,Ep,red,writelog,loc,aopt);

return;

end

% Update expectation on variance (2nd moments) using Jacobian

%----------------------------------------------------------------------

Hist.red(n,:) = red;

if variance_estimation

if n > 1

% this is also a prediction of the step size for the next iter

ored = red;

for l = 1:np

PR(l) = normcdf(mean(Hist.Jfull{end}(l,:)),mean(Hist.Jfull{end-1}(l,:)),std(Hist.Jfull{end-1}(l,:)));

end

red = red_x0 + (red_x0(:) .* PR(:));

% update saved inverse

%params.aopt.ipC = spm_inv(diag(red));

% aopt.red = red;

% update plot

s = subplot(5,3,14);

b = bar([red-ored]);

%b(1).FaceColor = [1 1 1];

b(1).FaceColor = [1 .7 .7];

title('Step Change','color','w','fontsize',18);

ax = gca;

ax.XGrid = 'off';

ax.YGrid = 'on';

s.YColor = [1 1 1];

s.XColor = [1 1 1];

s.Color = [.3 .3 .3];

drawnow;

end

end

% iterative descent on this (-gradient) trajectory

%======================================================================

while improve

% Log number of function calls on this iteration

nfun = nfun + 1;

% Compute The Parameter Step (from gradients and step sizes):

% % x[p,t+1] = x[p,t] + a[p]*-dfdx[p]

%------------------------------------------------------------------

% dx ~ x1 + ( a * J );

dx = compute_dx(x1,a,J,red,search_method,params);

if isGaussNewton && ismimo

% [note this is now just a newton method, not gn!]

%

% note for this full Newton method you need both Hess and Grad

% from the jaco_par.m function because the step size is

% determined by the inverse of the inner prouect of the hessian

pupdate(loc,n,nfun,e1,e1,'Newton ',toc);

% by using the variance (red) as a lambda on the inverse

% Hessian, this becomes a relaxed or 'damped' Newton scheme

H = [aopt.J];

H = (red.*H);%./norm(H);

%Hstep = pinv(H*H')*cat(1,aopt.Jo{:,1});

% the non-parallel finite different functions return gradients

% in reduced space - embed in full vector space

Jo = cat(1,aopt.Jo{:,1});

JJ = x0*0;

JJ(find(diag(pC))) = Jo;

% use cholesky factorisation of H (ensuring posdef)

R = chol( abs(makeposdef(H*(1*eye(length(H)))*H'))+ 1e-3*eye(length(x0)) );

Hstep = ( -R \ (R'*JJ));

Gdx = x1 - Hstep;

if obj(Gdx,params) < obj(dx,params) && ~docompare

dx = Gdx;

fprintf('Selected Newton Step\n');

end

end

if isGaussNewtonReg && ismimo

% [note this is now just a newton method, not gn!]

%

% note for this full Newton method you need both Hess and Grad

% from the jaco_par.m function because the step size is

% determined by the inverse of the inner prouect of the hessian

pupdate(loc,n,nfun,e1,e1,'Newton ',toc);

% by using the variance (red) as a lambda on the inverse

% Hessian, this becomes a relaxed or 'damped' Newton scheme

for i = 1:size(J,1);

for j = 1:size(J,1);

H(i,j) = spm_trace(aopt.J(i,:),aopt.J(j,:));

end

end

%H = [aopt.J];

H = (red.*H./norm(H));%./norm(H);

% the non-parallel finite different functions return gradients

% in reduced space - embed in full vector space

Jo = cat(1,aopt.Jo{:,1});

if length(Jo) ~= length(x1)

JJ = x0*0;

JJ(find(diag(pC))) = Jo;

else

JJ = Jo;

end

% essentially here we are tiuning this part of the Newton

% scheme:

% ______

% xhat = x - inv(H*L*H')*J

% tunable regularisation function

Gf = @(L) pinv(H*(L*eye(length(H)))*H');

Gff = @(x) obj(x1 - Gf(x)*JJ,params);

[XX] = fminsearch(Gff,1);

Hstep = pinv(H*(XX*eye(length(H)))*H')*JJ;

GRdx = x1 - Hstep;

%dx = GRdx;

% only take a Newton step if its better than the vanilla

% gradient step

if obj(GRdx,params) < obj(dx,params) && ~docompare

dx = GRdx;

fprintf('Selected Regularised Newton Step\n');

end

if forcenewton

dx = GRdx;

fprintf('Forced Newton Step\n');

end

end

if NatGrad

% retrieve derivatives

if ~ismimo; j = J(:)*er';

else; j = aopt.J;

end

lambda = 1/8;

D = size(j,1);

% compute natural gradient

fim = inv(lambda*eye(D)+j*j');

dx = x1 - sum(fim*j,2);

end

if isQR && ismimo

pupdate(loc,n,nfun,e1,e1,'QRmodel',toc);

% Use QR factorisation to estimate dx from jacobian and

% residual - i.e. under assumption local-linearity

% (ensure ismimo = 1)

% note this is the QR equivalent of the normal equations for a

% linear least squares problem

[q,r] = qr(params.aopt.J);

dx = q\((r./norm(r))*er);

end

% The following options are like 'E-steps' or line search options

% - i.e. they estimate the missing variables (parameter indices &

% values) that should be optimised in this iteration

%==================================================================

% Compute the probabilities of each (predicted) new parameter

% coming from the same distribution defined by the prior (last best)

dx = real(dx);

x1 = real(x1);

red = real(red);

% [Prior] distributions

pt = zeros(1,length(x1));

for i = 1:length(x1)

%vv = real(sqrt( red(i) ));

vv = real(sqrt( red(i) ))*2;

if vv <= 0 || isnan(vv) || isinf(vv); vv = 1/64; end

pd(i) = makedist('normal','mu', real(aopt.pp(i)),'sigma', vv);

end

% Curb parameter estimates trying to exceed their distirbution bounds

if EnforcePriorProb

odx = dx;

nst = 1;

for i = 1:length(x1)

if red(i)

if dx(i) < ( pd(i).mu - (nst*pd(i).sigma) )

dx(i) = pd(i).mu - (nst*pd(i).sigma);

elseif dx(i) > ( pd(i).mu + (nst*pd(i).sigma) )

dx(i) = pd(i).mu + (nst*pd(i).sigma);

end

end

end

end

% Compute relative change

pdx = pt*0;

for i = 1:length(x1)

if red(i)

%vv = real(sqrt( red(i) ));

vv = real(sqrt( red(i) ))*2;

if vv <= 0 || isnan(vv) || isinf(vv); vv = 1/64; end

pd(i) = makedist('normal','mu', real(aopt.pp(i)),'sigma', vv);

pdx(i) = normcdf(dx(i),pd(i).mu,pd(i).sigma);

%pdx(i) = (1./(1+exp(-pdf(pd(i),dx(i))))) ./ (1./(1+exp(-pdf(pd(i),aopt.pp(i)))));

else

end

end

pt = pdx;

prplot(pt);

aopt.pt = [aopt.pt pt(:)];

% If WeightByProbability is set, use p(dx) as a weight on dx

% iteratively until n% of p(dx[i]) are > threshold

% -------------------------------------------------------------

if WeightByProbability

dx = x1 + ( pt(:).*(dx-x1) );

pupdate(loc,n,1,e1,e1,'OptP(p)',toc);

optimise = true;

num_optloop = 0;

while optimise

pdx = pt*0;

num_optloop = num_optloop + 1;

for i = 1:length(x1)

if red(i)

vv = real(sqrt( red(i) ))*2;

if vv <= 0 || isnan(vv) || isinf(vv); vv = 1/64; end

pd(i) = makedist('normal','mu', real(aopt.pp(i)),'sigma', vv);

pdx(i) = normcdf(dx(i),pd(i).mu,pd(i).sigma);

%pdx(i) = (1./(1+exp(-pdf(pd(i),dx(i))))) ./ (1./(1+exp(-pdf(pd(i),aopt.pp(i)))));

else

end

end

% integrate (update) dx

dx = x1 + ( pt(:).*(dx-x1) );

% convergence

if length(find(pdx(~~red) > 0.8))./length(pdx(~~red)) > 0.7 || num_optloop > 2000

optimise = false;

end

end

end

% Save for computing gradient ascent on probabilities

p_hist(n,:) = pt;

Hist.pt(:,n) = pt;

% Update plot: probabilities

[~,oo] = sort(pt(:),'descend');

%probplot(cumprod(pt(oo)),0.95,oo);

% This is a variation on the Gauss-Newton algorithm - compute

% MLE via WLS - where the weights are the probabilities

%------------------------------------------------------------------

if DoMLE

iter_mle = true;

nmfun = 0;

clear b

while iter_mle

pupdate(loc,n,nmfun,e1,e1,'MLE/WLS',toc);

nmfun = nmfun + 1;

nfun = nfun + 1;

% parameter derivatives

if ~ismimo; j = J(:)*er';

else; j = aopt.J;

end

% weights and residuals

w = pt;

r0 = spm_vec(y) - spm_vec(params.aopt.fun(spm_unvec(x1,aopt.x0x0)));

if ~isempty(FS)

r0 = FS(r0);

end

db = ( pinv(j'*diag(w)*j)*j'*diag(w) )'*r0;

% b(s+1) = b(s) - (J'*J)^-1*J'*r(s)

try b = b - db;

catch; b = db;

end

% inclusion of a weight essentially makes this a Marquardt regularisation parameter

if isvector(a)

dxd = x1 - a.*b;

else

dxd = x1 - a*b; % recompose dx including step matrix (a)

end

% update x

if obj(dxd,params) < obj(x1,params) && (nmfun < 3)

x1 = dxd;

x0 = x1;

e1 = obj(x1,params);

e0 = e1;

else

iter_mle = false;

dx = dxd;

end

end

end

if DoMAP

% Do MAP estimation of parameter values with lambda as a

% hyperparameter

pupdate(loc,n,n,e1,e1,'MAP est',toc);

% retrieve derivatives

if ~ismimo; j = J(:)*er';

else; j = aopt.J;

end

% remove extra stuff in Jacobian added by feature selection

j = j(:,1:size(y,1));

lambda = 1/8;

D = size(j,1);

% try to force a wanring suppression within anonym func call

warning('off','MATLAB:singularMatrix')

wn = @() warning('off','MATLAB:nearlysingularMatrix');

wwn = @() suppress(wn);

warning('off','MATLAB:curvefit:fit:noStartPoint');

map = @(lambda) [feval(wwn) (lambda*eye(D)+j*j')\(j*y)];

gmap = @(lambda) obj(x1 - red.*map(lambda),params);

% optimise - find lambda

ddx = fminsearch(gmap,1/8);

% recover dx

dx = x1 - red.*map(ddx);

end

if DoMAP_Bayes

% Fully Bayesian MAP estimation of parameter values with

% lambda as an optimisable hyperparameter

% -- also provides an updated expectation for the variance

pupdate(loc,n,nfun,e1,e1,'bMAPest',toc);

% retrieve derivatives

if ~ismimo; j = J(:)*er';

else; j = aopt.J;

end

% remove extra stuff in Jacobian added by feature selection

j = j(:,1:size(y,1));

% largest eigenvalue normalisation

[ev,ei] = eig(j*j');

j = j ./ max(diag(ei));

lambda = 1/8;

D = size(j,1);

% inject warning silence to anon function 'map'

warning('off','MATLAB:singularMatrix')

wn = @() warning('off','MATLAB:nearlysingularMatrix');

wwn = @() suppress(wn);

warning('off','MATLAB:curvefit:fit:noStartPoint');

% compute residual and weight

r0 = spm_vec(y) - spm_vec(params.aopt.fun(spm_unvec(x1,aopt.x0x0)));

if length(aopt.iS) == length(r0)

r0 = r0.*aopt.iS.*r0';

r0 = r0(1:size(j,2),1:size(j,2));

else

r0 = r0.*aopt.iS(1:length(r0),1:length(r0)).*r0';

end

r0 = diag(r0);

% MAP projection using Gauss-Newton / Regularised Levenberg-marquardt

map = @(lambda) [feval(wwn) (lambda*eye(D)+j*j')\(j*r0)];

gmap = @(lambda) obj(x1 - red.*map(lambda),params);

% optimise - find lambda

ddx = fminsearch(gmap,1/8);

% recover dx

Bdx = x1 - red.*map(ddx);

if obj(Bdx,params) < obj(dx,params) && ~docompare

dx = Bdx;

end

% recover (co)variance

%if variance_estimation

% cv = inv((ddx/2)^(-2)*(j*j')+(ddx/2)^(-2)*eye(D));

% %cv = cv ./ norm(cv);

% red = 2*sqrt(diag(cv));

% aopt.pC = red;

% params.aopt = aopt;

%end

%dx = x1 - cv*map(ddx);

%dx = x1 - ((lambda*eye(D)+pinv(cv))+j*j')\(j*r0);

% note.

% inv(lambda*eye(D)+j*j') is the Fisher-information matrix

end

if docompare

pupdate(loc,n,nfun,e1,e1,'compare',toc);

OP = [dx(:) Gdx(:) Bdx(:) GRdx(:)];

K = [1 1 1 1]/4;

if params.aopt.parallel

options = optimset('Display','off','UseParallel',true);

else

options = optimset('Display','off');