void spline1dfitpenalizedw(/* Real */ ae_vector* x,
/* Real */ ae_vector* y,
/* Real */ ae_vector* w,
ae_int_t n,
ae_int_t m,
double rho,
ae_int_t* info,
spline1dinterpolant* s,
spline1dfitreport* rep,
ae_state *_state)
{
ae_frame _frame_block;
ae_vector _x;
ae_vector _y;
ae_vector _w;
ae_int_t i;
ae_int_t j;
ae_int_t b;
double v;
double relcnt;
double xa;
double xb;
double sa;
double sb;
ae_vector xoriginal;
ae_vector yoriginal;
double pdecay;
double tdecay;
ae_matrix fmatrix;
ae_vector fcolumn;
ae_vector y2;
ae_vector w2;
ae_vector xc;
ae_vector yc;
ae_vector dc;
double fdmax;
double admax;
ae_matrix amatrix;
ae_matrix d2matrix;
double fa;
double ga;
double fb;
double gb;
double lambdav;
ae_vector bx;
ae_vector by;
ae_vector bd1;
ae_vector bd2;
ae_vector tx;
ae_vector ty;
ae_vector td;
spline1dinterpolant bs;
ae_matrix nmatrix;
ae_vector rightpart;
fblslincgstate cgstate;
ae_vector c;
ae_vector tmp0;
ae_frame_make(_state, &_frame_block);
ae_vector_init_copy(&_x, x, _state, ae_true);
x = &_x;
ae_vector_init_copy(&_y, y, _state, ae_true);
y = &_y;
ae_vector_init_copy(&_w, w, _state, ae_true);
w = &_w;
*info = 0;
_spline1dinterpolant_clear(s);
_spline1dfitreport_clear(rep);
ae_vector_init(&xoriginal, 0, DT_REAL, _state, ae_true);
ae_vector_init(&yoriginal, 0, DT_REAL, _state, ae_true);
ae_matrix_init(&fmatrix, 0, 0, DT_REAL, _state, ae_true);
ae_vector_init(&fcolumn, 0, DT_REAL, _state, ae_true);
ae_vector_init(&y2, 0, DT_REAL, _state, ae_true);
ae_vector_init(&w2, 0, DT_REAL, _state, ae_true);
ae_vector_init(&xc, 0, DT_REAL, _state, ae_true);
ae_vector_init(&yc, 0, DT_REAL, _state, ae_true);
ae_vector_init(&dc, 0, DT_INT, _state, ae_true);
ae_matrix_init(&amatrix, 0, 0, DT_REAL, _state, ae_true);
ae_matrix_init(&d2matrix, 0, 0, DT_REAL, _state, ae_true);
ae_vector_init(&bx, 0, DT_REAL, _state, ae_true);
ae_vector_init(&by, 0, DT_REAL, _state, ae_true);
ae_vector_init(&bd1, 0, DT_REAL, _state, ae_true);
ae_vector_init(&bd2, 0, DT_REAL, _state, ae_true);
ae_vector_init(&tx, 0, DT_REAL, _state, ae_true);
ae_vector_init(&ty, 0, DT_REAL, _state, ae_true);
ae_vector_init(&td, 0, DT_REAL, _state, ae_true);
_spline1dinterpolant_init(&bs, _state, ae_true);
ae_matrix_init(&nmatrix, 0, 0, DT_REAL, _state, ae_true);
ae_vector_init(&rightpart, 0, DT_REAL, _state, ae_true);
_fblslincgstate_init(&cgstate, _state, ae_true);
ae_vector_init(&c, 0, DT_REAL, _state, ae_true);
ae_vector_init(&tmp0, 0, DT_REAL, _state, ae_true);
ae_assert(n>=1, "Spline1DFitPenalizedW: N<1!", _state);
ae_assert(m>=4, "Spline1DFitPenalizedW: M<4!", _state);
ae_assert(x->cnt>=n, "Spline1DFitPenalizedW: Length(X)<N!", _state);
ae_assert(y->cnt>=n, "Spline1DFitPenalizedW: Length(Y)<N!", _state);
ae_assert(w->cnt>=n, "Spline1DFitPenalizedW: Length(W)<N!", _state);
ae_assert(isfinitevector(x, n, _state), "Spline1DFitPenalizedW: X contains infinite or NAN values!", _state);
ae_assert(isfinitevector(y, n, _state), "Spline1DFitPenalizedW: Y contains infinite or NAN values!", _state);
ae_assert(isfinitevector(w, n, _state), "Spline1DFitPenalizedW: Y contains infinite or NAN values!", _state);
ae_assert(ae_isfinite(rho, _state), "Spline1DFitPenalizedW: Rho is infinite!", _state);
/*
* Prepare LambdaV
*/
v = -ae_log(ae_machineepsilon, _state)/ae_log(10, _state);
if( ae_fp_less(rho,-v) )
{
rho = -v;
}
if( ae_fp_greater(rho,v) )
{
rho = v;
}
lambdav = ae_pow(10, rho, _state);
/*
* Sort X, Y, W
*/
heapsortdpoints(x, y, w, n, _state);
/*
* Scale X, Y, XC, YC
*/
lsfitscalexy(x, y, w, n, &xc, &yc, &dc, 0, &xa, &xb, &sa, &sb, &xoriginal, &yoriginal, _state);
/*
* Allocate space
*/
ae_matrix_set_length(&fmatrix, n, m, _state);
ae_matrix_set_length(&amatrix, m, m, _state);
ae_matrix_set_length(&d2matrix, m, m, _state);
ae_vector_set_length(&bx, m, _state);
ae_vector_set_length(&by, m, _state);
ae_vector_set_length(&fcolumn, n, _state);
ae_matrix_set_length(&nmatrix, m, m, _state);
ae_vector_set_length(&rightpart, m, _state);
ae_vector_set_length(&tmp0, ae_maxint(m, n, _state), _state);
ae_vector_set_length(&c, m, _state);
/*
* Fill:
* * FMatrix by values of basis functions
* * TmpAMatrix by second derivatives of I-th function at J-th point
* * CMatrix by constraints
*/
fdmax = 0;
for (b=0; b<=m-1; b++)
{
/*
* Prepare I-th basis function
*/
for(j=0; j<=m-1; j++)
{
bx.ptr.p_double[j] = (double)(2*j)/(double)(m-1)-1;
by.ptr.p_double[j] = 0;
}
by.ptr.p_double[b] = 1;
//spline1dgriddiff2cubic(&bx, &by, m, 2, 0.0, 2, 0.0, &bd1, &bd2, _state);
test_gridDiff2Cubic(&bx, &by, m, &bd1, &bd2, _state);
// spline1dbuildcubic(&bx, &by, m, 2, 0.0, 2, 0.0, &bs, _state);
test_buildCubic(&bx, &by, m, &bs, _state);
/*
* Calculate B-th column of FMatrix
* Update FDMax (maximum column norm)
*/
spline1dconvcubic(&bx, &by, m, 2, 0.0, 2, 0.0, x, n, &fcolumn, _state);
ae_v_move(&fmatrix.ptr.pp_double[0][b], fmatrix.stride, &fcolumn.ptr.p_double[0], 1, ae_v_len(0,n-1));
v = 0;
for(i=0; i<=n-1; i++)
{
//fprintf(stderr, "fcoll %d %f\n", i, fcolumn.ptr.p_double[i]);
v = v+ae_sqr(w->ptr.p_double[i]*fcolumn.ptr.p_double[i], _state);
}
fdmax = ae_maxreal(fdmax, v, _state);
/*
* Fill temporary with second derivatives of basis function
*/
ae_v_move(&d2matrix.ptr.pp_double[b][0], 1, &bd2.ptr.p_double[0], 1, ae_v_len(0,m-1));
}
/*
* * calculate penalty matrix A
* * calculate max of diagonal elements of A
* * calculate PDecay - coefficient before penalty matrix
*/
for(i=0; i<=m-1; i++)
{
for(j=i; j<=m-1; j++)
{
/*
* calculate integral(B_i''*B_j'') where B_i and B_j are
* i-th and j-th basis splines.
* B_i and B_j are piecewise linear functions.
*/
v = 0;
for(b=0; b<=m-2; b++)
{
fa = d2matrix.ptr.pp_double[i][b];
fb = d2matrix.ptr.pp_double[i][b+1];
ga = d2matrix.ptr.pp_double[j][b];
gb = d2matrix.ptr.pp_double[j][b+1];
v = v+(bx.ptr.p_double[b+1]-bx.ptr.p_double[b])*(fa*ga+(fa*(gb-ga)+ga*(fb-fa))/2+(fb-fa)*(gb-ga)/3);
}
amatrix.ptr.pp_double[i][j] = v;
amatrix.ptr.pp_double[j][i] = v;
}
}
admax = 0;
for(i=0; i<=m-1; i++)
{
admax = ae_maxreal(admax, ae_fabs(amatrix.ptr.pp_double[i][i], _state), _state);
}
pdecay = lambdav*fdmax/admax;
/*
* Calculate TDecay for Tikhonov regularization
*/
tdecay = fdmax*(1+pdecay)*10*ae_machineepsilon;
/*
* Prepare system
*
* NOTE: FMatrix is spoiled during this process
*/
for(i=0; i<=n-1; i++)
{
v = w->ptr.p_double[i];
ae_v_muld(&fmatrix.ptr.pp_double[i][0], 1, ae_v_len(0,m-1), v);
}
rmatrixgemm(m, m, n, 1.0, &fmatrix, 0, 0, 1, &fmatrix, 0, 0, 0, 0.0, &nmatrix, 0, 0, _state);
for(i=0; i<=m-1; i++)
{
for(j=0; j<=m-1; j++)
{
nmatrix.ptr.pp_double[i][j] = nmatrix.ptr.pp_double[i][j]+pdecay*amatrix.ptr.pp_double[i][j];
}
}
for(i=0; i<=m-1; i++)
{
nmatrix.ptr.pp_double[i][i] = nmatrix.ptr.pp_double[i][i]+tdecay;
}
for(i=0; i<=m-1; i++)
{
rightpart.ptr.p_double[i] = 0;
}
for(i=0; i<=n-1; i++)
{
v = y->ptr.p_double[i]*w->ptr.p_double[i];
ae_v_addd(&rightpart.ptr.p_double[0], 1, &fmatrix.ptr.pp_double[i][0], 1, ae_v_len(0,m-1), v);
}
/*
* Solve system
*/
if( !spdmatrixcholesky(&nmatrix, m, ae_true, _state) )
{
*info = -4;
ae_frame_leave(_state);
return;
}
fblscholeskysolve(&nmatrix, 1.0, m, ae_true, &rightpart, &tmp0, _state);
ae_v_move(&c.ptr.p_double[0], 1, &rightpart.ptr.p_double[0], 1, ae_v_len(0,m-1));
/*
* add nodes to force linearity outside of the fitting interval
*/
spline1dgriddiffcubic(&bx, &c, m, 2, 0.0, 2, 0.0, &bd1, _state);
ae_vector_set_length(&tx, m+2, _state);
ae_vector_set_length(&ty, m+2, _state);
ae_vector_set_length(&td, m+2, _state);
ae_v_move(&tx.ptr.p_double[1], 1, &bx.ptr.p_double[0], 1, ae_v_len(1,m));
ae_v_move(&ty.ptr.p_double[1], 1, &rightpart.ptr.p_double[0], 1, ae_v_len(1,m));
ae_v_move(&td.ptr.p_double[1], 1, &bd1.ptr.p_double[0], 1, ae_v_len(1,m));
tx.ptr.p_double[0] = tx.ptr.p_double[1]-(tx.ptr.p_double[2]-tx.ptr.p_double[1]);
ty.ptr.p_double[0] = ty.ptr.p_double[1]-td.ptr.p_double[1]*(tx.ptr.p_double[2]-tx.ptr.p_double[1]);
td.ptr.p_double[0] = td.ptr.p_double[1];
tx.ptr.p_double[m+1] = tx.ptr.p_double[m]+(tx.ptr.p_double[m]-tx.ptr.p_double[m-1]);
ty.ptr.p_double[m+1] = ty.ptr.p_double[m]+td.ptr.p_double[m]*(tx.ptr.p_double[m]-tx.ptr.p_double[m-1]);
td.ptr.p_double[m+1] = td.ptr.p_double[m];
spline1dbuildhermite(&tx, &ty, &td, m+2, s, _state);
spline1dlintransx(s, 2/(xb-xa), -(xa+xb)/(xb-xa), _state);
spline1dlintransy(s, sb-sa, sa, _state);
*info = 1;
/*
* Fill report
*/
rep->rmserror = 0;
rep->avgerror = 0;
rep->avgrelerror = 0;
rep->maxerror = 0;
relcnt = 0;
spline1dconvcubic(&bx, &rightpart, m, 2, 0.0, 2, 0.0, x, n, &fcolumn, _state);
for(i=0; i<=n-1; i++)
{
v = (sb-sa)*fcolumn.ptr.p_double[i]+sa;
rep->rmserror = rep->rmserror+ae_sqr(v-yoriginal.ptr.p_double[i], _state);
rep->avgerror = rep->avgerror+ae_fabs(v-yoriginal.ptr.p_double[i], _state);
if( ae_fp_neq(yoriginal.ptr.p_double[i],0) )
{
rep->avgrelerror = rep->avgrelerror+ae_fabs(v-yoriginal.ptr.p_double[i], _state)/ae_fabs(yoriginal.ptr.p_double[i], _state);
relcnt = relcnt+1;
}
rep->maxerror = ae_maxreal(rep->maxerror, ae_fabs(v-yoriginal.ptr.p_double[i], _state), _state);
}
rep->rmserror = ae_sqrt(rep->rmserror/n, _state);
rep->avgerror = rep->avgerror/n;
if( ae_fp_neq(relcnt,0) )
{
rep->avgrelerror = rep->avgrelerror/relcnt;
}
ae_frame_leave(_state);
}
/*************************************************************************
Internal initialization subroutine
*************************************************************************/
static void odesolver_odesolverinit(ae_int_t solvertype,
/* Real */ ae_vector* y,
ae_int_t n,
/* Real */ ae_vector* x,
ae_int_t m,
double eps,
double h,
odesolverstate* state,
ae_state *_state)
{
ae_int_t i;
double v;
_odesolverstate_clear(state);
/*
* Prepare RComm
*/
ae_vector_set_length(&state->rstate.ia, 5+1, _state);
ae_vector_set_length(&state->rstate.ba, 0+1, _state);
ae_vector_set_length(&state->rstate.ra, 5+1, _state);
state->rstate.stage = -1;
state->needdy = ae_false;
/*
* check parameters.
*/
if( (n<=0||m<1)||ae_fp_eq(eps,(double)(0)) )
{
state->repterminationtype = -1;
return;
}
if( ae_fp_less(h,(double)(0)) )
{
h = -h;
}
/*
* quick exit if necessary.
* after this block we assume that M>1
*/
if( m==1 )
{
state->repnfev = 0;
state->repterminationtype = 1;
ae_matrix_set_length(&state->ytbl, 1, n, _state);
ae_v_move(&state->ytbl.ptr.pp_double[0][0], 1, &y->ptr.p_double[0], 1, ae_v_len(0,n-1));
ae_vector_set_length(&state->xg, m, _state);
ae_v_move(&state->xg.ptr.p_double[0], 1, &x->ptr.p_double[0], 1, ae_v_len(0,m-1));
return;
}
/*
* check again: correct order of X[]
*/
if( ae_fp_eq(x->ptr.p_double[1],x->ptr.p_double[0]) )
{
state->repterminationtype = -2;
return;
}
for(i=1; i<=m-1; i++)
{
if( (ae_fp_greater(x->ptr.p_double[1],x->ptr.p_double[0])&&ae_fp_less_eq(x->ptr.p_double[i],x->ptr.p_double[i-1]))||(ae_fp_less(x->ptr.p_double[1],x->ptr.p_double[0])&&ae_fp_greater_eq(x->ptr.p_double[i],x->ptr.p_double[i-1])) )
{
state->repterminationtype = -2;
return;
}
}
/*
* auto-select H if necessary
*/
if( ae_fp_eq(h,(double)(0)) )
{
v = ae_fabs(x->ptr.p_double[1]-x->ptr.p_double[0], _state);
for(i=2; i<=m-1; i++)
{
v = ae_minreal(v, ae_fabs(x->ptr.p_double[i]-x->ptr.p_double[i-1], _state), _state);
}
h = 0.001*v;
}
/*
* store parameters
*/
state->n = n;
state->m = m;
state->h = h;
state->eps = ae_fabs(eps, _state);
state->fraceps = ae_fp_less(eps,(double)(0));
ae_vector_set_length(&state->xg, m, _state);
ae_v_move(&state->xg.ptr.p_double[0], 1, &x->ptr.p_double[0], 1, ae_v_len(0,m-1));
if( ae_fp_greater(x->ptr.p_double[1],x->ptr.p_double[0]) )
{
state->xscale = (double)(1);
}
else
{
state->xscale = (double)(-1);
ae_v_muld(&state->xg.ptr.p_double[0], 1, ae_v_len(0,m-1), -1);
}
ae_vector_set_length(&state->yc, n, _state);
ae_v_move(&state->yc.ptr.p_double[0], 1, &y->ptr.p_double[0], 1, ae_v_len(0,n-1));
state->solvertype = solvertype;
state->repterminationtype = 0;
/*
* Allocate arrays
*/
ae_vector_set_length(&state->y, n, _state);
ae_vector_set_length(&state->dy, n, _state);
}