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); }