int ekf_step(void * v, double * z)
{
/* unpack incoming structure */
int * ptr = (int *)v;
int n = *ptr;
ptr++;
int m = *ptr;
ptr++;
int s = *ptr;
ekf_t ekf;
unpack(v, &ekf, n, m, s);
// NON-ADDITIVE
/* P_k = F_{k-1} P_{k-1} F^T_{k-1} + L_{k-1} Y_{k-1} L^T_{k-1} + Q_{k-1} */
mulmat(ekf.F, ekf.P, ekf.tmp0, n, n, n);
transpose(ekf.F, ekf.Ft, n, n);
mulmat(ekf.tmp0, ekf.Ft, ekf.Pp, n, n, n);
mulmat(ekf.L, ekf.Y, ekf.tmp6, n, s, s);
transpose(ekf.L, ekf.Lt, n, s);
mulmat(ekf.tmp6, ekf.Lt, ekf.tmp0, n, s, n);
accum(ekf.Pp, ekf.tmp0, n, n);
accum(ekf.Pp, ekf.Q, n, n);
/* G_k = P_k H^T_k (H_k P_k H^T_k + R)^{-1} */
transpose(ekf.H, ekf.Ht, m, n);
mulmat(ekf.Pp, ekf.Ht, ekf.tmp1, n, n, m);
mulmat(ekf.H, ekf.Pp, ekf.tmp2, m, n, n);
mulmat(ekf.tmp2, ekf.Ht, ekf.tmp3, m, n, m);
accum(ekf.tmp3, ekf.R, m, m);
if (cholsl(ekf.tmp3, ekf.tmp4, ekf.tmp5, m)) return 1;
mulmat(ekf.tmp1, ekf.tmp4, ekf.G, n, m, m);
/* \hat{x}_k = \hat{x_k} + G_k(z_k - h(\hat{x}_k)) */
sub(z, ekf.hx, ekf.tmp5, m);
mulvec(ekf.G, ekf.tmp5, ekf.tmp2, n, m);
add(ekf.fx, ekf.tmp2, ekf.x, n);
/* P_k = (I - G_k H_k) P_k */
mulmat(ekf.G, ekf.H, ekf.tmp0, n, m, n);
negate(ekf.tmp0, n, n);
mat_addeye(ekf.tmp0, n);
mulmat(ekf.tmp0, ekf.Pp, ekf.P, n, n, n);
/* success */
return 0;
}
int peidefunct(int nrow, int ncol, real_t par[], real_t res[],
int n, int m, int nobs, int *nbp, int first, int *sec,
int *max, int *nis, real_t eps1, int weight, int bp[],
real_t save[], real_t ymax[], real_t y[], real_t **yp,
real_t **fy, real_t **fp, int cobs[], real_t tobs[],
real_t obs[], real_t in[], real_t aux[], int clean,
int (*deriv)(int,int,real_t [],real_t [],real_t,real_t []),
int (*jacdfdy)(int,int,real_t [],real_t [],real_t,real_t **),
int (*jacdfdp)(int,int,real_t [],real_t [],real_t,real_t **),
void (*callystart)(int,int,real_t [],real_t [],real_t[]),
void (*monitor)(int,int,int,real_t [],real_t [],int,int))
{
/* this function is internally used by PEIDE */
void peidereset(int, int, real_t, real_t, real_t, real_t, real_t [],
real_t [], real_t *, real_t *, real_t *, int *);
void peideorder(int, int, real_t, real_t [], real_t [],
real_t *, real_t *, real_t *, real_t *, real_t *, int *);
void peidestep(int, int, int, real_t, real_t, real_t, real_t,
real_t [], real_t [], real_t [], real_t [], int *, real_t *);
real_t peideinterpol(int, int, int, real_t, real_t []);
int l,k,knew,fails,same,kpold,n6,nnpar,j5n,cobsii,*p,evaluate,
evaluated,decompose,conv,extra,npar,i,j,jj,ii;
real_t xold,hold,a0,tolup,tol,toldwn,tolconv,h,ch,chnew,error,
dfi,tobsdif,a[6],*delta,*lastdelta,*df,*y0,**jacob,xend,
hmax,hmin,eps,s,aa,x,t,c;
p=allocate_integer_vector(1,n);
delta=allocate_real_vector(1,n);
lastdelta=allocate_real_vector(1,n);
df=allocate_real_vector(1,n);
y0=allocate_real_vector(1,n);
jacob=allocate_real_matrix(1,n,1,n);
if (*sec) {
*sec=0;
goto Finish;
}
xend=tobs[nobs];
eps=in[2];
npar=m;
extra=(*nis)=0;
ii=1;
jj = (*nbp == 0) ? 0 : 1;
n6=n*6;
inivec(-3,-1,save,0.0);
inivec(n6+1,(6+m)*n,y,0.0);
inimat(1,nobs+(*nbp),1,m+(*nbp),yp,0.0);
t=tobs[1];
x=tobs[0];
(*callystart)(n,m,par,y,ymax);
hmax=tobs[1]-tobs[0];
hmin=hmax*in[1];
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
nnpar=n*npar;
Newstart:
k=1;
kpold=0;
same=2;
peideorder(n,k,eps,a,save,&tol,&tolup,&toldwn,&tolconv,
&a0,&decompose);
if (!(*deriv)(n,m,par,y,x,df)) {
save[-3]=3.0;
goto Finish;
}
s=FLT_MIN;
for (i=1; i<=n; i++) {
aa=matvec(1,n,i,fy,df)/ymax[i];
s += aa*aa;
}
h=sqrt(2.0*eps/sqrt(s));
if (h > hmax)
h=hmax;
else
if (h < hmin) h=hmin;
xold=x;
hold=h;
ch=1.0;
for (i=1; i<=n; i++) {
save[i]=y[i];
save[n+i]=y[n+i]=df[i]*h;
}
fails=0;
while (x < xend) {
if (x+h <= xend)
x += h;
else {
h=xend-x;
x=xend;
ch=h/hold;
c=1.0;
for (j=n; j<=k*n; j += n) {
c *= ch;
for (i=j+1; i<=j+n; i++) y[i] *= c;
}
same = (same < 3) ? 3 : same+1;
}
/* prediction */
for (l=1; l<=n; l++) {
for (i=l; i<=(k-1)*n+l; i += n)
for (j=(k-1)*n+l; j>=i; j -= n) y[j] += y[j+n];
delta[l]=0.0;
}
evaluated=0;
/* correction and estimation local error */
for (l=1; l<=3; l++) {
if (!(*deriv)(n,m,par,y,x,df)) {
save[-3]=3;
goto Finish;
}
for (i=1; i<=n; i++) df[i]=df[i]*h-y[n+i];
if (evaluate) {
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
}
if (decompose) {
/* decompose jacobian */
decompose=0;
c = -a0*h;
for (j=1; j<=n; j++) {
for (i=1; i<=n; i++) jacob[i][j]=fy[i][j]*c;
jacob[j][j] += 1.0;
}
dec(jacob,n,aux,p);
}
sol(jacob,n,p,df);
conv=1;
for (i=1; i<=n; i++) {
dfi=df[i];
y[i] += a0*dfi;
y[n+i] += dfi;
delta[i] += dfi;
conv=(conv && (fabs(dfi) < tolconv*ymax[i]));
}
if (conv) {
s=FLT_MIN;
for (i=1; i<=n; i++) {
aa=delta[i]/ymax[i];
s += aa*aa;
}
error=s;
break;
}
}
/* acceptance or rejection */
if (!conv) {
if (!evaluated)
evaluate=1;
else {
ch /= 4.0;
if (h < 4.0*hmin) {
save[-1] += 10.0;
hmin /= 10.0;
if (save[-1] > 40.0) goto Finish;
}
}
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
} else if (error > tol) {
fails++;
if (h > 1.1*hmin) {
if (fails > 2) {
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
goto Newstart;
} else {
/* calculate step and order */
peidestep(n,k,fails,tolup,toldwn,tol,error,delta,
lastdelta,y,ymax,&knew,&chnew);
if (knew != k) {
k=knew;
peideorder(n,k,eps,a,save,&tol,&tolup,
&toldwn,&tolconv,&a0,&decompose);
}
ch *= chnew;
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
}
} else {
if (k == 1) {
/* violate eps criterion */
save[-2] += 1.0;
same=4;
goto Errortestok;
}
k=1;
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
peideorder(n,k,eps,a,save,&tol,&tolup,
&toldwn,&tolconv,&a0,&decompose);
same=2;
}
} else {
Errortestok:
fails=0;
for (i=1; i<=n; i++) {
c=delta[i];
for (l=2; l<=k; l++) y[l*n+i] += a[l]*c;
if (fabs(y[i]) > ymax[i]) ymax[i]=fabs(y[i]);
}
same--;
if (same == 1)
dupvec(1,n,0,lastdelta,delta);
else if (same == 0) {
/* calculate step and order */
peidestep(n,k,fails,tolup,toldwn,tol,error,delta,
lastdelta,y,ymax,&knew,&chnew);
if (chnew > 1.1) {
if (k != knew) {
if (knew > k)
mulvec(knew*n+1,knew*n+n,-knew*n,y,delta,
a[k]/knew);
k=knew;
peideorder(n,k,eps,a,save,&tol,&tolup,
&toldwn,&tolconv,&a0,&decompose);
}
same=k+1;
if (chnew*h > hmax) chnew=hmax/h;
h *= chnew;
c=1.0;
for (j=n; j<=k*n; j += n) {
c *= chnew;
mulvec(j+1,j+n,0,y,y,c);
}
decompose=1;
} else
same=10;
}
(*nis)++;
/* start of an integration step of yp */
if (clean) {
hold=h;
xold=x;
kpold=k;
ch=1.0;
dupvec(1,k*n+n,0,save,y);
} else {
if (h != hold) {
ch=h/hold;
c=1.0;
for (j=n6+nnpar; j<=kpold*nnpar+n6; j += nnpar) {
c *= ch;
for (i=j+1; i<=j+nnpar; i++) y[i] *= c;
}
hold=h;
}
if (k > kpold)
inivec(n6+k*nnpar+1,n6+k*nnpar+nnpar,y,0.0);
xold=x;
kpold=k;
ch=1.0;
dupvec(1,k*n+n,0,save,y);
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
/* decompose jacobian */
decompose=0;
c = -a0*h;
for (j=1; j<=n; j++) {
for (i=1; i<=n; i++) jacob[i][j]=fy[i][j]*c;
jacob[j][j] += 1.0;
}
dec(jacob,n,aux,p);
if (!(*jacdfdp)(n,m,par,y,x,fp)) {
save[-3]=5.0;
goto Finish;
}
if (npar > m) inimat(1,n,m+1,npar,fp,0.0);
/* prediction */
for (l=0; l<=k-1; l++)
for (j=k-1; j>=l; j--)
elmvec(j*nnpar+n6+1,j*nnpar+n6+nnpar,nnpar,
y,y,1.0);
/* correction */
for (j=1; j<=npar; j++) {
j5n=(j+5)*n;
dupvec(1,n,j5n,y0,y);
for (i=1; i<=n; i++)
df[i]=h*(fp[i][j]+matvec(1,n,i,fy,y0))-
y[nnpar+j5n+i];
sol(jacob,n,p,df);
for (l=0; l<=k; l++) {
i=l*nnpar+j5n;
elmvec(i+1,i+n,-i,y,df,a[l]);
}
}
}
while (x >= t) {
/* calculate a row of the jacobian matrix and an
element of the residual vector */
tobsdif=(tobs[ii]-x)/h;
cobsii=cobs[ii];
res[ii]=peideinterpol(cobsii,n,k,tobsdif,y)-obs[ii];
if (!clean) {
for (i=1; i<=npar; i++)
yp[ii][i]=peideinterpol(cobsii+(i+5)*n,nnpar,k,
tobsdif,y);
/* introducing break-points */
if (bp[jj] != ii) {
} else if (first && fabs(res[ii]) < eps1) {
(*nbp)--;
for (i=jj; i<=(*nbp); i++) bp[i]=bp[i+1];
bp[*nbp+1]=0;
} else {
extra++;
if (first) par[m+jj]=obs[ii];
/* introducing a jacobian row and a residual
vector element for continuity requirements */
yp[nobs+jj][m+jj] = -weight;
mulrow(1,npar,nobs+jj,ii,yp,yp,weight);
res[nobs+jj]=weight*(res[ii]+obs[ii]-par[m+jj]);
}
}
if (ii == nobs)
goto Finish;
else {
t=tobs[ii+1];
if (bp[jj] == ii && jj < *nbp) jj++;
hmax=t-tobs[ii];
hmin=hmax*in[1];
ii++;
}
}
/* break-points introduce new initial values for y & yp */
if (extra > 0) {
for (i=1; i<=n; i++) {
y[i]=peideinterpol(i,n,k,tobsdif,y);
for (j=1; j<=npar; j++)
y[i+(j+5)*n]=peideinterpol(i+(j+5)*n,nnpar,
k,tobsdif,y);
}
for (l=1; l<=extra; l++) {
cobsii=cobs[bp[npar-m+l]];
y[cobsii]=par[npar+l];
for (i=1; i<=npar+extra; i++) y[cobsii+(5+i)*n]=0.0;
inivec(1+nnpar+(l+5)*n,nnpar+(l+6)*n,y,0.0);
y[cobsii+(5+npar+l)*n]=1.0;
}
npar += extra;
extra=0;
x=tobs[ii-1];
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
nnpar=n*npar;
goto Newstart;
}
}
}
Finish:
if (save[-2] > *max) *max=save[-2];
if (!first) (*monitor)(1,ncol,nrow,par,res,weight,*nis);
free_integer_vector(p,1);
free_real_vector(delta,1);
free_real_vector(lastdelta,1);
free_real_vector(df,1);
free_real_vector(y0,1);
free_real_matrix(jacob,1,n,1);
return (save[-1] <= 40.0 && save[-3] == 0.0);
}
void ark(real_t *t, real_t *te, int *m0, int *m, real_t u[],
void (*derivative)(int *, int *, real_t *, real_t[]),
real_t data[],
void (*out)(int *, int *, real_t *, real_t *, real_t [],
real_t []))
{
real_t *allocate_real_vector(int, int);
real_t **allocate_real_matrix(int, int, int, int);
void free_real_vector(real_t *, int);
void free_real_matrix(real_t **, int, int, int);
void inivec(int, int, real_t [], real_t);
void mulvec(int, int, int, real_t [], real_t [], real_t);
void dupvec(int, int, int, real_t [], real_t []);
real_t vecvec(int, int, int, real_t [], real_t []);
void elmvec(int, int, int, real_t [], real_t [], real_t);
void decsol(real_t **, int, real_t [], real_t []);
real_t arkmui(int, int, int, real_t []);
real_t arklabda(int, int, int, int, real_t []);
static real_t th1[8] = {1.0, 0.5, 1.0/6.0, 1.0/3.0, 1.0/24.0,
1.0/12.0, 0.125, 0.25};
static real_t ec0,ec1,ec2,tau0,tau1,tau2,taus,t2;
int p,n,q,start,step1,last,i,j,k,l,n1,m00;
real_t thetanm1,tau,betan,qinv,eta,*mu,*lambda,*thetha,*ro,*r,
**alfa,th[9],aux[4],s,ss,theta0,tauacc,taustab,
aa,bb,cc,ec,mt,lt;
n=data[1];
m00=(*m0);
mu=allocate_real_vector(1,n);
lambda=allocate_real_vector(1,n);
thetha=allocate_real_vector(0,n);
ro=allocate_real_vector(m00,*m);
r=allocate_real_vector(m00,*m);
alfa=allocate_real_matrix(1,8,1,n+1);
p=data[2];
ec1=ec2=0.0;
betan=data[3];
thetanm1 = (p == 3) ? 0.75 : 1.0;
theta0=1.0-thetanm1;
s=1.0;
for (j=n-1; j>=1; j--) {
s = -s*theta0+data[n+10-j];
mu[j]=data[n+11-j]/s;
lambda[j]=mu[j]-theta0;
}
for (i=1; i<=8; i++)
for (j=0; j<=n; j++)
if (i == 1) alfa[i][j+1]=1.0;
else if (j == 0) alfa[i][j+1]=0.0;
else if (i == 2 || i == 4 || i == 8)
alfa[i][j+1]=pow(arkmui(j,n,p,lambda),(i+2)/3);
else if ((i == 3 || i == 6) && j > 1) {
s=0.0;
for (l=1; l<=j-1; l++)
s += arklabda(j,l,n,p,lambda)*
pow(arkmui(l,n,p,lambda),i/3);
alfa[i][j+1]=s;
}
else if (i == 5 && j > 2) {
s=0.0;
for (l=2; l<=j-1; l++) {
ss=0.0;
for (k=1; k<=l-1; k++)
ss += arklabda(l,k,n,p,lambda)*
arkmui(k,n,p,lambda);
s += arklabda(j,l,n,p,lambda)*ss;
}
alfa[i][j+1]=s;
}
else if (i == 7 && j > 1) {
s=0.0;
for (l=1; l<=j-1; l++)
s += arklabda(j,l,n,p,lambda)*arkmui(l,n,p,lambda);
alfa[i][j+1]=s*arkmui(j,n,p,lambda);
}
else alfa[i][j+1]=0.0;
n1 = ((n < 4) ? n+1 : ((n < 7) ? 4 : 8));
for (i=1; i<=8; i++) th[i]=th1[i-1];
if (p == 3 && n < 7) th[1]=th[2]=0.0;
aux[2]=FLT_EPSILON;
decsol(alfa,n1,aux,th);
inivec(0,n,thetha,0.0);
dupvec(0,n1-1,1,thetha,th);
if (!(p == 3 && n < 7)) {
thetha[0] -= theta0;
thetha[n-1] -= thetanm1;
q=p+1;
} else
q=3;
qinv=1.0/q;
start=(data[8] == 0.0);
data[10]=0.0;
last=0;
dupvec(*m0,*m,0,r,u);
(*derivative)(m0,m,t,r);
do {
/* stepsize */
eta=sqrt(vecvec(*m0,*m,0,u,u))*data[7]+data[6];
if (eta > 0.0) {
if (start) {
if (data[8] == 0) {
tauacc=data[5];
step1=1;
} else
if (step1) {
tauacc=pow(eta/ec2,qinv);
if (tauacc > 10.0*tau2)
tauacc=10.0*tau2;
else
step1=0;
} else {
bb=(ec2-ec1)/tau1;
cc = -bb*t2+ec2;
ec=bb*(*t)+cc;
tauacc = (ec < 0.0) ? tau2 : pow(eta/ec,qinv);
start=0;
}
} else {
aa=((ec0-ec1)/tau0+(ec2-ec1)/tau1)/(tau1+tau0);
bb=(ec2-ec1)/tau1-(2.0*t2-tau1)*aa;
cc = -(aa*t2+bb)*t2+ec2;
ec=(aa*(*t)+bb)*(*t)+cc;
tauacc = ((ec < 0.0) ? taus : pow(eta/ec,qinv));
if (tauacc > 2.0*taus) tauacc=2.0*taus;
if (tauacc < taus/2.0) tauacc=taus/2.0;
}
} else
tauacc=data[5];
if (tauacc < data[5]) tauacc=data[5];
taustab=betan/data[4];
if (taustab < data[5]) {
data[10]=1.0;
break;
}
tau = ((tauacc > taustab) ? taustab : tauacc);
taus=tau;
if (tau >= (*te)-(*t)) {
tau=(*te)-(*t);
last=1;
}
tau0=tau1;
tau1=tau2;
tau2=tau;
/* difference scheme */
mulvec(*m0,*m,0,ro,r,thetha[0]);
if (p == 3) elmvec(*m0,*m,0,u,r,0.25*tau);
for (i=1; i<=n-1; i++) {
mt=mu[i]*tau;
lt=lambda[i]*tau;
for (j=(*m0); j<=(*m); j++) r[j]=lt*r[j]+u[j];
s=(*t)+mt;
(*derivative)(m0,m,&s,r);
if (thetha[i] != 0.0) elmvec(*m0,*m,0,ro,r,thetha[i]);
if (i == n) {
data[9]=sqrt(vecvec(*m0,*m,0,ro,ro))*tau;
ec0=ec1;
ec1=ec2;
ec2=data[9]/pow(tau,q);
}
}
elmvec(*m0,*m,0,u,r,thetanm1*tau);
dupvec(*m0,*m,0,r,u);
s=(*t)+tau;
(*derivative)(m0,m,&s,r);
if (thetha[n] != 0.0) elmvec(*m0,*m,0,ro,r,thetha[n]);
data[9]=sqrt(vecvec(*m0,*m,0,ro,ro))*tau;
ec0=ec1;
ec1=ec2;
ec2=data[9]/pow(tau,q);
t2=(*t);
if (last) {
last=0;
(*t)=(*te);
} else
(*t) += tau;
data[8] += 1.0;
(*out)(m0,m,t,te,u,data);
} while ((*t) != (*te));
free_real_vector(mu,1);
free_real_vector(lambda,1);
free_real_vector(thetha,0);
free_real_vector(ro,m00);
free_real_vector(r,m00);
free_real_matrix(alfa,1,8,1);
}
real_t flemin(int n, real_t x[], real_t g[], real_t h[],
real_t (*funct)(int, real_t[], real_t[]),
real_t in[], real_t out[])
{
real_t *allocate_real_vector(int, int);
void free_real_vector(real_t *, int);
real_t vecvec(int, int, int, real_t [], real_t []);
void elmvec(int, int, int, real_t [], real_t [], real_t);
real_t symmatvec(int, int, int, real_t [], real_t []);
void inivec(int, int, real_t [], real_t);
void inisymd(int, int, int, real_t [], real_t);
void mulvec(int, int, int, real_t [], real_t [], real_t);
void dupvec(int, int, int, real_t [], real_t []);
void linemin(int, real_t [], real_t [], real_t, real_t *, real_t [],
real_t (*)(int, real_t[], real_t[]), real_t, real_t *,
real_t, real_t *, int *, int, real_t []);
void davupd(real_t [], int, real_t [], real_t [], real_t, real_t);
void fleupd(real_t [], int, real_t [], real_t [], real_t, real_t);
int i,it,cntl,evl,evlmax;
real_t f,f0,fmin,mu,dg,dg0,nrmdelta,alfa,reltol,abstol,eps,tolg,
aid,*v,*delta,*s;
v=allocate_real_vector(1,n);
delta=allocate_real_vector(1,n);
s=allocate_real_vector(1,n);
reltol=in[1];
abstol=in[2];
mu=in[3];
tolg=in[4];
fmin=in[5];
alfa=in[6];
evlmax=in[7];
out[4]=0.0;
it=0;
f=(*funct)(n,x,g);
evl=1;
cntl=0;
if (alfa > 0.0) {
inivec(1,(n*(n+1))/2,h,0.0);
inisymd(1,n,0,h,alfa);
}
for (i=1; i<=n; i++) delta[i] = -symmatvec(1,n,i,h,g);
dg=sqrt(vecvec(1,n,0,g,g));
nrmdelta=sqrt(vecvec(1,n,0,delta,delta));
eps=sqrt(vecvec(1,n,0,x,x))*reltol+abstol;
dg0=vecvec(1,n,0,delta,g);
it++;
while ((nrmdelta > eps || dg > tolg) && (evl < evlmax)) {
dupvec(1,n,0,s,x);
dupvec(1,n,0,v,g);
if (it >= n)
alfa=1.0;
else {
if (it != 1)
alfa /= nrmdelta;
else {
alfa=2.0*(fmin-f)/dg0;
if (alfa > 1.0) alfa=1.0;
}
}
elmvec(1,n,0,x,delta,alfa);
f0=f;
f=(*funct)(n,x,g);
evl++;
dg=vecvec(1,n,0,delta,g);
if (it == 1 || f0-f < -mu*dg0*alfa) {
/* line minimization */
i=evlmax-evl;
cntl++;
linemin(n,s,delta,nrmdelta,&alfa,g,funct,f0,&f,
dg0,&dg,&i,0,in);
evl += i;
dupvec(1,n,0,x,s);
}
if (alfa != 1.0) mulvec(1,n,0,delta,delta,alfa);
mulvec(1,n,0,v,v,-1.0);
elmvec(1,n,0,v,g,1.0);
for (i=1; i<=n; i++) s[i]=symmatvec(1,n,i,h,v);
aid=vecvec(1,n,0,v,s);
dg=(dg-dg0)*alfa;
if (dg > 0.0)
if (dg >= aid)
fleupd(h,n,delta,s,1.0/dg,(1.0+aid/dg)/dg);
else
davupd(h,n,delta,s,1.0/dg,1.0/aid);
for (i=1; i<=n; i++) delta[i] = -symmatvec(1,n,i,h,g);
alfa *= nrmdelta;
nrmdelta=sqrt(vecvec(1,n,0,delta,delta));
eps=sqrt(vecvec(1,n,0,x,x))*reltol+abstol;
dg=sqrt(vecvec(1,n,0,g,g));
dg0=vecvec(1,n,0,delta,g);
if (dg0 > 0.0) {
out[4] = -1.0;
break;
}
it++;
}
out[0]=nrmdelta;
out[1]=dg;
out[2]=evl;
out[3]=cntl;
free_real_vector(v,1);
free_real_vector(delta,1);
free_real_vector(s,1);
return f;
}
void quanewbnd(int n, int lw, int rw,
real_t x[], real_t f[], real_t jac[],
int (*funct)(int, int, int, real_t[], real_t[]),
real_t in[], real_t out[])
{
real_t *allocate_real_vector(int, int);
void free_real_vector(real_t *, int);
real_t vecvec(int, int, int, real_t [], real_t []);
void elmvec(int, int, int, real_t [], real_t [], real_t);
void mulvec(int, int, int, real_t [], real_t [], real_t);
void dupvec(int, int, int, real_t [], real_t []);
void decsolbnd(real_t [], int, int, int, real_t [], real_t []);
int l,it,fcnt,fmax,err,b,i,j,k,r,m;
real_t macheps,reltol,abstol,tolres,nd,mz,res,*delta,mul,crit,
*pp,*s,aux[6],*lu;
delta=allocate_real_vector(1,n);
nd=0.0;
macheps=in[0];
reltol=in[1];
abstol=in[2];
tolres=in[3];
fmax=in[4];
mz=macheps*macheps;
it=fcnt=0;
b=lw+rw;
l=(n-1)*b+n;
b++;
res=sqrt(vecvec(1,n,0,f,f));
err=0;
while (1) {
if (err != 0 || (res < tolres &&
sqrt(nd) < sqrt(vecvec(1,n,0,x,x))*reltol+abstol)) break;
it++;
if (it != 1) {
/* update jac */
pp=allocate_real_vector(1,n);
s=allocate_real_vector(1,n);
crit=nd*mz;
for (i=1; i<=n; i++) pp[i]=delta[i]*delta[i];
r=k=1;
m=rw+1;
for (i=1; i<=n; i++) {
mul=0.0;
for (j=r; j<=m; j++) mul += pp[j];
j=r-k;
if (fabs(mul) > crit) elmvec(k,m-j,j,jac,delta,f[i]/mul);
k += b;
if (i > lw)
r++;
else
k--;
if (m < n) m++;
}
free_real_vector(pp,1);
free_real_vector(s,1);
}
/* direction */
lu=allocate_real_vector(1,l);
aux[2]=macheps;
mulvec(1,n,0,delta,f,-1.0);
dupvec(1,l,0,lu,jac);
decsolbnd(lu,n,lw,rw,aux,delta);
free_real_vector(lu,1);
if (aux[3] != n) {
err=3;
break;
} else {
elmvec(1,n,0,x,delta,1.0);
nd=vecvec(1,n,0,delta,delta);
/* evaluate */
fcnt += n;
if (!((*funct)(n,1,n,x,f))) {
err=2;
break;
}
if (fcnt > fmax) err=1;
res=sqrt(vecvec(1,n,0,f,f));
}
}
out[1]=sqrt(nd);
out[2]=res;
out[3]=fcnt;
out[4]=it;
out[5]=err;
free_real_vector(delta,1);
}
