void KF_deriv_steady_C (int *dim, double *sy, double *sZ, double *sT, double *sH,
double *sR, double *sV, double *sQ, double *sa0, double *sP0,
double *tol, int *maxiter,
std::vector<double> *invf, std::vector<double> *vof,
double *dvof, std::vector<double> *dfinvfsq,
gsl_matrix *a_pred, std::vector<gsl_matrix*> *P_pred,
gsl_matrix *K, std::vector<gsl_matrix*> *L,
std::vector<gsl_matrix*> *da_pred,
std::vector< std::vector<gsl_matrix*> > *dP_pred,
std::vector<gsl_matrix*> *dK)
{
//int s, p = dim[1], mp1 = m + 1;
int i, j, k, n = dim[0], m = dim[2],
jm1, r = dim[3], rp1 = r + 1,
conv = 0, counter = 0;
//double v, f, fim1, df[rp1], dv, dtmp; //Kisum, Kim1sum;
double v, f, fim1, dv, dtmp; //Kisum, Kim1sum;
std::vector<double> df(rp1);
//double mll = 0.0; // for debugging
// data and state space model matrices
gsl_vector_view Z = gsl_vector_view_array(sZ, m);
gsl_matrix_view T = gsl_matrix_view_array(sT, m, m);
gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m);
// storage vectors and matrices
gsl_vector *Vm = gsl_vector_alloc(m);
gsl_vector *Vm_cp = gsl_vector_alloc(m);
gsl_vector *Vm_cp2 = gsl_vector_alloc(m);
gsl_vector *Vm_cp3 = gsl_vector_alloc(m);
gsl_vector *Vm3 = gsl_vector_alloc(m);
gsl_matrix *Mmm = gsl_matrix_alloc(m, m);
gsl_matrix *M1m = gsl_matrix_alloc(1, m);
gsl_matrix *Mm1 = gsl_matrix_alloc(m, 1);
gsl_vector_view a0 = gsl_vector_view_array(sa0, m);
gsl_vector *a_upd = gsl_vector_alloc(m);
gsl_vector_memcpy(a_upd, &a0.vector);
gsl_matrix_view P0 = gsl_matrix_view_array(sP0, m, m);
gsl_matrix *P_upd = gsl_matrix_alloc(m, m);
gsl_matrix_memcpy(P_upd, &P0.matrix);
gsl_vector_view K_irow, m_irow, m2_irow, m3_irow, K_im1row; //Kri;
gsl_matrix_view maux1;
gsl_matrix_view Zm = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m);
gsl_vector *mZ = gsl_vector_alloc(m);
gsl_vector_memcpy(mZ, &Z.vector);
gsl_vector_scale(mZ, -1.0);
//std::vector<std::vector<gsl_matrix*> *> *da_pred;
std::vector<gsl_matrix*> dP_upd(rp1);
for (j = 0; j < rp1; j++)
{
da_pred[0].at(j) = gsl_matrix_alloc(n, m);
dP_upd.at(j) = gsl_matrix_calloc(m, m);
}
gsl_matrix *da_upd = gsl_matrix_calloc(rp1, m);
// filtering recursions
for (i = 0; i < n; i++)
{
m_irow = gsl_matrix_row(a_pred, i);
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, a_upd, 0.0, &m_irow.vector);
P_pred[0].at(i) = gsl_matrix_alloc(m, m);
if (conv == 0) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, P_upd,
0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix,
0.0, P_pred[0].at(i));
gsl_matrix_add(P_pred[0].at(i), &Q.matrix);
} else {
gsl_matrix_memcpy(P_pred[0].at(i), P_pred[0].at(i-1));
}
gsl_blas_ddot(&Z.vector, &m_irow.vector, &v);
v = sy[i] - v;
if (conv == 0) {
gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred[0].at(i), &Z.vector,
0.0, Vm);
gsl_blas_ddot(&Z.vector, Vm, &f);
f += *sH;
invf->at(i) = 1.0 / f;
} else {
invf->at(i) = invf->at(i-1);
}
gsl_vector_memcpy(Vm_cp, Vm);
gsl_vector_memcpy(Vm_cp2, Vm);
gsl_vector_memcpy(Vm_cp3, Vm);
vof->at(i) = v * invf->at(i); // v[i]/f[i];
if (conv == 0) {
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm, 0), m, 1);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &maux1.matrix,
&maux1.matrix, 0.0, Mmm);
gsl_matrix_scale(Mmm, invf->at(i));
gsl_matrix_memcpy(P_upd, P_pred[0].at(i));
gsl_matrix_sub(P_upd, Mmm);
}
gsl_vector_memcpy(a_upd, &m_irow.vector);
gsl_vector_scale(Vm_cp3, vof->at(i));
gsl_vector_add(a_upd, Vm_cp3);
K_irow = gsl_matrix_row(K, i);
gsl_vector_scale(Vm_cp, invf->at(i));
if (conv == 0) {
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, Vm_cp, 0.0, &K_irow.vector);
} else {
K_im1row = gsl_matrix_row(K, i-1);
gsl_vector_memcpy(&K_irow.vector, &K_im1row.vector);
}
L[0].at(i) = gsl_matrix_alloc(m, m);
if (conv == 0) {
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), m, 1);
gsl_matrix_memcpy(L[0].at(i), &T.matrix);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix,
&Zm.matrix, 1.0, L[0].at(i));
} else {
gsl_matrix_memcpy(L[0].at(i), L[0].at(i-1));
}
// derivatives
dK[0].at(i) = gsl_matrix_alloc(rp1, m);
for (j = 0; j < rp1; j++)
{
k = i + j * n;
m_irow = gsl_matrix_row(da_upd, j);
m2_irow = gsl_matrix_row(da_pred[0].at(j), i);
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, &m_irow.vector,
0.0, &m2_irow.vector);
gsl_blas_ddot(mZ, &m2_irow.vector, &dv);
(dP_pred[0].at(i)).at(j) = gsl_matrix_alloc(m, m);
if (conv == 0) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, dP_upd.at(j),
0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix,
0.0, (dP_pred[0].at(i)).at(j));
if (j != 0)
{
jm1 = j - 1;
dtmp = gsl_matrix_get((dP_pred[0].at(i)).at(j), jm1, jm1);
gsl_matrix_set((dP_pred[0].at(i)).at(j), jm1, jm1, dtmp + 1.0);
}
} else {
gsl_matrix_memcpy((dP_pred[0].at(i)).at(j), (dP_pred[0].at(i-1)).at(j));
}
if (conv == 0) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Zm.matrix,
(dP_pred[0].at(i)).at(j), 0.0, M1m);
m_irow = gsl_matrix_row(M1m, 0);
gsl_blas_ddot(&m_irow.vector, &Z.vector, &df[j]);
if (j == 0) {
df[j] += 1.0;
}
}
dvof[k] = (dv * f - v * df[j]) * pow(invf->at(i), 2);
m_irow = gsl_matrix_row(da_upd, j);
gsl_blas_dgemv(CblasNoTrans, vof->at(i), (dP_pred[0].at(i)).at(j), &Z.vector,
0.0, &m_irow.vector);
gsl_vector_add(&m_irow.vector, &m2_irow.vector);
dtmp = -1.0 * df[j] * invf->at(i);
gsl_blas_daxpy(dtmp, Vm_cp3, &m_irow.vector);
gsl_blas_daxpy(dv, Vm_cp, &m_irow.vector);
dfinvfsq->at(k) = df[j] * pow(invf->at(i), 2);
if (conv == 0) {
gsl_matrix_memcpy(dP_upd.at(j), (dP_pred[0].at(i)).at(j));
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, (dP_pred[0].at(i)).at(j),
&Zm.matrix, 0.0, Mm1);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mm1, &maux1.matrix,
1.0, dP_upd.at(j));
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), m, 1);
gsl_matrix_memcpy(Mm1, &maux1.matrix);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, dfinvfsq->at(k), Mm1,
&maux1.matrix, 1.0, dP_upd.at(j));
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), m, 1);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix,
&Zm.matrix, 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mmm,
(dP_pred[0].at(i)).at(j), 1.0, dP_upd.at(j));
}
m3_irow = gsl_matrix_row(dK[0].at(i), j);
if (conv == 0) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix,
(dP_pred[0].at(i)).at(j), 0.0, Mmm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, &m3_irow.vector);
gsl_vector_scale(&m3_irow.vector, invf->at(i));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix,
P_pred[0].at(i), 0.0, Mmm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, Vm3);
gsl_vector_scale(Vm3, dfinvfsq->at(k));
gsl_vector_sub(&m3_irow.vector, Vm3);
} else {
K_im1row = gsl_matrix_row(dK[0].at(i-1), j);
gsl_vector_memcpy(&m3_irow.vector, &K_im1row.vector);
}
}
// check if convergence to the steady state has been reached
if ((i > 0) & (conv == 0))
{
if (i == 1)
{
fim1 = f + 1.0;
}
if (fabs(f - fim1) < *tol)
{
counter += 1;
}
fim1 = f;
if (counter == *maxiter) {
conv = 1;
dim[5] = i;
}
}
}
// deallocate memory
for (j = 0; j < rp1; j++)
{
gsl_matrix_free(dP_upd.at(j));
}
gsl_vector_free(mZ);
gsl_vector_free(a_upd);
gsl_matrix_free(P_upd);
gsl_vector_free(Vm);
gsl_vector_free(Vm_cp);
gsl_vector_free(Vm_cp2);
gsl_vector_free(Vm_cp3);
gsl_vector_free(Vm3);
gsl_matrix_free(Mmm);
gsl_matrix_free(M1m);
gsl_matrix_free(Mm1);
gsl_matrix_free(da_upd);
}
void GICPOptimizer::fdf(const gsl_vector *x, void *params, double * f, gsl_vector *g) {
std::cout << ">>> fdf" << std::endl;
GICPOptData *opt_data = (GICPOptData *)params;
double pt1[3];
double pt2[3];
double res[3]; // residual
double temp[3]; // temp local vector
double temp_mat[9]; // temp matrix used for accumulating the rotation gradient
gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3);
gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3);
gsl_vector_view gsl_res = gsl_vector_view_array(res, 3);
gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3);
gsl_vector_view gsl_gradient_t = gsl_vector_subvector(g, 0, 3); // translation comp. of gradient
gsl_vector_view gsl_gradient_r = gsl_vector_subvector(g, 3, 3); // rotation comp. of gradient
gsl_matrix_view gsl_temp_mat_r = gsl_matrix_view_array(temp_mat, 3, 3);
gsl_matrix_view gsl_M;
dgc_transform_t t;
double temp_double;
// take the base transformation
dgc_transform_copy(t, opt_data->base_t);
// apply the current state
apply_state(t, x);
// zero all accumulator variables
*f = 0;
gsl_vector_set_zero(g);
gsl_vector_set_zero(&gsl_temp.vector);
gsl_matrix_set_zero(&gsl_temp_mat_r.matrix);
for(int i = 0; i < opt_data->p1->Size(); i++) {
int j = opt_data->nn_indecies[i];
if(j != -1) {
// get point 1
pt1[0] = (*opt_data->p1)[i].x;
pt1[1] = (*opt_data->p1)[i].y;
pt1[2] = (*opt_data->p1)[i].z;
// get point 2
pt2[0] = (*opt_data->p2)[j].x;
pt2[1] = (*opt_data->p2)[j].y;
pt2[2] = (*opt_data->p2)[j].z;
//cout << "accessing " << i << " of " << opt_data->p1->Size() << ", " << opt_data->p2->Size() << endl;
//get M-matrix
gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3);
print_gsl_matrix(&gsl_M.matrix, "M");
//transform point 1
dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t);
std::cout << "pt1 " << pt1[0] << "," << pt1[1] << "," << pt1[2] << std::endl;
res[0] = pt1[0] - pt2[0];
res[1] = pt1[1] - pt2[1];
res[2] = pt1[2] - pt2[2];
std::cout << "res " << res[0] << "," << res[1] << "," << res[2] << std::endl;
// compute the transformed residual
// temp := M*res
//print_gsl_matrix(&gsl_M.matrix, "gsl_m");
gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector);
print_gsl_vector(&gsl_temp.vector, "temp");
// compute M-norm of the residual
// temp_double := res'*temp = temp'*M*res
gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double);
// accumulate total error: f += res'*M*res
*f += temp_double/(double)opt_data->num_matches;
std::cout << "f " << *f << std::endl;
// accumulate translation gradient:
// gsl_gradient_t += 2*M*res
gsl_blas_dsymv(CblasLower, 2./(double)opt_data->num_matches, &gsl_M.matrix, &gsl_res.vector, 1., &gsl_gradient_t.vector);
if(opt_data->solve_rotation) {
// accumulate the rotation gradient matrix
// get back the original untransformed point to compute the rotation gradient
pt1[0] = (*opt_data->p1)[i].x;
pt1[1] = (*opt_data->p1)[i].y;
pt1[2] = (*opt_data->p1)[i].z;
dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], opt_data->base_t);
// gsl_temp_mat_r += 2*(gsl_temp).(gsl_pt1)' [ = (2*M*residual).(gsl_pt1)' ]
gsl_blas_dger(2./(double)opt_data->num_matches, &gsl_pt1.vector, &gsl_temp.vector, &gsl_temp_mat_r.matrix);
}
}
}
print_gsl_vector(g, "gradient");
// the above loop sets up the gradient with respect to the translation, and the matrix derivative w.r.t. the rotation matrix
// this code sets up the matrix derivatives dR/dPhi, dR/dPsi, dR/dTheta. i.e. the derivatives of the whole rotation matrix with respect to the euler angles
// note that this code assumes the XYZ order of euler angles, with the Z rotation corresponding to bearing. This means the Z angle is negative of what it would be
// in the regular XYZ euler-angle convention.
if(opt_data->solve_rotation) {
// now use the d/dR matrix to compute the derivative with respect to euler angles and put it directly into g[3], g[4], g[5];
compute_dr(x, &gsl_temp_mat_r.matrix, g);
}
print_gsl_matrix(&gsl_temp_mat_r.matrix, "R");
print_gsl_vector(g, "gradient");
std::cout << "<<< fdf" << std::endl;
}
void
test_pontius ()
{
size_t i, j;
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (pontius_n, pontius_p);
gsl_multifit_robust_workspace * work_rob =
gsl_multifit_robust_alloc (gsl_multifit_robust_ols, pontius_n, pontius_p);
gsl_matrix * X = gsl_matrix_alloc (pontius_n, pontius_p);
gsl_vector_view y = gsl_vector_view_array (pontius_y, pontius_n);
gsl_vector * c = gsl_vector_alloc (pontius_p);
gsl_vector * r = gsl_vector_alloc (pontius_n);
gsl_matrix * cov = gsl_matrix_alloc (pontius_p, pontius_p);
double chisq, chisq_res;
double expected_c[3] = { 0.673565789473684E-03,
0.732059160401003E-06,
-0.316081871345029E-14};
double expected_sd[3] = { 0.107938612033077E-03,
0.157817399981659E-09,
0.486652849992036E-16 };
double expected_chisq = 0.155761768796992E-05;
gsl_vector_view diag = gsl_matrix_diagonal (cov);
gsl_vector_view exp_c = gsl_vector_view_array(expected_c, pontius_p);
gsl_vector_view exp_sd = gsl_vector_view_array(expected_sd, pontius_p);
for (i = 0 ; i < pontius_n; i++)
{
for (j = 0; j < pontius_p; j++)
{
gsl_matrix_set(X, i, j, pow(pontius_x[i], j));
}
}
/* test unweighted least squares */
gsl_multifit_linear (X, &y.vector, c, cov, &chisq, work);
gsl_multifit_linear_residuals(X, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq_res);
test_pontius_results("pontius gsl_multifit_linear",
c, &exp_c.vector,
&diag.vector, &exp_sd.vector,
chisq, chisq_res, expected_chisq);
/* test robust least squares */
gsl_multifit_robust (X, &y.vector, c, cov, work_rob);
test_pontius_results("pontius gsl_multifit_robust",
c, &exp_c.vector,
&diag.vector, &exp_sd.vector,
1.0, 1.0, 1.0);
/* test weighted least squares */
{
gsl_vector * w = gsl_vector_alloc (pontius_n);
double expected_cov[3][3] ={
{2.76754385964916e-01 , -3.59649122807024e-07, 9.74658869395731e-14},
{-3.59649122807024e-07, 5.91630591630603e-13, -1.77210703526497e-19},
{9.74658869395731e-14, -1.77210703526497e-19, 5.62573661988878e-26} };
gsl_vector_set_all (w, 1.0);
gsl_multifit_wlinear (X, w, &y.vector, c, cov, &chisq, work);
gsl_multifit_linear_residuals(X, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq_res);
test_pontius_results("pontius gsl_multifit_wlinear",
c, &exp_c.vector,
NULL, NULL,
chisq, chisq_res, expected_chisq);
for (i = 0; i < pontius_p; i++)
{
for (j = 0; j < pontius_p; j++)
{
gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-10,
"pontius gsl_multifit_wlinear cov(%d,%d)", i, j) ;
}
}
gsl_vector_free(w);
}
gsl_vector_free(c);
gsl_vector_free(r);
gsl_matrix_free(cov);
gsl_matrix_free(X);
gsl_multifit_linear_free (work);
gsl_multifit_robust_free (work_rob);
}
int
main (void)
{
const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust;
gsl_multifit_nlinear_workspace *w;
gsl_multifit_nlinear_fdf fdf;
gsl_multifit_nlinear_parameters fdf_params =
gsl_multifit_nlinear_default_parameters();
const size_t n = N;
const size_t p = 3;
gsl_vector *f;
gsl_matrix *J;
gsl_matrix *covar = gsl_matrix_alloc (p, p);
double y[N], weights[N];
struct data d = { n, y };
double x_init[3] = { 1.0, 1.0, 0.0 }; /* starting values */
gsl_vector_view x = gsl_vector_view_array (x_init, p);
gsl_vector_view wts = gsl_vector_view_array(weights, n);
gsl_rng * r;
double chisq, chisq0;
int status, info;
size_t i;
const double xtol = 1e-8;
const double gtol = 1e-8;
const double ftol = 0.0;
gsl_rng_env_setup();
r = gsl_rng_alloc(gsl_rng_default);
/* define the function to be minimized */
fdf.f = expb_f;
fdf.df = expb_df; /* set to NULL for finite-difference Jacobian */
fdf.fvv = NULL; /* not using geodesic acceleration */
fdf.n = n;
fdf.p = p;
fdf.params = &d;
/* this is the data to be fitted */
for (i = 0; i < n; i++)
{
double t = i;
double yi = 1.0 + 5 * exp (-0.1 * t);
double si = 0.1 * yi;
double dy = gsl_ran_gaussian(r, si);
weights[i] = 1.0 / (si * si);
y[i] = yi + dy;
printf ("data: "F_ZU" %g %g\n", i, y[i], si);
};
/* allocate workspace with default parameters */
w = gsl_multifit_nlinear_alloc (T, &fdf_params, n, p);
/* initialize solver with starting point and weights */
gsl_multifit_nlinear_winit (&x.vector, &wts.vector, &fdf, w);
/* compute initial cost function */
f = gsl_multifit_nlinear_residual(w);
gsl_blas_ddot(f, f, &chisq0);
/* solve the system with a maximum of 20 iterations */
status = gsl_multifit_nlinear_driver(20, xtol, gtol, ftol,
callback, NULL, &info, w);
/* compute covariance of best fit parameters */
J = gsl_multifit_nlinear_jac(w);
gsl_multifit_nlinear_covar (J, 0.0, covar);
/* compute final cost */
gsl_blas_ddot(f, f, &chisq);
#define FIT(i) gsl_vector_get(w->x, i)
#define ERR(i) sqrt(gsl_matrix_get(covar,i,i))
fprintf(stderr, "summary from method '%s/%s'\n",
gsl_multifit_nlinear_name(w),
gsl_multifit_nlinear_trs_name(w));
fprintf(stderr, "number of iterations: "F_ZU"\n",
gsl_multifit_nlinear_niter(w));
fprintf(stderr, "function evaluations: "F_ZU"\n", fdf.nevalf);
fprintf(stderr, "Jacobian evaluations: "F_ZU"\n", fdf.nevaldf);
fprintf(stderr, "reason for stopping: %s\n",
(info == 1) ? "small step size" : "small gradient");
fprintf(stderr, "initial |f(x)| = %f\n", sqrt(chisq0));
fprintf(stderr, "final |f(x)| = %f\n", sqrt(chisq));
{
double dof = n - p;
double c = GSL_MAX_DBL(1, sqrt(chisq / dof));
fprintf(stderr, "chisq/dof = %g\n", chisq / dof);
fprintf (stderr, "A = %.5f +/- %.5f\n", FIT(0), c*ERR(0));
fprintf (stderr, "lambda = %.5f +/- %.5f\n", FIT(1), c*ERR(1));
fprintf (stderr, "b = %.5f +/- %.5f\n", FIT(2), c*ERR(2));
}
fprintf (stderr, "status = %s\n", gsl_strerror (status));
gsl_multifit_nlinear_free (w);
gsl_matrix_free (covar);
gsl_rng_free (r);
return 0;
}
/***************************************************************************************************************
Iterative NLLS fit algorithm
****************************************************************************************************************/
int iteration(const unsigned char *data, int w, int h, fit_t * results)
{
fprintf(fitDebug, "IMMAGINE: %dx%d \nSTARTING STRUCT: \n", w, h);
printFit(fitDebug, results);
fflush(fitDebug);
int npixels = w * h;
double *diff = new double[npixels];
double min = 255;
double max = -255;
int temp;
int x, y;
int index;
gsl_vector *delta = gsl_vector_alloc(8);
gsl_vector *vettore = gsl_vector_alloc(8);
fprintf(fitDebug, "Done most of it\n");
fflush(fitDebug);
/** variables used to keep track of the square error*/
double square;
int iteration = 0;
double squares[10];
for (int i = 0; i < 10; i++)
squares[i] = 0;
fprintf(fitDebug, "Starting with with algebra stuff..");
fflush(fitDebug);
int dimension = 8 * npixels;
fprintf(fitDebug, "dimension..");
fflush(fitDebug);
double *M = new double[dimension];
fprintf(fitDebug, "M..");
fflush(fitDebug);
double matrix[8 * 8];
fprintf(fitDebug, "matrix..");
fflush(fitDebug);
double vector[8];
fprintf(fitDebug, "Done with algebra stuff\n");
fflush(fitDebug);
double initial_error = 0;
double diff_x, diff_y;
double frac_x, frac_y, sig2x, sig2y, dexp;
int base;
double test;
fprintf(fitDebug, "Done with inizialization\n");
fflush(fitDebug);
/**ITERATION LOOP */
while (iteration < 1) {
/*
printf("LOOP %d\n",iteration+1);
fflush(stdout);
*/
/* fake inizialization to follow the weird MATLAB PATTERN
TOPLEFT-COLUMNWISE-STARTING DOWN */
int row = h;
int column = 1;
/** ITERATIVE PROCEDURE OVER THE IMAGE*/
fprintf(fitDebug, "Iteration: %d\n", iteration);
fflush(fitDebug);
for (int i = 0; i < npixels; i++) {
//WEIRD ! !!!I 'M USING THE ROWS AS OFFSET TO IMPLEMENT THE COLUMWISE FASHION
x = (i + 1) % h;
y = (i + 1) / h;
//WEIRD INDEX CALCULATION
int index = (row - 1) * w + (column - 1);
base = i * 8;
test = evaluateGaussian(results, x, y);
diff[i] = data[index] - test;
diff_x = x - results->x_0;
diff_y = y - results->y_0;
sig2x = pow(results->sigma_x, 2);
sig2y = pow(results->sigma_y, 2);
frac_x = pow(diff_x, 2) / sig2x;
frac_y = pow(diff_y, 2) / sig2y;
dexp = exp(frac_x + frac_y);
M[base] = 1 / dexp;
M[base + 1] = (results->A * (2 * x - 2 * results->x_0)) / (sig2x * dexp);
M[base + 2] = (results->A * (2 * y - 2 * results->y_0)) / (sig2y * dexp);
M[base + 3] = (2 * results->A * pow(diff_x, 2)) / (pow(results->sigma_x, 3) * dexp);
M[base + 4] = (2 * results->A * pow(diff_y, 2)) / (pow(results->sigma_y, 3) * dexp);
//derivative of the slopePlan !
M[base + 5] = x;
M[base + 6] = y;
M[base + 7] = 1.0;
//WEIRD CIRCULAR INCREMENT
row--;
if (row == 0) {
row = h;
column++;
}
}
square = 0.0;
/* square calculation and array adjustment */
for (int index1 = 0; index1 < npixels; index1++) {
square = square + pow(diff[index1], 2);
}
squares[iteration] = square;
iteration++;
/**initial error print*/
if (iteration == 1) {
initial_error = square;
fprintf(fitDebug, "ERRORE INIZIA: %09.0f\n", initial_error);
fflush(fitDebug);
}
fflush(stdout);
/**
FILE* fp = fopen("matrice.mat","w");
for (int index1 = 0;index1<500;index1++){
for (int index2=0;index2<8;index2++){
fprintf(fp,"%08f\t",M[index1*8+index2]);
}
fprintf(fp,"\n");
}
*/
gsl_matrix_view gsl_M = gsl_matrix_view_array(M, npixels, 8);
gsl_matrix_view matrice = gsl_matrix_view_array(matrix, 8, 8);
gsl_vector_view differenze = gsl_vector_view_array(diff, npixels);
//printf("\nMM\n");
/* Compute matrix = M'*M */
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &gsl_M.matrix, &gsl_M.matrix, 0.0, &matrice.matrix);
/**
for (int index1 = 0;index1<8;index1++){
for (int index2 =0;index2<8;index2++){
// printf("%8f\t",matrix[index1*8 + index2]);
}
// printf("\n");
}
*/
/** Compute matrix = M'*M
printf("MM2\n");
for (int index1 = 0;index1<8;index1++){
for(int index2 = 0;index2<8;index2++){
matrix[index1*8 + index2] = 0;
for (int index3 = 0;index3<npixels;index3++){
matrix[index1*8 + index2] = matrix[index1*8 + index2] + M[index3*8+index1]*M[index3*8+index2];
}
}
}
for (int index1 = 0;index1<8;index1++){
for (int index2 =0;index2<8;index2++){
printf("%08.2f\t",matrix[index1*8 + index2]);
}
printf("\n");
}
*/
//printf("\nVECTOR\n");
/* Compute vector = M'*diff for (int i =8; i<16 ;i++){ for (int
k=0;k<npixels;k++){ vector[i] = vector[i] + M[k*8 + i]*diff[k];
} } for (int index1 = 0;index1<8;index1++){
printf("%08.2f\t",vector[index1]); } printf("\n"); */
/* Compute vector = M'*diff */
gsl_blas_dgemv(CblasTrans, 1.0, &gsl_M.matrix, &differenze.vector, 0.0, vettore);
//gsl_vector_fprintf(stdout, vettore, "%g");
//printf("\n");
/* Compute the delta vector of deviation */
int s;
gsl_permutation *p = gsl_permutation_alloc(8);
gsl_linalg_LU_decomp(&matrice.matrix, p, &s);
gsl_linalg_LU_solve(&matrice.matrix, p, vettore, delta);
//printf("delta = \n");
//gsl_vector_fprintf(stdout, delta, "%g");
/** result adjustment */
results->A = results->A + gsl_vector_get(delta, 0);
//printf("New A is: %f\n", results->A);
//ADDING
results->x_0 = results->x_0 + gsl_vector_get(delta, 1);
results->y_0 = results->y_0 + gsl_vector_get(delta, 2);
results->sigma_x = results->sigma_x + gsl_vector_get(delta, 3);
results->sigma_y = results->sigma_y + gsl_vector_get(delta, 4);
results->a = results->a + gsl_vector_get(delta, 5);
results->b = results->b + gsl_vector_get(delta, 6);
results->c = results->c + +gsl_vector_get(delta, 7);
//RIPROVA ! !!
//printf("New x_0 is %f\n", results->x_0);
}
#if DEBUG
fprintf(fitDebug, "ERRORE FINALE: %09.0f\n", square);
fprintf(fitDebug, "STRUCT FINALE: \n");
printFit(fitDebug, results);
fflush(fitDebug);
#endif
//printResults(results);
/* FREE!!!!!!!! */
if (M)
delete M;
if (diff)
delete diff;
gsl_vector_free(vettore);
gsl_vector_free(delta);
if (square > initial_error) {
return 0;
} else {
return 1;
}
}
void testFileInnerProduct(){
printf("\nTest file inner product:\n");
FILE *fr;
const int maxLineLength = 20000;
char line[maxLineLength];
int totalTests = 0, successTests = 0;
time_t start_t, end_t;
double diff_t;
time(&start_t);
fr = fopen("tests/tests-c/innerProductTests.txt", "rt");
while(1){
struct StabilizerState state1;
struct StabilizerState state2;
int eps, outEps, p, outP, m, outM;
gsl_complex ans;
//populate state1:
//read n
if(!readInt(fr, line, maxLineLength, &state1.n)){
break;
}
//read k
if(!readInt(fr, line, maxLineLength, &state1.k)){
break;
}
//read Q
if(!readInt(fr, line, maxLineLength, &state1.Q)){
break;
}
//read h
double vectorDatah1[state1.n];
if(!readArray(fr, line, maxLineLength, vectorDatah1)){
break;
}
gsl_vector_view vectorViewh1 = gsl_vector_view_array(vectorDatah1, state1.n);
state1.h = &vectorViewh1.vector;
//read D
double vectorDataD1[state1.n];
if(!readArray(fr, line, maxLineLength, vectorDataD1)){
break;
}
gsl_vector_view vectorViewD1 = gsl_vector_view_array(vectorDataD1, state1.n);
state1.D = &vectorViewD1.vector;
//read G
double matrixDataG1[state1.n*state1.n];
if(!readArray(fr, line, maxLineLength, matrixDataG1)){
break;
}
gsl_matrix_view matrixViewG1 = gsl_matrix_view_array(matrixDataG1, state1.n, state1.n);
state1.G = &matrixViewG1.matrix;
//read Gbar
double matrixDataGbar1[state1.n*state1.n];
if(!readArray(fr, line, maxLineLength, matrixDataGbar1)){
break;
}
gsl_matrix_view matrixViewGbar1 = gsl_matrix_view_array(matrixDataGbar1, state1.n, state1.n);
state1.Gbar = &matrixViewGbar1.matrix;
//read J
double matrixDataJ1[state1.n*state1.n];
if(!readArray(fr, line, maxLineLength, matrixDataJ1)){
break;
}
gsl_matrix_view matrixViewJ1 = gsl_matrix_view_array(matrixDataJ1, state1.n, state1.n);
state1.J = &matrixViewJ1.matrix;
//populate state2:
//read n
if(!readInt(fr, line, maxLineLength, &state2.n)){
break;
}
//read k
if(!readInt(fr, line, maxLineLength, &state2.k)){
break;
}
//read Q
if(!readInt(fr, line, maxLineLength, &state2.Q)){
break;
}
//read h
double vectorDatah2[state2.n];
if(!readArray(fr, line, maxLineLength, vectorDatah2)){
break;
}
gsl_vector_view vectorViewh2 = gsl_vector_view_array(vectorDatah2, state2.n);
state2.h = &vectorViewh2.vector;
//read D
double vectorDataD2[state2.n];
if(!readArray(fr, line, maxLineLength, vectorDataD2)){
break;
}
gsl_vector_view vectorViewD2 = gsl_vector_view_array(vectorDataD2, state2.n);
state2.D = &vectorViewD2.vector;
//read G
double matrixDataG2[state2.n*state2.n];
if(!readArray(fr, line, maxLineLength, matrixDataG2)){
break;
}
gsl_matrix_view matrixViewG2 = gsl_matrix_view_array(matrixDataG2, state2.n, state2.n);
state2.G = &matrixViewG2.matrix;
//read Gbar
double matrixDataGbar2[state2.n*state2.n];
if(!readArray(fr, line, maxLineLength, matrixDataGbar2)){
break;
}
gsl_matrix_view matrixViewGbar2 = gsl_matrix_view_array(matrixDataGbar2, state2.n, state2.n);
state2.Gbar = &matrixViewGbar2.matrix;
//read J
double matrixDataJ2[state2.n*state2.n];
if(!readArray(fr, line, maxLineLength, matrixDataJ2)){
break;
}
gsl_matrix_view matrixViewJ2 = gsl_matrix_view_array(matrixDataJ2, state2.n, state2.n);
state2.J = &matrixViewJ2.matrix;
//read outEps
if(!readInt(fr, line, maxLineLength, &outEps)){
break;
}
//read outP
if(!readInt(fr, line, maxLineLength, &outP)){
break;
}
//read outM
if(!readInt(fr, line, maxLineLength, &outM)){
break;
}
innerProduct(&state1, &state2, &eps, &p, &m, &ans, 1);
int isEpsWorking = eps == outEps ? 1 : 0;
int isPWorking = p == outP ? 1 : 0;
int isMWorking = mod(m, 8) == mod(outM, 8) ? 1 : 0;
totalTests++;
if((eps==0&&isEpsWorking) || isEpsWorking*isPWorking*isMWorking > 0){
successTests++;
}
else{
printf("eps: %d, outEps: %d, p: %d, outP: %d, m: %d, outM: %d", eps, outEps, p, outP, m, outM);
printf("Test number %d failed.\n", totalTests);
}
/*
if (totalTests == 100) {
break;
}*/
}
fclose(fr);
printf("%d out of %d tests successful.\n", successTests, totalTests);
time(&end_t);
diff_t = difftime(end_t, start_t);
printf("Time elapsed: %f s\n", diff_t);
printf("----------------------\n");
}
void testFileMeasurePauli(){
printf("\nTest file measurePauli:\n");
FILE *fr;
const int maxLineLength = 20000;
char line[maxLineLength];
int totalTests = 0, successTests = 0;
time_t start_t, end_t;
double diff_t;
time(&start_t);
fr = fopen("tests/tests-c/measurePauliTests.txt", "rt");
while(1){
struct StabilizerState state;
int m, outk, outQ;
double outResult;
gsl_vector *zeta, *xi, *outh, *outD;
gsl_matrix *outG, *outGbar, *outJ;
//read n
if(!readInt(fr, line, maxLineLength, &state.n)){
break;
}
//read k
if(!readInt(fr, line, maxLineLength, &state.k)){
break;
}
//read Q
if(!readInt(fr, line, maxLineLength, &state.Q)){
break;
}
//read m
if(!readInt(fr, line, maxLineLength, &m)){
break;
}
//read h
double vectorDatah[state.n];
if(!readArray(fr, line, maxLineLength, vectorDatah)){
break;
}
gsl_vector_view vectorViewh = gsl_vector_view_array(vectorDatah, state.n);
state.h = &vectorViewh.vector;
//read D
double vectorDataD[state.n];
if(!readArray(fr, line, maxLineLength, vectorDataD)){
break;
}
gsl_vector_view vectorViewD = gsl_vector_view_array(vectorDataD, state.n);
state.D = &vectorViewD.vector;
//read zeta
double vectorDatazeta[state.n];
if(!readArray(fr, line, maxLineLength, vectorDatazeta)){
break;
}
gsl_vector_view vectorViewzeta = gsl_vector_view_array(vectorDatazeta, state.n);
zeta = &vectorViewzeta.vector;
//read xi
double vectorDataxi[state.n];
if(!readArray(fr, line, maxLineLength, vectorDataxi)){
break;
}
gsl_vector_view vectorViewxi = gsl_vector_view_array(vectorDataxi, state.n);
xi = &vectorViewxi.vector;
//read G
double matrixDataG[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataG)){
break;
}
gsl_matrix_view matrixViewG = gsl_matrix_view_array(matrixDataG, state.n, state.n);
state.G = &matrixViewG.matrix;
//read Gbar
double matrixDataGbar[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataGbar)){
break;
}
gsl_matrix_view matrixViewGbar = gsl_matrix_view_array(matrixDataGbar, state.n, state.n);
state.Gbar = &matrixViewGbar.matrix;
//read J
double matrixDataJ[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataJ)){
break;
}
gsl_matrix_view matrixViewJ = gsl_matrix_view_array(matrixDataJ, state.n, state.n);
state.J = &matrixViewJ.matrix;
//read outResult
if(!readDouble(fr, line, maxLineLength, &outResult)){
break;
}
//read outk
if(!readInt(fr, line, maxLineLength, &outk)){
break;
}
//read outQ
if(!readInt(fr, line, maxLineLength, &outQ)){
break;
}
//read outh
double vectorDataouth[state.n];
if(!readArray(fr, line, maxLineLength, vectorDataouth)){
break;
}
gsl_vector_view vectorViewouth = gsl_vector_view_array(vectorDataouth, state.n);
outh = &vectorViewouth.vector;
//read outD
double vectorDataoutD[state.n];
if(!readArray(fr, line, maxLineLength, vectorDataoutD)){
break;
}
gsl_vector_view vectorViewoutD = gsl_vector_view_array(vectorDataoutD, state.n);
outD = &vectorViewoutD.vector;
//read outG
double matrixDataoutG[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataoutG)){
break;
}
gsl_matrix_view matrixViewoutG = gsl_matrix_view_array(matrixDataoutG, state.n, state.n);
outG = &matrixViewoutG.matrix;
//read outGbar
double matrixDataoutGbar[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataoutGbar)){
break;
}
gsl_matrix_view matrixViewoutGbar = gsl_matrix_view_array(matrixDataoutGbar, state.n, state.n);
outGbar = &matrixViewoutGbar.matrix;
//read outJ
double matrixDataoutJ[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataoutJ)){
break;
}
gsl_matrix_view matrixViewoutJ = gsl_matrix_view_array(matrixDataoutJ, state.n, state.n);
outJ = &matrixViewoutJ.matrix;
double result = measurePauli(&state, m, zeta, xi);
int isResultWorking = result - outResult < 0.0001 ? 1 : 0;
int iskWorking = state.k == outk ? 1 : 0;
int isQWorking = state.Q == outQ ? 1 : 0;
int ishWorking = isVectorWorking(state.h, outh, state.k);
int isDWorking = isVectorWorking(state.D, outD, state.k);
int isGWorking = isMatrixWorking(state.G, outG, state.k);
int isGbarWorking = isMatrixWorking(state.Gbar, outGbar, state.k);
int isJWorking = isMatrixWorking(state.J, outJ, state.k);
totalTests++;
if(isResultWorking*iskWorking*isQWorking*ishWorking*isDWorking*isGWorking*isGbarWorking*isJWorking > 0){
successTests++;
}
else{
printf("%d %d %d %d %d %d %d %d ", isResultWorking, iskWorking, isQWorking, ishWorking, isDWorking, isGWorking, isGbarWorking, isJWorking);
printf("Test number %d failed.\n", totalTests);
}
}
fclose(fr);
printf("%d out of %d tests successful.\n", successTests, totalTests);
time(&end_t);
diff_t = difftime(end_t, start_t);
printf("Time elapsed: %f s\n", diff_t);
printf("----------------------\n");
}
/* Removes the ballistic term from the beginning of the ACF,
* just like in Omer's paper.
*/
extern void takeAwayBallistic(double *ct, double *t, int len, real tMax, int nexp, gmx_bool bDerivative)
{
/* Use nonlinear regression with GSL instead.
* Fit with 4 exponentials and one constant term,
* subtract the fatest exponential. */
int nData,i,status, iter;
balData *BD;
double *guess, /* Initial guess. */
*A, /* The fitted parameters. (A1, B1, A2, B2,... C) */
a[2],
ddt[2];
gmx_bool sorted;
size_t n;
size_t p;
nData = 0;
do {
nData++;
} while (t[nData]<tMax+t[0] && nData<len);
p = nexp*2+1; /* Number of parameters. */
#ifdef HAVE_LIBGSL
const gsl_multifit_fdfsolver_type *T
= gsl_multifit_fdfsolver_lmsder;
gsl_multifit_fdfsolver *s; /* The solver itself. */
gsl_multifit_function_fdf fitFunction; /* The function to be fitted. */
gsl_matrix *covar; /* Covariance matrix for the parameters.
* We'll not use the result, though. */
gsl_vector_view theParams;
nData = 0;
do {
nData++;
} while (t[nData]<tMax+t[0] && nData<len);
guess = NULL;
n = nData;
snew(guess, p);
snew(A, p);
covar = gsl_matrix_alloc (p, p);
/* Set up an initial gess for the parameters.
* The solver is somewhat sensitive to the initial guess,
* but this worked fine for a TIP3P box with -geminate dd
* EDIT: In fact, this seems like a good starting pont for other watermodels too. */
for (i=0; i<nexp; i++)
{
guess[i*2] = 0.1;
guess[i*2+1] = -0.5 + (((double)i)/nexp - 0.5)*0.3;
}
guess[nexp * 2] = 0.01;
theParams = gsl_vector_view_array(guess, p);
snew(BD,1);
BD->n = n;
BD->y = ct;
BD->tDelta = t[1]-t[0];
BD->nexp = nexp;
fitFunction.f = &balFunc_f;
fitFunction.df = &balFunc_df;
fitFunction.fdf = &balFunc_fdf;
fitFunction.n = nData;
fitFunction.p = p;
fitFunction.params = BD;
s = gsl_multifit_fdfsolver_alloc (T, nData, p);
if (s==NULL)
gmx_fatal(FARGS, "Could not set up the nonlinear solver.");
gsl_multifit_fdfsolver_set(s, &fitFunction, &theParams.vector);
/* \=============================================/ */
iter = 0;
do
{
iter++;
status = gsl_multifit_fdfsolver_iterate (s);
if (status)
break;
status = gsl_multifit_test_delta (s->dx, s->x, 1e-4, 1e-4);
}
while (iter < 5000 && status == GSL_CONTINUE);
if (iter == 5000)
{
fprintf(stderr, "The non-linear fitting did not converge in 5000 steps.\n"
"Check the quality of the fit!\n");
}
else
{
fprintf(stderr, "Non-linear fitting of ballistic term converged in %d steps.\n\n", (int)iter);
}
for (i=0; i<nexp; i++) {
fprintf(stdout, "%c * exp(%c * t) + ", 'A'+(char)i*2, 'B'+(char)i*2);
}
fprintf(stdout, "%c\n", 'A'+(char)nexp*2);
fprintf(stdout, "Here are the actual numbers for A-%c:\n", 'A'+nexp*2);
for (i=0; i<nexp; i++)
{
A[i*2] = gsl_vector_get(s->x, i*2);
A[i*2+1] = gsl_vector_get(s->x, i*2+1);
fprintf(stdout, " %g*exp(%g * x) +", A[i*2], A[i*2+1]);
}
A[i*2] = gsl_vector_get(s->x, i*2); /* The last and constant term */
fprintf(stdout, " %g\n", A[i*2]);
fflush(stdout);
/* Implement some check for parameter quality */
for (i=0; i<nexp; i++)
{
if (A[i*2]<0 || A[i*2]>1) {
fprintf(stderr, "WARNING: ----------------------------------\n"
" | A coefficient does not lie within [0,1].\n"
" | This may or may not be a problem.\n"
" | Double check the quality of the fit!\n");
}
if (A[i*2+1]>0) {
fprintf(stderr, "WARNING: ----------------------------------\n"
" | One factor in the exponent is positive.\n"
" | This could be a problem if the coefficient\n"
" | is large. Double check the quality of the fit!\n");
}
}
if (A[i*2]<0 || A[i*2]>1) {
fprintf(stderr, "WARNING: ----------------------------------\n"
" | The constant term does not lie within [0,1].\n"
" | This may or may not be a problem.\n"
" | Double check the quality of the fit!\n");
}
/* Sort the terms */
sorted = (nexp > 1) ? FALSE : TRUE;
while (!sorted)
{
sorted = TRUE;
for (i=0;i<nexp-1;i++)
{
ddt[0] = A[i*2] * A[i*2+1];
ddt[1] =A[i*2+2] * A[i*2+3];
if ((bDerivative && (ddt[0]<0 && ddt[1]<0 && ddt[0]>ddt[1])) || /* Compare derivative at t=0... */
(!bDerivative && (A[i*2+1] > A[i*2+3]))) /* Or just the coefficient in the exponent */
{
sorted = FALSE;
a[0] = A[i*2]; /* coefficient */
a[1] = A[i*2+1]; /* parameter in the exponent */
A[i*2] = A[i*2+2];
A[i*2+1] = A[i*2+3];
A[i*2+2] = a[0];
A[i*2+3] = a[1];
}
}
}
/* Subtract the fastest component */
fprintf(stdout, "Fastest component is %g * exp(%g * t)\n"
"Subtracting fastest component from ACF.\n", A[0], A[1]);
for (i=0; i<len; i++) {
ct[i] = (ct[i] - A[0] * exp(A[1] * i*BD->tDelta)) / (1-A[0]);
}
sfree(guess);
sfree(A);
gsl_multifit_fdfsolver_free(s);
gsl_matrix_free(covar);
fflush(stdout);
#else
/* We have no gsl. */
fprintf(stderr, "Sorry, can't take away ballistic component without gsl. "
"Recompile using --with-gsl.\n");
return;
#endif /* HAVE_LIBGSL */
}
void
test_longley ()
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (longley_n, longley_p);
gsl_multifit_robust_workspace * work_rob =
gsl_multifit_robust_alloc (gsl_multifit_robust_ols, longley_n, longley_p);
gsl_matrix_view X = gsl_matrix_view_array (longley_x, longley_n, longley_p);
gsl_vector_view y = gsl_vector_view_array (longley_y, longley_n);
gsl_vector * c = gsl_vector_alloc (longley_p);
gsl_vector * r = gsl_vector_alloc (longley_n);
gsl_matrix * cov = gsl_matrix_alloc (longley_p, longley_p);
double chisq, chisq_res;
double expected_c[7] = { -3482258.63459582,
15.0618722713733,
-0.358191792925910E-01,
-2.02022980381683,
-1.03322686717359,
-0.511041056535807E-01,
1829.15146461355 };
double expected_sd[7] = { 890420.383607373,
84.9149257747669,
0.334910077722432E-01,
0.488399681651699,
0.214274163161675,
0.226073200069370,
455.478499142212 } ;
double expected_chisq = 836424.055505915;
gsl_vector_view diag = gsl_matrix_diagonal (cov);
gsl_vector_view exp_c = gsl_vector_view_array(expected_c, longley_p);
gsl_vector_view exp_sd = gsl_vector_view_array(expected_sd, longley_p);
/* test unweighted least squares */
gsl_multifit_linear (&X.matrix, &y.vector, c, cov, &chisq, work);
gsl_multifit_linear_residuals(&X.matrix, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq_res);
test_longley_results("longley gsl_multifit_linear",
c, &exp_c.vector,
&diag.vector, &exp_sd.vector,
chisq, chisq_res, expected_chisq);
/* test robust least squares */
gsl_multifit_robust (&X.matrix, &y.vector, c, cov, work_rob);
test_longley_results("longley gsl_multifit_robust",
c, &exp_c.vector,
&diag.vector, &exp_sd.vector,
1.0, 1.0, 1.0);
/* test weighted least squares */
{
size_t i, j;
gsl_vector * w = gsl_vector_alloc (longley_n);
double expected_cov[7][7] = { { 8531122.56783558,
-166.727799925578, 0.261873708176346, 3.91188317230983,
1.1285582054705, -0.889550869422687, -4362.58709870581},
{-166.727799925578, 0.0775861253030891, -1.98725210399982e-05,
-0.000247667096727256, -6.82911920718824e-05, 0.000136160797527761,
0.0775255245956248},
{0.261873708176346, -1.98725210399982e-05, 1.20690316701888e-08,
1.66429546772984e-07, 3.61843600487847e-08, -6.78805814483582e-08,
-0.00013158719037715},
{3.91188317230983, -0.000247667096727256, 1.66429546772984e-07,
2.56665052544717e-06, 6.96541409215597e-07, -9.00858307771567e-07,
-0.00197260370663974},
{1.1285582054705, -6.82911920718824e-05, 3.61843600487847e-08,
6.96541409215597e-07, 4.94032602583969e-07, -9.8469143760973e-08,
-0.000576921112208274},
{-0.889550869422687, 0.000136160797527761, -6.78805814483582e-08,
-9.00858307771567e-07, -9.8469143760973e-08, 5.49938542664952e-07,
0.000430074434198215},
{-4362.58709870581, 0.0775255245956248, -0.00013158719037715,
-0.00197260370663974, -0.000576921112208274, 0.000430074434198215,
2.23229587481535 }} ;
gsl_vector_set_all (w, 1.0);
gsl_multifit_wlinear (&X.matrix, w, &y.vector, c, cov, &chisq, work);
gsl_multifit_linear_residuals(&X.matrix, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq_res);
test_longley_results("longley gsl_multifit_wlinear",
c, &exp_c.vector,
NULL, NULL,
chisq, chisq_res, expected_chisq);
for (i = 0; i < longley_p; i++)
{
for (j = 0; j < longley_p; j++)
{
gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-7,
"longley gsl_multifit_wlinear cov(%d,%d)", i, j) ;
}
}
gsl_vector_free(w);
}
gsl_vector_free(c);
gsl_vector_free(r);
gsl_matrix_free(cov);
gsl_multifit_linear_free (work);
gsl_multifit_robust_free (work_rob);
} /* test_longley() */
/*---------------------------------------------------------------------------
Wrapper functions to push call the approbriate Python Objects
---------------------------------------------------------------------------*/
static int
PyGSL_odeiv_func(double t, const double y[], double f[], void *params)
{
int dimension, flag = GSL_FAILURE;
PyObject *arglist = NULL, *result = NULL;
PyArrayObject *yo = NULL;
PyGSL_odeiv_step * step;
gsl_vector_view yv, fv;
PyGSL_error_info info;
FUNC_MESS_BEGIN();
step = (PyGSL_odeiv_step *) params;
if(!PyGSL_ODEIV_STEP_Check(step)){
PyGSL_add_traceback(module, this_file, __FUNCTION__,
__LINE__ - 2);
gsl_error("Param not a step type!",
this_file, __LINE__ -2, GSL_EFAULT);
goto fail;
}
dimension = step->system.dimension;
/* Do I need to copy the array ??? */
yv = gsl_vector_view_array((double *) y, dimension);
yo = PyGSL_copy_gslvector_to_pyarray(&yv.vector);
if (yo == NULL) goto fail;
FUNC_MESS("\t\tBuild args");
arglist = Py_BuildValue("(dOO)", t, yo, step->arguments);
FUNC_MESS("\t\tEnd Build args");
info.callback = step->py_func;
info.message = "odeiv_func";
result = PyEval_CallObject(step->py_func, arglist);
if((flag = PyGSL_CHECK_PYTHON_RETURN(result, 1, &info)) != GSL_SUCCESS){
goto fail;
}
info.argnum = 1;
fv = gsl_vector_view_array(f, dimension);
if((flag = PyGSL_copy_pyarray_to_gslvector(&fv.vector, result, dimension,
&info)) != GSL_SUCCESS){
goto fail;
}
Py_DECREF(arglist); arglist = NULL;
Py_DECREF(yo); yo = NULL;
Py_DECREF(result); result = NULL;
FUNC_MESS_END();
return GSL_SUCCESS;
fail:
FUNC_MESS(" IN Fail BEGIN");
Py_XDECREF(yo);
Py_XDECREF(result);
Py_XDECREF(arglist);
FUNC_MESS(" IN Fail END");
assert(flag != GSL_SUCCESS);
longjmp(step->buffer, flag);
return flag;
}
static int
PyGSL_odeiv_jac(double t, const double y[], double *dfdy, double dfdt[],
void *params)
{
int dimension, flag = GSL_FAILURE;
PyGSL_odeiv_step *step = NULL;
PyGSL_error_info info;
PyObject *arglist = NULL, *result = NULL, *tmp=NULL;
PyArrayObject *yo = NULL;
gsl_vector_view yv, dfdtv;
gsl_matrix_view dfdyv;
FUNC_MESS_BEGIN();
step = (PyGSL_odeiv_step *) params;
if(!PyGSL_ODEIV_STEP_Check(step)){
PyGSL_add_traceback(module, this_file, __FUNCTION__,
__LINE__ - 2);
gsl_error("Param not a step type!",
this_file, __LINE__ -2, GSL_EFAULT);
goto fail;
}
dimension = step->system.dimension;
yv = gsl_vector_view_array((double *) y, dimension);
yo = PyGSL_copy_gslvector_to_pyarray(&yv.vector);
if (yo == NULL) goto fail;
arglist = Py_BuildValue("(dOO)", t, yo, step->arguments);
assert(step->py_jac);
result = PyEval_CallObject(step->py_jac, arglist);
info.callback = step->py_jac;
info.message = "odeiv_jac";
if((flag = PyGSL_CHECK_PYTHON_RETURN(result, 2, &info)) != GSL_SUCCESS){
goto fail;
}
info.argnum = 1;
tmp = PyTuple_GET_ITEM(result, 0);
dfdyv = gsl_matrix_view_array((double *) dfdy, dimension, dimension);
if((flag = PyGSL_copy_pyarray_to_gslmatrix(&dfdyv.matrix, tmp, dimension, dimension, &info)) != GSL_SUCCESS){
goto fail;
}
info.argnum = 2;
tmp = PyTuple_GET_ITEM(result, 1);
dfdtv = gsl_vector_view_array((double *) dfdt, dimension);
if((flag = PyGSL_copy_pyarray_to_gslvector(&dfdtv.vector, tmp, dimension, &info)) != GSL_SUCCESS){
goto fail;
}
Py_DECREF(arglist); arglist = NULL;
Py_DECREF(result); result = NULL;
Py_DECREF(yo); yo = NULL;
FUNC_MESS_END();
return GSL_SUCCESS;
fail:
FUNC_MESS("IN Fail");
assert(flag != GSL_SUCCESS);
longjmp(step->buffer, flag);
return flag;
}
int InterpolaVPR_GSL::interpola_VPR(const float* vpr, int hvprmax, int livmin)
{
LOG_CATEGORY("radar.vpr");
static const unsigned N = 10;
const gsl_multifit_fdfsolver_type *T;
gsl_multifit_fdfsolver *s;
int status;
unsigned int i;
const size_t n = N;
const size_t p = 5;
char file_vprint[512];
gsl_matrix *covar = gsl_matrix_alloc (p, p);
double a[5];
struct data d(N);
gsl_multifit_function_fdf f;
double x_init[5] = { 4, 0.2, 3. , 1.4, -0.4 };
gsl_vector_view x = gsl_vector_view_array (x_init, p);
//////////////////////////////////////////////////////////////////////////////
int ier_int=0;
double xint,yint;
/* punti interessanti per inizializzare parametri*/
int in1=(int)((hvprmax-TCK_VPR/2)/TCK_VPR); //indice del massimo
int in2=(int)((hvprmax+HALF_BB)/TCK_VPR); //indice del massimo + 500 m
int in3=in2+1;
int in4=in2+5; //indice del massimo + 1000 m
if (in4 > NMAXLAYER-1) {
ier_int=1;
return ier_int;
}
B=vpr[in1]-vpr[in2];
E=hvprmax/1000.;
G=0.25;
C=vpr[in2-1];
F=vpr[in4]<vpr[in3]?(vpr[in4]-vpr[in3])/((in4-in3)*TCK_VPR/1000.):0.;
// fprintf(stderr, "const unsigned NMAXLAYER=%d;\n", NMAXLAYER);
// fprintf(stderr, "float vpr[] = {");
// for (unsigned i = 0; i < NMAXLAYER; ++i)
// fprintf(stderr, "%s%f", i==0?"":",", (double)vpr[i]);
// fprintf(stderr, "};\n");
x_init[0]= a[0]=B;
x_init[1]= a[1]=E;
x_init[2]= a[2]=G;
x_init[3]= a[3]=C;
x_init[4]= a[4]=F;
/////////////////////////////////////////////////////////////////////////////////////////////////////////
f.f = &expb_f;
f.df = &expb_df;
f.fdf = &expb_fdf;
f.n = n;
f.p = p;
f.params = &d;
/* This is the data to be fitted */
for (i = 0; i < n; i++)
{
d.t[i]= ((hvprmax-1000.)>livmin)? (i*TCK_VPR+(hvprmax-800)-TCK_VPR)/1000. : (livmin+i*TCK_VPR)/1000.;
d.y[i]= ((hvprmax-1000.)>livmin)? vpr[i+(int)(((hvprmax-800)-TCK_VPR)/TCK_VPR)] : vpr[i+(int)(livmin/TCK_VPR)];
d.sigma[i] = 0.5;
};
T = gsl_multifit_fdfsolver_lmsder;
s = gsl_multifit_fdfsolver_alloc (T, n, p);
gsl_multifit_fdfsolver_set (s, &f, &x.vector);
//print_state (0, s);
bool found = false;
for (unsigned iter = 0; !found && iter < 500; ++iter)
{
//fprintf(stderr, "Iter %d\n", iter);
//d.print();
int status = gsl_multifit_fdfsolver_iterate (s);
if (status != 0)
{
LOG_ERROR("gsl_multifit_fdfsolver_iterate: %s", gsl_strerror(status));
return 1;
}
//print_state (iter, s);
status = gsl_multifit_test_delta (s->dx, s->x,
1e-4, 1e-4);
switch (status)
{
case GSL_SUCCESS: found = true; break;
case GSL_CONTINUE: break;
default:
LOG_ERROR("gsl_multifit_test_delta: %s", gsl_strerror(status));
return 1;
}
}
#if GSL_MAJOR_VERSION == 2
// Use of GSL 2.0 taken from https://sft.its.cern.ch/jira/browse/ROOT-7776
gsl_matrix* J = gsl_matrix_alloc(s->fdf->n, s->fdf->p);
gsl_multifit_fdfsolver_jac(s, J);
gsl_multifit_covar(J, 0.0, covar);
#else
gsl_multifit_covar(s->J, 0.0, covar);
#endif
#define FIT(i) gsl_vector_get(s->x, i)
#define ERR(i) sqrt(gsl_matrix_get(covar,i,i))
{
double chi = gsl_blas_dnrm2(s->f);
double dof = n - p;
double c = GSL_MAX_DBL(1, chi / sqrt(dof));
// printf("chisq/dof = %g\n", pow(chi, 2.0) / dof);
// printf ("B = %.5f +/- %.5f\n", FIT(0), c*ERR(0));
// printf ("E = %.5f +/- %.5f\n", FIT(1), c*ERR(1));
// printf ("G = %.5f +/- %.5f\n", FIT(2), c*ERR(2));
// printf ("C = %.5f +/- %.5f\n", FIT(3), c*ERR(3));
// printf ("F = %.5f +/- %.5f\n", FIT(4), c*ERR(4));
}
B = a[0] = FIT(0);
E = a[1] = FIT(1);
G = a[2] = FIT(2);
C = a[3] = FIT(3);
F = a[4] = FIT(4);
gsl_multifit_fdfsolver_free (s);
gsl_matrix_free (covar);
/////////////////////////////////////////////////////////
if (testfit(a) == 1)
return 1;
for (i=1; i<=N; i++)
{
xint=(i*TCK_VPR-TCK_VPR/2)/1000.;
yint= lineargauss(xint, a);
vpr_int[i-1] = yint;
}
return 0;
}
void KFKSDS_deriv_C (int *dim, double *sy, double *sZ, double *sT, double *sH,
double *sR, double *sV, double *sQ, double *sa0, double *sP0, double *dvof,
double *epshat, double *vareps, double *etahat, double *vareta,
double *r, double *N, double *dr, double *dN,
double *dahat, double *dvareps)
{
//int s, p = dim[1], mp1 = m + 1;
int i, ip1, j, k, n = dim[0], m = dim[2],
ir = dim[3], rp1 = ir + 1, nrp1 = n * rp1,
rp1m = rp1 * m, iaux, irp1m,
irsod = ir * sizeof(double), msod = m * sizeof(double),
nsod = n * sizeof(double), rp1msod = rp1 * msod;
//double invf[n], vof[n], msHsq, dfinvfsq[nrp1];
double msHsq;
std::vector<double> invf(n);
std::vector<double> vof(n);
std::vector<double> dfinvfsq(nrp1);
gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m);
gsl_vector_view Z = gsl_vector_view_array(sZ, m);
gsl_vector * Z_cp = gsl_vector_alloc(m);
gsl_matrix * ZtZ = gsl_matrix_alloc(m, m);
gsl_matrix_view maux1, maux2;
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), m, 1);
gsl_vector_memcpy(Z_cp, &Z.vector);
maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix,
&maux2.matrix, 0.0, ZtZ);
gsl_matrix * a_pred = gsl_matrix_alloc(n, m);
std::vector<gsl_matrix*> P_pred(n);
gsl_matrix * K = gsl_matrix_alloc(n, m);
gsl_vector_view K_irow;
std::vector<gsl_matrix*> L(n);
gsl_vector_view Qdiag = gsl_matrix_diagonal(&Q.matrix);
gsl_vector * Qdiag_msq = gsl_vector_alloc(m);
gsl_vector_memcpy(Qdiag_msq, &Qdiag.vector);
gsl_vector_mul(Qdiag_msq, &Qdiag.vector);
gsl_vector_scale(Qdiag_msq, -1.0);
std::vector<gsl_matrix*> da_pred(rp1);
std::vector< std::vector<gsl_matrix*> > dP_pred(n, std::vector<gsl_matrix*>(rp1));
std::vector<gsl_matrix*> dK(n);
// filtering
KF_deriv_aux_C(dim, sy, sZ, sT, sH, sR, sV, sQ, sa0, sP0,
&invf, &vof, dvof, &dfinvfsq, a_pred, &P_pred, K,
&L, &da_pred, &dP_pred, &dK);
// state vector smoothing and disturbances smoothing
gsl_matrix_view V = gsl_matrix_view_array(sV, ir, ir);
gsl_matrix_view R = gsl_matrix_view_array(sR, m, ir);
gsl_vector_view vaux;
gsl_vector *vaux2 = gsl_vector_alloc(m);
gsl_matrix *Mmm = gsl_matrix_alloc(m, m);
gsl_matrix *Mmm2 = gsl_matrix_alloc(m, m);
gsl_matrix *Mrm = gsl_matrix_alloc(ir, m);
gsl_vector_memcpy(Z_cp, &Z.vector);
gsl_matrix *r0 = gsl_matrix_alloc(n + 1, m);
gsl_vector_view r_row_t;
gsl_vector_view r_row_tp1 = gsl_matrix_row(r0, n);
gsl_vector_set_zero(&r_row_tp1.vector);
std::vector<gsl_matrix*> N0(n + 1);
N0.at(n) = gsl_matrix_calloc(m, m);
gsl_vector_view Ndiag;
gsl_vector *var_eps = gsl_vector_alloc(n);
msHsq = -1.0 * pow(*sH, 2);
//vaux = gsl_vector_view_array(invf, n);
vaux = gsl_vector_view_array(&invf[0], n);
gsl_vector_set_all(var_eps, msHsq);
gsl_vector_mul(var_eps, &vaux.vector);
gsl_vector_add_constant(var_eps, *sH);
gsl_vector *vr = gsl_vector_alloc(ir);
gsl_matrix *dL = gsl_matrix_alloc(m, m);
std::vector<gsl_matrix*> dr0(n + 1);
dr0.at(n) = gsl_matrix_calloc(rp1, m);
gsl_vector_view dr_row_t, dr_row_tp1;
std::vector< std::vector<gsl_matrix*> > dN0(n + 1, std::vector<gsl_matrix*>(rp1));
for (j = 0; j < rp1; j++)
{
(dN0.at(n)).at(j) = gsl_matrix_calloc(m, m);
}
for (i = n-1; i > -1; i--)
{
ip1 = i + 1;
iaux = (i-1) * rp1m;
irp1m = i * rp1m;
if (i != n-1) //the case i=n-1 was initialized above
r_row_tp1 = gsl_matrix_row(r0, ip1);
r_row_t = gsl_matrix_row(r0, i);
gsl_blas_dgemv(CblasTrans, 1.0, L.at(i), &r_row_tp1.vector,
0.0, &r_row_t.vector);
gsl_vector_memcpy(Z_cp, &Z.vector);
gsl_vector_scale(Z_cp, vof.at(i));
gsl_vector_add(&r_row_t.vector, Z_cp);
gsl_vector_memcpy(vaux2, &r_row_tp1.vector);
memcpy(&r[i * m], vaux2->data, msod);
N0.at(i) = gsl_matrix_alloc(m, m);
gsl_matrix_memcpy(N0.at(i), ZtZ);
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i), N0.at(ip1), 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i), invf.at(i), N0.at(i));
vaux = gsl_matrix_diagonal(N0.at(ip1));
gsl_vector_memcpy(vaux2, &vaux.vector);
memcpy(&N[i * m], vaux2->data, msod);
K_irow = gsl_matrix_row(K, i);
gsl_blas_ddot(&K_irow.vector, &r_row_tp1.vector, &epshat[i]);
epshat[i] -= vof.at(i);
epshat[i] *= -*sH;
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), 1, m);
maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix, N0.at(ip1),
0.0, &maux2.matrix);
vaux = gsl_vector_view_array(gsl_vector_ptr(var_eps, i), 1);
gsl_blas_dgemv(CblasNoTrans, msHsq, &maux2.matrix, &K_irow.vector,
1.0, &vaux.vector);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &V.matrix, &R.matrix,
0.0, Mrm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mrm, &r_row_tp1.vector,
0.0, vr);
memcpy(&etahat[i*ir], vr->data, irsod);
Ndiag = gsl_matrix_diagonal(N0.at(ip1));
gsl_vector_memcpy(Z_cp, &Ndiag.vector);
gsl_vector_mul(Z_cp, Qdiag_msq);
gsl_vector_add(Z_cp, &Qdiag.vector);
gsl_blas_dgemv(CblasTrans, 1.0, &R.matrix, Z_cp, 0.0, vr);
memcpy(&vareta[i*ir], vr->data, irsod);
// derivatives
dr0.at(i) = gsl_matrix_alloc(rp1, m);
for (j = 0; j < rp1; j++)
{
k = i + j * n;
gsl_vector_memcpy(Z_cp, &Z.vector);
gsl_vector_scale(Z_cp, dvof[k]);
vaux = gsl_matrix_row(dK.at(i), j);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&vaux.vector, 0), m, 1);
maux2 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix,
&maux2.matrix, 0.0, dL);
dr_row_t = gsl_matrix_row(dr0.at(i), j);
dr_row_tp1 = gsl_matrix_row(dr0.at(ip1), j);
gsl_blas_dgemv(CblasTrans, 1.0, dL, &r_row_tp1.vector, 0.0, &dr_row_t.vector);
gsl_vector_add(&dr_row_t.vector, Z_cp);
gsl_blas_dgemv(CblasTrans, 1.0, L.at(i), &dr_row_tp1.vector, 1.0, &dr_row_t.vector);
(dN0.at(i)).at(j) = gsl_matrix_alloc(m, m);
gsl_matrix_memcpy((dN0.at(i)).at(j), ZtZ);
gsl_matrix_scale((dN0.at(i)).at(j), -1.0 * dfinvfsq.at(k));
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, dL, N0.at(ip1), 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i),
1.0, (dN0.at(i)).at(j));
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i),
(dN0.at(ip1)).at(j), 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i),
1.0, (dN0.at(i)).at(j));
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i),
N0.at(ip1), 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, dL,
1.0, (dN0.at(i)).at(j));
if (i != 0)
{
vaux = gsl_matrix_diagonal((dN0.at(i)).at(j));
gsl_vector_memcpy(vaux2, &vaux.vector);
memcpy(&dN[iaux + j * m], vaux2->data, msod);
}
vaux = gsl_matrix_row(da_pred.at(j), i);
gsl_blas_dgemv(CblasNoTrans, 1.0, (dP_pred.at(i)).at(j) , &r_row_t.vector,
1.0, &vaux.vector);
gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred.at(i), &dr_row_t.vector,
1.0, &vaux.vector);
gsl_vector_memcpy(vaux2, &vaux.vector);
memcpy(&dahat[irp1m + j * m], vaux2->data, msod);
gsl_matrix_memcpy(Mmm, (dP_pred.at(i)).at(j));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, (dP_pred.at(i)).at(j),
N0.at(i), 0.0, Mmm2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2, P_pred.at(i),
1.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, P_pred.at(i),
(dN0.at(i)).at(j), 0.0, Mmm2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2, P_pred.at(i),
1.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, P_pred.at(i),
N0.at(i), 0.0, Mmm2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2,
(dP_pred.at(i)).at(j), 1.0, Mmm);
gsl_matrix_mul_elements(Mmm, ZtZ);
std::vector<double> vmm(Mmm->data, Mmm->data + m*m);
dvareps[i*rp1 + j] = std::accumulate(vmm.begin(), vmm.end(), 0.0);
gsl_matrix_free((dN0.at(ip1)).at(j));
gsl_matrix_free((dP_pred.at(i)).at(j));
}
if (i != 0)
{
memcpy(&dr[iaux], (dr0.at(i))->data, rp1msod);
}
gsl_matrix_free(dr0.at(ip1));
gsl_matrix_free(dK.at(i));
gsl_matrix_free(P_pred.at(i));
gsl_matrix_free(L.at(i));
gsl_matrix_free(N0.at(ip1));
}
gsl_matrix_free(N0.at(0));
gsl_matrix_free(dr0.at(0));
for (j = 0; j < rp1; j++)
{
gsl_matrix_free((dN0.at(0)).at(j));
gsl_matrix_free(da_pred.at(j));
}
memcpy(&vareps[0], var_eps->data, nsod);
gsl_matrix_free(Mmm);
gsl_matrix_free(Mmm2);
gsl_matrix_free(Mrm);
gsl_matrix_free(r0);
gsl_matrix_free(K);
gsl_matrix_free(dL);
gsl_matrix_free(a_pred);
gsl_vector_free(Z_cp);
gsl_matrix_free(ZtZ);
gsl_vector_free(var_eps);
gsl_vector_free(vr);
gsl_vector_free(Qdiag_msq);
gsl_vector_free(vaux2);
}}
void KF_deriv_aux_C (int *dim, double *sy, double *sZ, double *sT, double *sH,
double *sR, double *sV, double *sQ, double *sa0, double *sP0,
std::vector<double> *invf, std::vector<double> *vof,
double *dvof, std::vector<double> *dfinvfsq,
gsl_matrix *a_pred, std::vector<gsl_matrix*> *P_pred,
gsl_matrix *K, std::vector<gsl_matrix*> *L,
std::vector<gsl_matrix*> *da_pred,
std::vector< std::vector<gsl_matrix*> > *dP_pred,
std::vector<gsl_matrix*> *dK)
{
//int s, p = dim[1], mp1 = m + 1;
int i, j, k, n = dim[0], m = dim[2],
jm1, r = dim[3], rp1 = r + 1;
double v, f, df, dv, dtmp;
// data and state space model matrices
gsl_vector_view Z = gsl_vector_view_array(sZ, m);
gsl_matrix_view T = gsl_matrix_view_array(sT, m, m);
gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m);
// storage vectors and matrices
gsl_vector *Vm = gsl_vector_alloc(m);
gsl_vector *Vm_cp = gsl_vector_alloc(m);
gsl_vector *Vm_cp2 = gsl_vector_alloc(m);
gsl_vector *Vm3 = gsl_vector_alloc(m);
gsl_matrix *Mmm = gsl_matrix_alloc(m, m);
gsl_matrix *M1m = gsl_matrix_alloc(1, m);
gsl_matrix *Mm1 = gsl_matrix_alloc(m, 1);
gsl_vector_view a0 = gsl_vector_view_array(sa0, m);
gsl_vector *a_upd = gsl_vector_alloc(m);
gsl_vector_memcpy(a_upd, &a0.vector);
gsl_matrix_view P0 = gsl_matrix_view_array(sP0, m, m);
gsl_matrix *P_upd = gsl_matrix_alloc(m, m);
gsl_matrix_memcpy(P_upd, &P0.matrix);
gsl_vector_view K_irow, m_irow, m2_irow, m3_irow;
gsl_matrix_view maux1;
gsl_matrix_view Zm = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m);
gsl_vector *mZ = gsl_vector_alloc(m);
gsl_vector_memcpy(mZ, &Z.vector);
gsl_vector_scale(mZ, -1.0);
std::vector<gsl_matrix*> dP_upd(rp1);
for (j = 0; j < rp1; j++)
{
da_pred[0].at(j) = gsl_matrix_alloc(n, m);
dP_upd.at(j) = gsl_matrix_calloc(m, m);
}
gsl_matrix *da_upd = gsl_matrix_calloc(rp1, m);
// filtering recursions
for (i = 0; i < n; i++)
{
m_irow = gsl_matrix_row(a_pred, i);
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, a_upd, 0.0, &m_irow.vector);
P_pred[0].at(i) = gsl_matrix_alloc(m, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, P_upd,
0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix,
0.0, P_pred[0].at(i));
gsl_matrix_add(P_pred[0].at(i), &Q.matrix);
gsl_blas_ddot(&Z.vector, &m_irow.vector, &v);
v = sy[i] - v;
gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred[0].at(i), &Z.vector,
0.0, Vm);
gsl_blas_ddot(&Z.vector, Vm, &f);
f += *sH;
gsl_vector_memcpy(Vm_cp, Vm);
gsl_vector_memcpy(Vm_cp2, Vm);
invf->at(i) = 1.0 / f;
vof->at(i) = v * invf->at(i); // v[i]/f[i];
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm, 0), m, 1);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &maux1.matrix,
&maux1.matrix, 0.0, Mmm);
gsl_matrix_scale(Mmm, invf->at(i));
gsl_vector_memcpy(a_upd, &m_irow.vector);
gsl_vector_scale(Vm, vof->at(i));
gsl_vector_add(a_upd, Vm);
gsl_matrix_memcpy(P_upd, P_pred[0].at(i));
gsl_matrix_sub(P_upd, Mmm);
K_irow = gsl_matrix_row(K, i);
gsl_vector_scale(Vm_cp, invf->at(i));
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, Vm_cp, 0.0, &K_irow.vector);
L[0].at(i) = gsl_matrix_alloc(m, m);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), m, 1);
gsl_matrix_memcpy(L[0].at(i), &T.matrix);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix,
&Zm.matrix, 1.0, L[0].at(i));
// derivatives
dK[0].at(i) = gsl_matrix_alloc(rp1, m);
for (j = 0; j < rp1; j++)
{
k = i + j * n;
m_irow = gsl_matrix_row(da_upd, j);
m2_irow = gsl_matrix_row(da_pred[0].at(j), i);
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, &m_irow.vector,
0.0, &m2_irow.vector);
gsl_blas_ddot(mZ, &m2_irow.vector, &dv);
(dP_pred[0].at(i)).at(j) = gsl_matrix_alloc(m, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, dP_upd.at(j),
0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix,
0.0, (dP_pred[0].at(i)).at(j));
if (j != 0)
{
jm1 = j - 1;
dtmp = gsl_matrix_get((dP_pred[0].at(i)).at(j), jm1, jm1);
gsl_matrix_set((dP_pred[0].at(i)).at(j), jm1, jm1, dtmp + 1.0);
}
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Zm.matrix,
(dP_pred[0].at(i)).at(j), 0.0, M1m);
m_irow = gsl_matrix_row(M1m, 0);
gsl_blas_ddot(&m_irow.vector, &Z.vector, &df);
if (j == 0) {
df += 1.0;
}
dvof[k] = (dv * f - v * df) * pow(invf->at(i), 2);
m_irow = gsl_matrix_row(da_upd, j);
gsl_blas_dgemv(CblasNoTrans, vof->at(i), (dP_pred[0].at(i)).at(j), &Z.vector,
0.0, &m_irow.vector);
gsl_vector_add(&m_irow.vector, &m2_irow.vector);
dtmp = -1.0 * df * invf->at(i);
gsl_blas_daxpy(dtmp, Vm, &m_irow.vector);
gsl_blas_daxpy(dv, Vm_cp, &m_irow.vector);
gsl_matrix_memcpy(dP_upd.at(j), (dP_pred[0].at(i)).at(j));
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, (dP_pred[0].at(i)).at(j),
&Zm.matrix, 0.0, Mm1);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mm1, &maux1.matrix,
1.0, dP_upd.at(j));
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), m, 1);
gsl_matrix_memcpy(Mm1, &maux1.matrix);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), 1, m);
dfinvfsq->at(k) = df * pow(invf->at(i), 2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, dfinvfsq->at(k), Mm1,
&maux1.matrix, 1.0, dP_upd.at(j));
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), m, 1);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix,
&Zm.matrix, 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mmm,
(dP_pred[0].at(i)).at(j), 1.0, dP_upd.at(j));
m3_irow = gsl_matrix_row(dK[0].at(i), j);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix,
(dP_pred[0].at(i)).at(j), 0.0, Mmm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, &m3_irow.vector);
gsl_vector_scale(&m3_irow.vector, invf->at(i));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix,
P_pred[0].at(i), 0.0, Mmm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, Vm3);
gsl_vector_scale(Vm3, dfinvfsq->at(k));
gsl_vector_sub(&m3_irow.vector, Vm3);
}
}
// deallocate memory
for (j = 0; j < rp1; j++)
{
gsl_matrix_free(dP_upd.at(j));
}
gsl_vector_free(mZ);
gsl_vector_free(a_upd);
gsl_matrix_free(P_upd);
gsl_vector_free(Vm);
gsl_vector_free(Vm_cp);
gsl_vector_free(Vm_cp2);
gsl_vector_free(Vm3);
gsl_matrix_free(Mmm);
gsl_matrix_free(M1m);
gsl_matrix_free(Mm1);
gsl_matrix_free(da_upd);
}
static void
nonsymmv_get_right_eigenvectors(gsl_matrix *T, gsl_matrix *Z,
gsl_vector_complex *eval,
gsl_matrix_complex *evec,
gsl_eigen_nonsymmv_workspace *w)
{
const size_t N = T->size1;
const double smlnum = GSL_DBL_MIN * N / GSL_DBL_EPSILON;
const double bignum = (1.0 - GSL_DBL_EPSILON) / smlnum;
int i; /* looping */
size_t iu, /* looping */
ju,
ii;
gsl_complex lambda; /* current eigenvalue */
double lambda_re, /* Re(lambda) */
lambda_im; /* Im(lambda) */
gsl_matrix_view Tv, /* temporary views */
Zv;
gsl_vector_view y, /* temporary views */
y2,
ev,
ev2;
double dat[4], /* scratch arrays */
dat_X[4];
double scale; /* scale factor */
double xnorm; /* |X| */
gsl_vector_complex_view ecol, /* column of evec */
ecol2;
int complex_pair; /* complex eigenvalue pair? */
double smin;
/*
* Compute 1-norm of each column of upper triangular part of T
* to control overflow in triangular solver
*/
gsl_vector_set(w->work3, 0, 0.0);
for (ju = 1; ju < N; ++ju)
{
gsl_vector_set(w->work3, ju, 0.0);
for (iu = 0; iu < ju; ++iu)
{
gsl_vector_set(w->work3, ju,
gsl_vector_get(w->work3, ju) +
fabs(gsl_matrix_get(T, iu, ju)));
}
}
for (i = (int) N - 1; i >= 0; --i)
{
iu = (size_t) i;
/* get current eigenvalue and store it in lambda */
lambda_re = gsl_matrix_get(T, iu, iu);
if (iu != 0 && gsl_matrix_get(T, iu, iu - 1) != 0.0)
{
lambda_im = sqrt(fabs(gsl_matrix_get(T, iu, iu - 1))) *
sqrt(fabs(gsl_matrix_get(T, iu - 1, iu)));
}
else
{
lambda_im = 0.0;
}
GSL_SET_COMPLEX(&lambda, lambda_re, lambda_im);
smin = GSL_MAX(GSL_DBL_EPSILON * (fabs(lambda_re) + fabs(lambda_im)),
smlnum);
smin = GSL_MAX(smin, GSL_NONSYMMV_SMLNUM);
if (lambda_im == 0.0)
{
int k, l;
gsl_vector_view bv, xv;
/* real eigenvector */
/*
* The ordering of eigenvalues in 'eval' is arbitrary and
* does not necessarily follow the Schur form T, so store
* lambda in the right slot in eval to ensure it corresponds
* to the eigenvector we are about to compute
*/
gsl_vector_complex_set(eval, iu, lambda);
/*
* We need to solve the system:
*
* (T(1:iu-1, 1:iu-1) - lambda*I)*X = -T(1:iu-1,iu)
*/
/* construct right hand side */
for (k = 0; k < i; ++k)
{
gsl_vector_set(w->work,
(size_t) k,
-gsl_matrix_get(T, (size_t) k, iu));
}
gsl_vector_set(w->work, iu, 1.0);
for (l = i - 1; l >= 0; --l)
{
size_t lu = (size_t) l;
if (lu == 0)
complex_pair = 0;
else
complex_pair = gsl_matrix_get(T, lu, lu - 1) != 0.0;
if (!complex_pair)
{
double x;
/*
* 1-by-1 diagonal block - solve the system:
*
* (T_{ll} - lambda)*x = -T_{l(iu)}
*/
Tv = gsl_matrix_submatrix(T, lu, lu, 1, 1);
bv = gsl_vector_view_array(dat, 1);
gsl_vector_set(&bv.vector, 0,
gsl_vector_get(w->work, lu));
xv = gsl_vector_view_array(dat_X, 1);
gsl_schur_solve_equation(1.0,
&Tv.matrix,
lambda_re,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
/* scale x to avoid overflow */
x = gsl_vector_get(&xv.vector, 0);
if (xnorm > 1.0)
{
if (gsl_vector_get(w->work3, lu) > bignum / xnorm)
{
x /= xnorm;
scale /= xnorm;
}
}
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
gsl_vector_set(w->work, lu, x);
if (lu > 0)
{
gsl_vector_view v1, v2;
/* update right hand side */
v1 = gsl_matrix_subcolumn(T, lu, 0, lu);
v2 = gsl_vector_subvector(w->work, 0, lu);
gsl_blas_daxpy(-x, &v1.vector, &v2.vector);
} /* if (l > 0) */
} /* if (!complex_pair) */
else
{
double x11, x21;
/*
* 2-by-2 diagonal block
*/
Tv = gsl_matrix_submatrix(T, lu - 1, lu - 1, 2, 2);
bv = gsl_vector_view_array(dat, 2);
gsl_vector_set(&bv.vector, 0,
gsl_vector_get(w->work, lu - 1));
gsl_vector_set(&bv.vector, 1,
gsl_vector_get(w->work, lu));
xv = gsl_vector_view_array(dat_X, 2);
gsl_schur_solve_equation(1.0,
&Tv.matrix,
lambda_re,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
/* scale X(1,1) and X(2,1) to avoid overflow */
x11 = gsl_vector_get(&xv.vector, 0);
x21 = gsl_vector_get(&xv.vector, 1);
if (xnorm > 1.0)
{
double beta;
beta = GSL_MAX(gsl_vector_get(w->work3, lu - 1),
gsl_vector_get(w->work3, lu));
if (beta > bignum / xnorm)
{
x11 /= xnorm;
x21 /= xnorm;
scale /= xnorm;
}
}
/* scale if necessary */
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
gsl_vector_set(w->work, lu - 1, x11);
gsl_vector_set(w->work, lu, x21);
/* update right hand side */
if (lu > 1)
{
gsl_vector_view v1, v2;
v1 = gsl_matrix_subcolumn(T, lu - 1, 0, lu - 1);
v2 = gsl_vector_subvector(w->work, 0, lu - 1);
gsl_blas_daxpy(-x11, &v1.vector, &v2.vector);
v1 = gsl_matrix_subcolumn(T, lu, 0, lu - 1);
gsl_blas_daxpy(-x21, &v1.vector, &v2.vector);
}
--l;
} /* if (complex_pair) */
} /* for (l = i - 1; l >= 0; --l) */
/*
* At this point, w->work is an eigenvector of the
* Schur form T. To get an eigenvector of the original
* matrix, we multiply on the left by Z, the matrix of
* Schur vectors
*/
ecol = gsl_matrix_complex_column(evec, iu);
y = gsl_matrix_column(Z, iu);
if (iu > 0)
{
gsl_vector_view x;
Zv = gsl_matrix_submatrix(Z, 0, 0, N, iu);
x = gsl_vector_subvector(w->work, 0, iu);
/* compute Z * w->work and store it in Z(:,iu) */
gsl_blas_dgemv(CblasNoTrans,
1.0,
&Zv.matrix,
&x.vector,
gsl_vector_get(w->work, iu),
&y.vector);
} /* if (iu > 0) */
/* store eigenvector into evec */
ev = gsl_vector_complex_real(&ecol.vector);
ev2 = gsl_vector_complex_imag(&ecol.vector);
scale = 0.0;
for (ii = 0; ii < N; ++ii)
{
double a = gsl_vector_get(&y.vector, ii);
/* store real part of eigenvector */
gsl_vector_set(&ev.vector, ii, a);
/* set imaginary part to 0 */
gsl_vector_set(&ev2.vector, ii, 0.0);
if (fabs(a) > scale)
scale = fabs(a);
}
if (scale != 0.0)
scale = 1.0 / scale;
/* scale by magnitude of largest element */
gsl_blas_dscal(scale, &ev.vector);
} /* if (GSL_IMAG(lambda) == 0.0) */
else
{
gsl_vector_complex_view bv, xv;
size_t k;
int l;
gsl_complex lambda2;
/* complex eigenvector */
/*
* Store the complex conjugate eigenvalues in the right
* slots in eval
*/
GSL_SET_REAL(&lambda2, GSL_REAL(lambda));
GSL_SET_IMAG(&lambda2, -GSL_IMAG(lambda));
gsl_vector_complex_set(eval, iu - 1, lambda);
gsl_vector_complex_set(eval, iu, lambda2);
/*
* First solve:
*
* [ T(i:i+1,i:i+1) - lambda*I ] * X = 0
*/
if (fabs(gsl_matrix_get(T, iu - 1, iu)) >=
fabs(gsl_matrix_get(T, iu, iu - 1)))
{
gsl_vector_set(w->work, iu - 1, 1.0);
gsl_vector_set(w->work2, iu,
lambda_im / gsl_matrix_get(T, iu - 1, iu));
}
else
{
gsl_vector_set(w->work, iu - 1,
-lambda_im / gsl_matrix_get(T, iu, iu - 1));
gsl_vector_set(w->work2, iu, 1.0);
}
gsl_vector_set(w->work, iu, 0.0);
gsl_vector_set(w->work2, iu - 1, 0.0);
/* construct right hand side */
for (k = 0; k < iu - 1; ++k)
{
gsl_vector_set(w->work, k,
-gsl_vector_get(w->work, iu - 1) *
gsl_matrix_get(T, k, iu - 1));
gsl_vector_set(w->work2, k,
-gsl_vector_get(w->work2, iu) *
gsl_matrix_get(T, k, iu));
}
/*
* We must solve the upper quasi-triangular system:
*
* [ T(1:i-2,1:i-2) - lambda*I ] * X = s*(work + i*work2)
*/
for (l = i - 2; l >= 0; --l)
{
size_t lu = (size_t) l;
if (lu == 0)
complex_pair = 0;
else
complex_pair = gsl_matrix_get(T, lu, lu - 1) != 0.0;
if (!complex_pair)
{
gsl_complex bval;
gsl_complex x;
/*
* 1-by-1 diagonal block - solve the system:
*
* (T_{ll} - lambda)*x = work + i*work2
*/
Tv = gsl_matrix_submatrix(T, lu, lu, 1, 1);
bv = gsl_vector_complex_view_array(dat, 1);
xv = gsl_vector_complex_view_array(dat_X, 1);
GSL_SET_COMPLEX(&bval,
gsl_vector_get(w->work, lu),
gsl_vector_get(w->work2, lu));
gsl_vector_complex_set(&bv.vector, 0, bval);
gsl_schur_solve_equation_z(1.0,
&Tv.matrix,
&lambda,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
if (xnorm > 1.0)
{
if (gsl_vector_get(w->work3, lu) > bignum / xnorm)
{
gsl_blas_zdscal(1.0/xnorm, &xv.vector);
scale /= xnorm;
}
}
/* scale if necessary */
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
wv = gsl_vector_subvector(w->work2, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
x = gsl_vector_complex_get(&xv.vector, 0);
gsl_vector_set(w->work, lu, GSL_REAL(x));
gsl_vector_set(w->work2, lu, GSL_IMAG(x));
/* update the right hand side */
if (lu > 0)
{
gsl_vector_view v1, v2;
v1 = gsl_matrix_subcolumn(T, lu, 0, lu);
v2 = gsl_vector_subvector(w->work, 0, lu);
gsl_blas_daxpy(-GSL_REAL(x), &v1.vector, &v2.vector);
v2 = gsl_vector_subvector(w->work2, 0, lu);
gsl_blas_daxpy(-GSL_IMAG(x), &v1.vector, &v2.vector);
} /* if (lu > 0) */
} /* if (!complex_pair) */
else
{
gsl_complex b1, b2, x1, x2;
/*
* 2-by-2 diagonal block - solve the system
*/
Tv = gsl_matrix_submatrix(T, lu - 1, lu - 1, 2, 2);
bv = gsl_vector_complex_view_array(dat, 2);
xv = gsl_vector_complex_view_array(dat_X, 2);
GSL_SET_COMPLEX(&b1,
gsl_vector_get(w->work, lu - 1),
gsl_vector_get(w->work2, lu - 1));
GSL_SET_COMPLEX(&b2,
gsl_vector_get(w->work, lu),
gsl_vector_get(w->work2, lu));
gsl_vector_complex_set(&bv.vector, 0, b1);
gsl_vector_complex_set(&bv.vector, 1, b2);
gsl_schur_solve_equation_z(1.0,
&Tv.matrix,
&lambda,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
x1 = gsl_vector_complex_get(&xv.vector, 0);
x2 = gsl_vector_complex_get(&xv.vector, 1);
if (xnorm > 1.0)
{
double beta;
beta = GSL_MAX(gsl_vector_get(w->work3, lu - 1),
gsl_vector_get(w->work3, lu));
if (beta > bignum / xnorm)
{
gsl_blas_zdscal(1.0/xnorm, &xv.vector);
scale /= xnorm;
}
}
/* scale if necessary */
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
wv = gsl_vector_subvector(w->work2, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
gsl_vector_set(w->work, lu - 1, GSL_REAL(x1));
gsl_vector_set(w->work, lu, GSL_REAL(x2));
gsl_vector_set(w->work2, lu - 1, GSL_IMAG(x1));
gsl_vector_set(w->work2, lu, GSL_IMAG(x2));
/* update right hand side */
if (lu > 1)
{
gsl_vector_view v1, v2, v3, v4;
v1 = gsl_matrix_subcolumn(T, lu - 1, 0, lu - 1);
v4 = gsl_matrix_subcolumn(T, lu, 0, lu - 1);
v2 = gsl_vector_subvector(w->work, 0, lu - 1);
v3 = gsl_vector_subvector(w->work2, 0, lu - 1);
gsl_blas_daxpy(-GSL_REAL(x1), &v1.vector, &v2.vector);
gsl_blas_daxpy(-GSL_REAL(x2), &v4.vector, &v2.vector);
gsl_blas_daxpy(-GSL_IMAG(x1), &v1.vector, &v3.vector);
gsl_blas_daxpy(-GSL_IMAG(x2), &v4.vector, &v3.vector);
} /* if (lu > 1) */
--l;
} /* if (complex_pair) */
} /* for (l = i - 2; l >= 0; --l) */
/*
* At this point, work + i*work2 is an eigenvector
* of T - backtransform to get an eigenvector of the
* original matrix
*/
y = gsl_matrix_column(Z, iu - 1);
y2 = gsl_matrix_column(Z, iu);
if (iu > 1)
{
gsl_vector_view x;
/* compute real part of eigenvectors */
Zv = gsl_matrix_submatrix(Z, 0, 0, N, iu - 1);
x = gsl_vector_subvector(w->work, 0, iu - 1);
gsl_blas_dgemv(CblasNoTrans,
1.0,
&Zv.matrix,
&x.vector,
gsl_vector_get(w->work, iu - 1),
&y.vector);
/* now compute the imaginary part */
x = gsl_vector_subvector(w->work2, 0, iu - 1);
gsl_blas_dgemv(CblasNoTrans,
1.0,
&Zv.matrix,
&x.vector,
gsl_vector_get(w->work2, iu),
&y2.vector);
}
else
{
gsl_blas_dscal(gsl_vector_get(w->work, iu - 1), &y.vector);
gsl_blas_dscal(gsl_vector_get(w->work2, iu), &y2.vector);
}
/*
* Now store the eigenvectors into evec - the real parts
* are Z(:,iu - 1) and the imaginary parts are
* +/- Z(:,iu)
*/
/* get views of the two eigenvector slots */
ecol = gsl_matrix_complex_column(evec, iu - 1);
ecol2 = gsl_matrix_complex_column(evec, iu);
/*
* save imaginary part first as it may get overwritten
* when copying the real part due to our storage scheme
* in Z/evec
*/
ev = gsl_vector_complex_imag(&ecol.vector);
ev2 = gsl_vector_complex_imag(&ecol2.vector);
scale = 0.0;
for (ii = 0; ii < N; ++ii)
{
double a = gsl_vector_get(&y2.vector, ii);
scale = GSL_MAX(scale,
fabs(a) + fabs(gsl_vector_get(&y.vector, ii)));
gsl_vector_set(&ev.vector, ii, a);
gsl_vector_set(&ev2.vector, ii, -a);
}
/* now save the real part */
ev = gsl_vector_complex_real(&ecol.vector);
ev2 = gsl_vector_complex_real(&ecol2.vector);
for (ii = 0; ii < N; ++ii)
{
double a = gsl_vector_get(&y.vector, ii);
gsl_vector_set(&ev.vector, ii, a);
gsl_vector_set(&ev2.vector, ii, a);
}
if (scale != 0.0)
scale = 1.0 / scale;
/* scale by largest element magnitude */
gsl_blas_zdscal(scale, &ecol.vector);
gsl_blas_zdscal(scale, &ecol2.vector);
/*
* decrement i since we took care of two eigenvalues at
* the same time
*/
--i;
} /* if (GSL_IMAG(lambda) != 0.0) */
} /* for (i = (int) N - 1; i >= 0; --i) */
} /* nonsymmv_get_right_eigenvectors() */
int SolveSVD (double a[], double b[], double x[], int neq, int nvar)
{
// get A
gsl_matrix_view av = gsl_matrix_view_array (a, neq, nvar);
if (neq < nvar) { // M < N ... do the transposed matrix
gsl_matrix *atrans = gsl_matrix_alloc (nvar, neq);
gsl_matrix_transpose_memcpy (atrans, &av.matrix);
gsl_matrix *v = gsl_matrix_alloc (neq, neq);
gsl_vector *s = gsl_vector_alloc (neq);
gsl_vector *work = gsl_vector_alloc (neq);
gsl_linalg_SV_decomp (atrans, v, s, work);
// x = A+ b
gsl_matrix *splus = gsl_matrix_calloc (neq, neq);
// compute sigma_plus
for (int i = 0; i < neq; i++) {
double sigma;
if ((sigma = gsl_vector_get (s,i)) != 0.0)
gsl_matrix_set (splus, i,i, 1.0/sigma);
}
gsl_linalg_matmult (atrans, splus, atrans);
gsl_linalg_matmult_mod (atrans, GSL_LINALG_MOD_NONE, v, GSL_LINALG_MOD_TRANSPOSE, atrans);
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_matrix_view bmv = gsl_matrix_view_vector (&bv.vector, neq, 1);
gsl_matrix *xx = gsl_matrix_alloc (nvar,1);
gsl_linalg_matmult (atrans, &bmv.matrix, xx);
// gsl_matrix_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++) {
x[i] = gsl_matrix_get(xx,i,0);
}
gsl_matrix_free (splus); gsl_matrix_free (xx);
gsl_matrix_free (atrans);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
} else { // M >= N
gsl_matrix *v = gsl_matrix_alloc (nvar, nvar);
gsl_vector *s = gsl_vector_alloc (nvar);
gsl_vector *work = gsl_vector_alloc (nvar);
gsl_linalg_SV_decomp (&av.matrix, v, s, work);
// x = A+ b
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_vector *xx = gsl_vector_alloc (nvar);
gsl_linalg_SV_solve (&av.matrix, v, s, &bv.vector, xx);
// gsl_vector_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++)
x[i] = gsl_vector_get (xx, i);
gsl_vector_free (xx);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
}
return 1;
}
void testFileShrink(){
printf("\nTest file shrink:\n");
FILE *fr;
const int maxLineLength = 20000;
char line[maxLineLength];
int totalTests = 0, successTests = 0;
time_t start_t, end_t;
double diff_t;
time(&start_t);
fr = fopen("tests/tests-c/shrinkTests.txt", "rt");
while(1){
struct StabilizerState state;
int alpha, outk, outQ, outStatus;
gsl_vector *xi, *outh, *outD;
gsl_matrix *outG, *outGbar, *outJ;
//read n
if(!readInt(fr, line, maxLineLength, &state.n)){
break;
}
//read k
if(!readInt(fr, line, maxLineLength, &state.k)){
break;
}
//read Q
if(!readInt(fr, line, maxLineLength, &state.Q)){
break;
}
//read alpha
if(!readInt(fr, line, maxLineLength, &alpha)){
break;
}
//read h
double vectorDatah[state.n];
if(!readArray(fr, line, maxLineLength, vectorDatah)){
break;
}
gsl_vector_view vectorViewh = gsl_vector_view_array(vectorDatah, state.n);
state.h = &vectorViewh.vector;
//printf("\nh: ");for(int i=0;i<state.n;i++){printf("%.0f ", gsl_vector_get(state.h, i));}
//read D
double vectorDataD[state.n];
if(!readArray(fr, line, maxLineLength, vectorDataD)){
break;
}
gsl_vector_view vectorViewD = gsl_vector_view_array(vectorDataD, state.n);
state.D = &vectorViewD.vector;
//printf("\nD: ");for(int i=0;i<state.n;i++){printf("%.0f ", gsl_vector_get(state.D, i));}
//read xi
double vectorDataxi[state.n];
if(!readArray(fr, line, maxLineLength, vectorDataxi)){
break;
}
gsl_vector_view vectorViewxi = gsl_vector_view_array(vectorDataxi, state.n);
xi = &vectorViewxi.vector;
//read G
double matrixDataG[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataG)){
break;
}
gsl_matrix_view matrixViewG = gsl_matrix_view_array(matrixDataG, state.n, state.n);
state.G = &matrixViewG.matrix;
//read Gbar
double matrixDataGbar[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataGbar)){
break;
}
gsl_matrix_view matrixViewGbar = gsl_matrix_view_array(matrixDataGbar, state.n, state.n);
state.Gbar = &matrixViewGbar.matrix;
//read J
double matrixDataJ[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataJ)){
break;
}
gsl_matrix_view matrixViewJ = gsl_matrix_view_array(matrixDataJ, state.n, state.n);
state.J = &matrixViewJ.matrix;
//read outStatus
if(!readInt(fr, line, maxLineLength, &outStatus)){
break;
}
//read outk
if(!readInt(fr, line, maxLineLength, &outk)){
break;
}
//read outQ
if(!readInt(fr, line, maxLineLength, &outQ)){
break;
}
//read outh
double vectorDataouth[state.n];
if(!readArray(fr, line, maxLineLength, vectorDataouth)){
break;
}
gsl_vector_view vectorViewouth = gsl_vector_view_array(vectorDataouth, state.n);
outh = &vectorViewouth.vector;
//read outD
double vectorDataoutD[state.n];
if(!readArray(fr, line, maxLineLength, vectorDataoutD)){
break;
}
gsl_vector_view vectorViewoutD = gsl_vector_view_array(vectorDataoutD, state.n);
outD = &vectorViewoutD.vector;
//printf("\noutD: ");for(int i=0;i<state.n;i++){printf("%.0f ", gsl_vector_get(outD, i));}
//read outG
double matrixDataoutG[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataoutG)){
break;
}
gsl_matrix_view matrixViewoutG = gsl_matrix_view_array(matrixDataoutG, state.n, state.n);
outG = &matrixViewoutG.matrix;
//read outGbar
double matrixDataoutGbar[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataoutGbar)){
break;
}
gsl_matrix_view matrixViewoutGbar = gsl_matrix_view_array(matrixDataoutGbar, state.n, state.n);
outGbar = &matrixViewoutGbar.matrix;
//read outJ
double matrixDataoutJ[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataoutJ)){
break;
}
gsl_matrix_view matrixViewoutJ = gsl_matrix_view_array(matrixDataoutJ, state.n, state.n);
outJ = &matrixViewoutJ.matrix;
int status = shrink(&state, xi, alpha, 0);
int isStatusWorking = status == outStatus ? 1 : 0;
int iskWorking = state.k == outk ? 1 : 0;
int isQWorking = state.Q == outQ ? 1 : 0;
int ishWorking = isVectorWorking(state.h, outh, state.k);
int isDWorking = isVectorWorking(state.D, outD, state.k);
int isGWorking = isMatrixWorking(state.G, outG, state.k);
int isGbarWorking = isMatrixWorking(state.Gbar, outGbar, state.k);
int isJWorking = isMatrixWorking(state.J, outJ, state.k);
//printf("%d %d %d %d %d %d %d %d ", isStatusWorking, iskWorking, isQWorking, ishWorking, isDWorking, isGWorking, isGbarWorking, isJWorking);
totalTests++;
if(isStatusWorking*iskWorking*isQWorking*ishWorking*isDWorking*isGWorking*isGbarWorking*isJWorking > 0){
successTests++;
}
else{
printf("Test number %d failed.\n", totalTests);
}
}
fclose(fr);
printf("%d out of %d tests successful.\n", successTests, totalTests);
time(&end_t);
diff_t = difftime(end_t, start_t);
printf("Time elapsed: %f s\n", diff_t);
printf("----------------------\n");
}
void
ConnectAdapter::initMUSIC(int argc, char** argv)
{
MUSIC::Setup* setup = new MUSIC::Setup (argc, argv);
setup->config("stoptime", &stoptime);
setup->config("music_timestep", ×tep);
setup->config("weights_filename", &weights_filename);
port_in = setup->publishContInput("in");
port_out = setup->publishContOutput("out");
comm = setup->communicator ();
int rank = comm.Get_rank ();
int nProcesses = comm.Get_size ();
if (nProcesses > 1)
{
std::cout << "ERROR: num processes (np) not equal 1" << std::endl;
comm.Abort(1);
}
// get dimensions of data
if (port_in->hasWidth() && port_out->hasWidth())
{
size_data_in = port_in->width();
size_data_out = port_out->width();
}
else
{
std::cout << "ERROR: Port-width not defined" << std::endl;
comm.Abort(1);
}
data_in = new double[size_data_in];
for (int i = 0; i < size_data_in; ++i)
{
data_in[i] = 0.;
}
vec_data_in = gsl_vector_view_array(data_in, size_data_in);
data_out = new double[size_data_out];
for (int i = 0; i < size_data_out; ++i)
{
data_out[i] = 0.;
}
vec_data_out = gsl_vector_view_array(data_out, size_data_out);
weights = new double[size_data_out * size_data_in];
for (int i = 0; i < size_data_out * size_data_in; ++i)
{
weights[i] = 0.;
}
mat_weights = gsl_matrix_view_array(weights, size_data_out, size_data_in);
// Declare where in memory to put command_data
MUSIC::ArrayData dmap_in(data_in,
MPI::DOUBLE,
0,
size_data_in);
port_in->map (&dmap_in, 0., 1, false);
MUSIC::ArrayData dmap_out(data_out,
MPI::DOUBLE,
0,
size_data_out);
port_out ->map (&dmap_out, 1);
MPI::COMM_WORLD.Barrier();
runtime = new MUSIC::Runtime (setup, timestep);
}
void testFileExtend(){
printf("\nTest file extend:\n");
FILE *fr;
const int maxLineLength = 20000;
char line[maxLineLength];
int totalTests = 0, successTests = 0;
time_t start_t, end_t;
double diff_t;
time(&start_t);
fr = fopen("tests/tests-c/extendTests.txt", "rt");
while(1){
struct StabilizerState state;
int outk;
gsl_vector *xi;
gsl_matrix *outG, *outGbar;
//read n
if(!readInt(fr, line, maxLineLength, &state.n)){
break;
}
//read k
if(!readInt(fr, line, maxLineLength, &state.k)){
break;
}
//read xi
double vectorDataxi[state.n];
if(!readArray(fr, line, maxLineLength, vectorDataxi)){
break;
}
gsl_vector_view vectorViewxi = gsl_vector_view_array(vectorDataxi, state.n);
xi = &vectorViewxi.vector;
//read G
double matrixDataG[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataG)){
break;
}
gsl_matrix_view matrixViewG = gsl_matrix_view_array(matrixDataG, state.n, state.n);
state.G = &matrixViewG.matrix;
//read Gbar
double matrixDataGbar[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataGbar)){
break;
}
gsl_matrix_view matrixViewGbar = gsl_matrix_view_array(matrixDataGbar, state.n, state.n);
state.Gbar = &matrixViewGbar.matrix;
//read outk
if(!readInt(fr, line, maxLineLength, &outk)){
break;
}
//read outG
double matrixDataoutG[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataoutG)){
break;
}
gsl_matrix_view matrixViewoutG = gsl_matrix_view_array(matrixDataoutG, state.n, state.n);
outG = &matrixViewoutG.matrix;
//read outGbar
double matrixDataoutGbar[state.n*state.n];
if(!readArray(fr, line, maxLineLength, matrixDataoutGbar)){
break;
}
gsl_matrix_view matrixViewoutGbar = gsl_matrix_view_array(matrixDataoutGbar, state.n, state.n);
outGbar = &matrixViewoutGbar.matrix;
extend(&state, xi);
int iskWorking = state.k == outk ? 1 : 0;
int isGWorking = isMatrixWorking(state.G, outG, state.k);
int isGbarWorking = isMatrixWorking(state.Gbar, outGbar, state.k);
//printf("%d %d %d %d %d %d %d %d ", isStatusWorking, iskWorking, isQWorking, ishWorking, isDWorking, isGWorking, isGbarWorking, isJWorking);
totalTests++;
if(iskWorking*isGWorking*isGbarWorking > 0){
successTests++;
}
else{
printf("Test number %d failed.\n", totalTests);
}
}
fclose(fr);
printf("%d out of %d tests successful.\n", successTests, totalTests);
time(&end_t);
diff_t = difftime(end_t, start_t);
printf("Time elapsed: %f s\n", diff_t);
printf("----------------------\n");
}
static int init(void * vstate, const double xa[], const double ya[], size_t size)
{
struct state_t * state = (struct state_t *) vstate;
switch (size)
{
case 2:
{
double A[16];
double x[4];
double b[4];
/* Set up the system */
{
int i;
double h = xa[1] - xa[0];
for (i=0; i<16; i++)
A[i] = 0.0;
/* Zeroth-order, all points have the same value */
/* a = y0 */
A[0*4+0] = 1.0;
b[0] = ya[0];
/* a, b, c, d = y1 */
A[1*4+0] = 1.0;
A[1*4+1] = h;
A[1*4+2] = h*h;
A[1*4+3] = h*h*h;
b[1] = ya[1];
/* The last two equations depend on the endpoint types */
if (state->ltype == BT_INTERP_SLOPE)
{
/* b = alpha */
A[2*4+1] = 1.0;
b[2] = state->alpha;
}
else /* state->ltype == WAM_INTERP_NATURAL */
{
/* c = 0 */
A[2*4+2] = 2.0;
b[2] = 0.0;
}
if (state->rtype == BT_INTERP_SLOPE)
{
/* b, c, d = beta */
A[3*4+1] = 1.0;
A[3*4+2] = 2*h;
A[3*4+3] = 3*h*h;
b[3] = state->beta;
}
else /* state->ltype == WAM_INTERP_NATURAL */
{
/* c, d = 0 */
A[3*4+2] = 2.0;
A[3*4+3] = 6*h;
b[3] = 0.0;
}
}
/* Solve the system */
{
gsl_matrix_view A_view = gsl_matrix_view_array(A,4,4);
gsl_vector_view b_view = gsl_vector_view_array(b,4);
gsl_vector_view x_view = gsl_vector_view_array(x,4);
gsl_permutation * p = gsl_permutation_alloc(4);
int s;
gsl_linalg_LU_decomp( &A_view.matrix, p, &s);
gsl_linalg_LU_solve( &A_view.matrix, p, &b_view.vector, &x_view.vector );
gsl_permutation_free(p);
}
/* Save the solved values */
state->s_2p->a = x[0];
state->s_2p->b = x[1];
state->s_2p->c = x[2];
state->s_2p->d = x[3];
} break;
case 3:
{
double A[64];
double x[8];
double b[8];
/* Set up the system */
{
int i;
double hL = xa[1] - xa[0];
double hR = xa[2] - xa[1];
for (i=0; i<64; i++)
A[i] = 0.0;
/* Zeroth-order, all points have the same value */
/* aL = y0 */
A[0*8+0] = 1.0;
b[0] = ya[0];
/* aL, bL, cL, dL = y1 */
A[1*8+0] = 1.0;
A[1*8+1] = hL;
A[1*8+2] = hL*hL;
A[1*8+3] = hL*hL*hL;
b[1] = ya[1];
/* aR = y1 */
A[2*8+4] = 1.0;
b[2] = ya[1];
/* aR, bR, cR, dR = y2 */
A[3*8+4] = 1.0;
A[3*8+5] = hR;
A[3*8+6] = hR*hR;
A[3*8+7] = hR*hR*hR;
b[3] = ya[2];
/* First-order, the slopes are the same at the midpoint */
/* bL, cL, dL, -bR = 0 */
A[4*8+1] = 1.0;
A[4*8+2] = 2*hL;
A[4*8+3] = 3*hL*hL;
A[4*8+5] = -1.0;
b[4] = 0.0;
/* Second-order, the concavity is the same at the midpoint */
/* cL, dL, -cR = 0 */
A[5*8+2] = 2.0;
A[5*8+3] = 6*hL;
A[5*8+6] = -2.0;
b[5] = 0.0;
/* The last two equations depend on the endpoint types */
if (state->ltype == BT_INTERP_SLOPE)
{
/* bL = alpha */
A[6*8+1] = 1.0;
b[6] = state->alpha;
}
else /* state->ltype == WAM_INTERP_NATURAL */
{
/* cL = 0 */
A[6*8+2] = 2.0;
b[6] = 0.0;
}
if (state->rtype == BT_INTERP_SLOPE)
{
/* bR, cR, dR = beta */
A[7*8+5] = 1.0;
A[7*8+6] = 2*hR;
A[7*8+7] = 3*hR*hR;
b[7] = state->beta;
}
else /* state->rtype == WAM_INTERP_NATURAL */
{
/* cR, dR = 0 */
A[7*8+6] = 2.0;
A[7*8+7] = 6*hL;
b[7] = 0.0;
}
}
/* Solve the system */
{
gsl_matrix_view A_view = gsl_matrix_view_array(A,8,8);
gsl_vector_view b_view = gsl_vector_view_array(b,8);
gsl_vector_view x_view = gsl_vector_view_array(x,8);
gsl_permutation * p = gsl_permutation_alloc(8);
int s;
gsl_linalg_LU_decomp( &A_view.matrix, p, &s);
gsl_linalg_LU_solve( &A_view.matrix, p, &b_view.vector, &x_view.vector );
gsl_permutation_free(p);
}
/* Save the solved values */
state->s_3p->aL = x[0];
state->s_3p->bL = x[1];
state->s_3p->cL = x[2];
state->s_3p->dL = x[3];
state->s_3p->aR = x[4];
state->s_3p->bR = x[5];
state->s_3p->cR = x[6];
state->s_3p->dR = x[7];
} break;
default:
{
/* spline calculation with natural boundary conditions
* or with defined first and/or last derivatives
* see [Engeln-Mullges + Uhlig, p. 258]
*/
/* Note - this is mostly duplication. Oh well. */
size_t i;
size_t num_points = size;
size_t max_index = num_points - 1; /* Engeln-Mullges + Uhlig "n" */
size_t sys_size = max_index - 1; /* linear system is sys_size x sys_size */
/* Note - moved outer c setting to below */
/* Set up the system for the inner c's */
for (i = 0; i < sys_size; i++)
{
const double h_i = xa[i + 1] - xa[i];
const double h_ip1 = xa[i + 2] - xa[i + 1];
const double ydiff_i = ya[i + 1] - ya[i];
const double ydiff_ip1 = ya[i + 2] - ya[i + 1];
const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
state->cstate->offdiag[i] = h_ip1;
/* added in here ######### */
if (state->ltype == BT_INTERP_SLOPE && i==0)
{
state->cstate->diag[i] = 1.5 * h_i + 2.0 * h_ip1;
state->cstate->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - 0.5 * ( 3.0*(ydiff_i*g_i) - state->alpha ) );
continue;
}
if (state->rtype == BT_INTERP_SLOPE && i == sys_size-1)
{
state->cstate->diag[i] = 2.0 * h_i + 1.5 * h_ip1;
state->cstate->g[i] = 3.0 * ( 0.5 * ( 3.0*(ydiff_ip1*g_ip1) - state->beta ) - ydiff_i * g_i );
continue;
}
/* ####################### */
state->cstate->diag[i] = 2.0 * (h_ip1 + h_i);
state->cstate->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - ydiff_i * g_i);
}
/* Solve the system for the inner c's */
{
gsl_vector_view g_vec = gsl_vector_view_array(state->cstate->g, sys_size);
gsl_vector_view diag_vec = gsl_vector_view_array(state->cstate->diag, sys_size);
gsl_vector_view offdiag_vec = gsl_vector_view_array(state->cstate->offdiag, sys_size - 1);
gsl_vector_view solution_vec = gsl_vector_view_array ((state->cstate->c) + 1, sys_size);
int status = gsl_linalg_solve_symm_tridiag(&diag_vec.vector,
&offdiag_vec.vector,
&g_vec.vector,
&solution_vec.vector);
if (status != GSL_SUCCESS) return status;
}
/* Set the outer c's. */
if (state->ltype == BT_INTERP_SLOPE)
{
const double h_0 = xa[1] - xa[0];
const double ydiff_0 = ya[1] - ya[0];
state->cstate->c[0] = 1.0/(2.0*h_0) * ( (3.0/h_0)*ydiff_0 - 3.0*state->alpha - state->cstate->c[1]*h_0 );
}
else
state->cstate->c[0] = 0.0;
if (state->rtype == BT_INTERP_SLOPE)
{
const double h_nm1 = xa[max_index] - xa[max_index-1];
const double ydiff_nm1 = ya[max_index] - ya[max_index-1];
state->cstate->c[max_index] = - 1.0/(2.0*h_nm1) * ( (3.0/h_nm1)*ydiff_nm1 - 3.0*state->beta + state->cstate->c[max_index-1]*h_nm1 );
}
else
state->cstate->c[max_index] = 0.0;
} break;
}
return GSL_SUCCESS;
}
void testFileExponentialSum(){
printf("\nTest file exponentialSum:\n");
FILE *fr;
const int maxLineLength = 20000;
char line[maxLineLength];
int totalTests = 0, successTests = 0;
time_t start_t, end_t;
double diff_t;
time(&start_t);
fr = fopen("tests/tests-c/exponentialSumTests.txt", "rt");
while(1){
struct StabilizerState state;
int eps, outEps, p, outP, m, outM;
gsl_complex ans;
//read n
if(!readInt(fr, line, maxLineLength, &state.n)){
break;
}
double vectorData[state.n];
double matrixData[state.n*state.n];
gsl_vector_view vectorView;
gsl_matrix_view matrixView;
//read k
if(!readInt(fr, line, maxLineLength, &state.k)){
break;
}
//read Q
if(!readInt(fr, line, maxLineLength, &state.Q)){
break;
}
//read D
if(!readArray(fr, line, maxLineLength, vectorData)){
break;
}
vectorView = gsl_vector_view_array(vectorData, state.n);
state.D = &vectorView.vector;
//read J
if(!readArray(fr, line, maxLineLength, matrixData)){
break;
}
matrixView = gsl_matrix_view_array(matrixData, state.n, state.n);
state.J = &matrixView.matrix;
//read outEps
if(!readInt(fr, line, maxLineLength, &outEps)){
break;
}
//read outP
if(!readInt(fr, line, maxLineLength, &outP)){
break;
}
//read outM
if(!readInt(fr, line, maxLineLength, &outM)){
break;
}
exponentialSum(&state, &eps, &p, &m, &ans, 1);
int isEpsWorking = eps == outEps ? 1 : 0;
int isPWorking = p == outP ? 1 : 0;
int isMWorking = mod(m, 8) == mod(outM, 8) ? 1 : 0;
totalTests++;
if(isEpsWorking*isPWorking*isMWorking > 0){
successTests++;
}
else{
printf("Test number %d failed.\n", totalTests);
}
}
fclose(fr);
printf("%d out of %d tests successful.\n", successTests, totalTests);
time(&end_t);
diff_t = difftime(end_t, start_t);
printf("Time elapsed: %f s\n", diff_t);
printf("----------------------\n");
}
/**
* \brief A variant of the Savitzky-Golay algorithm able to handle non-uniformly distributed data.
*
* In comparison to smoothSavGol(), this method trades proper handling of the X coordinates for
* runtime efficiency by abandoning a central idea of Savitzky-Golay algorithm, namely that
* polynomial smoothing can be expressed as a convolution.
*
* TODO: integrate this option into the GUI.
*/
void SmoothFilter::smoothModifiedSavGol(double *x_in, double *y_inout)
{
// total number of points in smoothing window
int points = d_left_points + d_right_points + 1;
if (points < d_polynom_order+1) {
QMessageBox::critical((ApplicationWindow *)parent(), tr("SciDAVis") + " - " + tr("Error"),
tr("The polynomial order must be lower than the number of left points plus the number of right points!"));
return;
}
// allocate memory for the result
QVector<double> result(d_n);
// allocate memory for the linear algegra computations
// Vandermonde matrix for x values of points in the current smoothing window
gsl_matrix *vandermonde = gsl_matrix_alloc(points, d_polynom_order+1);
// stores part of the QR decomposition of vandermonde
gsl_vector *tau = gsl_vector_alloc(qMin(points, d_polynom_order+1));
// coefficients of polynomial approximation computed for each smoothing window
gsl_vector *poly = gsl_vector_alloc(d_polynom_order+1);
// residual of the (least-squares) approximation (by-product of GSL's algorithm)
gsl_vector *residual = gsl_vector_alloc(points);
for (int target_index = 0; target_index < d_n; target_index++) {
int offset = target_index - d_left_points;
// use a fixed number of points; near left/right borders, use offset to change
// effective number of left/right points considered
if (target_index < d_left_points)
offset += d_left_points - target_index;
else if (target_index + d_right_points >= d_n)
offset += d_n - 1 - (target_index + d_right_points);
// fill Vandermonde matrix
for (int i = 0; i < points; ++i) {
gsl_matrix_set(vandermonde, i, 0, 1.0);
for (int j = 1; j <= d_polynom_order; ++j)
gsl_matrix_set(vandermonde, i, j, gsl_matrix_get(vandermonde,i,j-1) * x_in[offset + i]);
}
// Y values within current smoothing window
gsl_vector_view y_slice = gsl_vector_view_array(y_inout+offset, points);
// compute QR decomposition of Vandermonde matrix
if (int error=gsl_linalg_QR_decomp(vandermonde, tau))
QMessageBox::critical((ApplicationWindow *)parent(), tr("SciDAVis") + " - " + tr("Error"),
tr("Internal error in Savitzky-Golay algorithm: QR decomposition failed.\n")
+ gsl_strerror(error));
// least-squares-solve vandermonde*poly=y_slice using the QR decomposition now stored in
// vandermonde and tau
else if (int error=gsl_linalg_QR_lssolve(vandermonde, tau, &y_slice.vector, poly, residual))
QMessageBox::critical((ApplicationWindow *)parent(), tr("SciDAVis") + " - " + tr("Error"),
tr("Internal error in Savitzky-Golay algorithm: least-squares solution failed.\n")
+ gsl_strerror(error));
else
result[target_index] = gsl_poly_eval(poly->data, d_polynom_order+1, x_in[target_index]);
}
// deallocate memory
gsl_vector_free(residual);
gsl_vector_free(poly);
gsl_vector_free(tau);
gsl_matrix_free(vandermonde);
// write result into *y_inout
qCopy(result.begin(), result.end(), y_inout);
}
//------------------------------------------------------------------------------
// findCorrection () : Uses a GSL Levenberg-Marquardt algorithm to fit the lines
// in FittedLines to the wavenumbers in the user-specified calibration standard.
// The result is the optimal wavenumber correction factor for the uncalibrated
// data, which is stored in the class variable WaveCorrection. Information about
// the fit residuals are saved by calling calcDiffStats().
//
void ListCal::findCorrection () {
// Prepare the GSL Solver and associated objects. A non-linear solver is used,
// the precise type of which is determined by SOLVER_TYPE, defined in
// MgstFcn.h.
const size_t NumParameters = 1;
const size_t NumLines = FittedLines.size ();
double GuessArr [NumParameters];
for (unsigned int i = 0; i < NumParameters; i ++) { GuessArr[i] = WaveCorrection; }
const gsl_multifit_fdfsolver_type *SolverType;
gsl_multifit_fdfsolver *Solver;
gsl_multifit_function_fdf FitFunction;
gsl_matrix *Covariance = gsl_matrix_alloc (NumParameters, NumParameters);
gsl_vector_view VectorView = gsl_vector_view_array (GuessArr, NumParameters);
FitFunction.f = &fitFn;
FitFunction.df = &derivFn;
FitFunction.fdf = &fitAndDerivFns;
FitFunction.n = NumLines;
FitFunction.p = NumParameters;
FitFunction.params = &FittedLines;
SolverType = SOLVER_TYPE;
Solver = gsl_multifit_fdfsolver_alloc(SolverType, NumLines, NumParameters);
gsl_multifit_fdfsolver_set (Solver, &FitFunction, &VectorView.vector);
// Perform the fitting, one iteration at a time until one of the following
// conditions is met: The absolute and relative changes in the fit parameters
// become smaller than SOLVER_TOL, or the max number of allowed iterations,
// SOLVER_MAX_ITERATIONS, is reached.
unsigned int Iteration = 0;
int Status;
do {
Iteration ++;
Status = gsl_multifit_fdfsolver_iterate (Solver);
if (Status) break;
Status = gsl_multifit_test_delta (Solver->dx, Solver->x, SOLVER_TOL, SOLVER_TOL);
} while (Status == GSL_CONTINUE && Iteration < SOLVER_MAX_ITERATIONS);
// Output all the fit parameters with their associated error.
gsl_multifit_covar (Solver -> J, 0.0, Covariance);
#define FIT(i) gsl_vector_get (Solver -> x, i)
#define ERR(i) sqrt (gsl_matrix_get (Covariance, i, i))
double chi = gsl_blas_dnrm2 (Solver -> f);
double dof = NumLines - double(NumParameters);
double c = chi / sqrt (dof);
cout << "Correction factor: " << FIT(0) << " +/- " << c*ERR(0) << " ("
<< "reduced chi^2 = " << pow(chi, 2) / dof << ", "
<< "lines fitted = " << NumLines << ", c = " << c << ")" << endl;
// Apply the wavenumber correction to all the lines loaded from the
// uncalibrated spectrum
WaveCorrection = FIT(0);
WaveCorrectionError = c*ERR(0);
calcDiffStats ();
cout << "dSig/Sig Mean Residual: " << DiffMean / LC_DATA_SCALE
<< ", StdDev: " << DiffStdDev / LC_DATA_SCALE
<< ", StdErr: " << DiffStdErr / LC_DATA_SCALE << endl;
// Clean up the memory and exit
gsl_multifit_fdfsolver_free (Solver);
gsl_matrix_free (Covariance);
}
void sci_2_gsl_vector (gsl_vector *v,const double * sci_vector, int n){
gsl_vector_view vv=gsl_vector_view_array ((double * )&(sci_vector[1]), n);
gsl_vector_memcpy(v,&vv.vector);
}
int
gsl_eigen_hermv (gsl_matrix_complex * A, gsl_vector * eval,
gsl_matrix_complex * evec,
gsl_eigen_hermv_workspace * w)
{
if (A->size1 != A->size2)
{
GSL_ERROR ("matrix must be square to compute eigenvalues", GSL_ENOTSQR);
}
else if (eval->size != A->size1)
{
GSL_ERROR ("eigenvalue vector must match matrix size", GSL_EBADLEN);
}
else if (evec->size1 != A->size1 || evec->size2 != A->size1)
{
GSL_ERROR ("eigenvector matrix must match matrix size", GSL_EBADLEN);
}
else
{
const size_t N = A->size1;
double *const d = w->d;
double *const sd = w->sd;
size_t a, b;
/* handle special case */
if (N == 1)
{
gsl_complex A00 = gsl_matrix_complex_get (A, 0, 0);
gsl_vector_set (eval, 0, GSL_REAL(A00));
gsl_matrix_complex_set (evec, 0, 0, GSL_COMPLEX_ONE);
return GSL_SUCCESS;
}
/* Transform the matrix into a symmetric tridiagonal form */
{
gsl_vector_view d_vec = gsl_vector_view_array (d, N);
gsl_vector_view sd_vec = gsl_vector_view_array (sd, N - 1);
gsl_vector_complex_view tau_vec = gsl_vector_complex_view_array (w->tau, N-1);
gsl_linalg_hermtd_decomp (A, &tau_vec.vector);
gsl_linalg_hermtd_unpack (A, &tau_vec.vector, evec, &d_vec.vector, &sd_vec.vector);
}
/* Make an initial pass through the tridiagonal decomposition
to remove off-diagonal elements which are effectively zero */
chop_small_elements (N, d, sd);
/* Progressively reduce the matrix until it is diagonal */
b = N - 1;
while (b > 0)
{
if (sd[b - 1] == 0.0 || isnan(sd[b - 1]))
{
b--;
continue;
}
/* Find the largest unreduced block (a,b) starting from b
and working backwards */
a = b - 1;
while (a > 0)
{
if (sd[a - 1] == 0.0)
{
break;
}
a--;
}
{
size_t i;
const size_t n_block = b - a + 1;
double *d_block = d + a;
double *sd_block = sd + a;
double * const gc = w->gc;
double * const gs = w->gs;
/* apply QR reduction with implicit deflation to the
unreduced block */
qrstep (n_block, d_block, sd_block, gc, gs);
/* Apply Givens rotation Gij(c,s) to matrix Q, Q <- Q G */
for (i = 0; i < n_block - 1; i++)
{
const double c = gc[i], s = gs[i];
size_t k;
for (k = 0; k < N; k++)
{
gsl_complex qki = gsl_matrix_complex_get (evec, k, a + i);
gsl_complex qkj = gsl_matrix_complex_get (evec, k, a + i + 1);
/* qki <= qki * c - qkj * s */
/* qkj <= qki * s + qkj * c */
gsl_complex x1 = gsl_complex_mul_real(qki, c);
gsl_complex y1 = gsl_complex_mul_real(qkj, -s);
gsl_complex x2 = gsl_complex_mul_real(qki, s);
gsl_complex y2 = gsl_complex_mul_real(qkj, c);
gsl_complex qqki = gsl_complex_add(x1, y1);
gsl_complex qqkj = gsl_complex_add(x2, y2);
gsl_matrix_complex_set (evec, k, a + i, qqki);
gsl_matrix_complex_set (evec, k, a + i + 1, qqkj);
}
}
/* remove any small off-diagonal elements */
chop_small_elements (n_block, d_block, sd_block);
}
}
{
gsl_vector_view d_vec = gsl_vector_view_array (d, N);
gsl_vector_memcpy (eval, &d_vec.vector);
}
return GSL_SUCCESS;
}
}
void PlaneDetector::solve()
{
// prepare calculation data
DetectionData baseData;
// prepare gsl variables
const gsl_multifit_fdfsolver_type *T;
gsl_multifit_fdfsolver *s;
int n = (int)data.size();
#ifdef VECTOR_SOLVER
const int p = 4;
double x_init[p] = {0.0, 0.0, 1.0, 0.0};
#endif
#ifdef ANGLE_SOLVER
const int p = 3;
// double x_init[p] = {0.0, 0.0, 0.0};
double x_init[p] = {0.0, 90.0 / 180.0 * M_PI, 0.0};
#endif
#ifdef VECTOR2_SOLVER
const int p = 6;
// double x_init[p] = {1.0, 0.0, 0.0, 500.0, 0.0, 0.0};
double x_init[p] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
#endif
#ifdef ANGLE2_SOLVER
const int p = 5;
double x_init[p] = {0.0, M_PI / 2.0 + 0.1, 000.0, 0.0, 0.0};
#endif
gsl_multifit_function_fdf f;
f.f = &pd_func_f;
f.df = &pd_func_df;
f.fdf = &pd_func_fdf;
f.n = n;
f.p = p;
f.params = &data;
gsl_vector_view x = gsl_vector_view_array(x_init, p);
T = gsl_multifit_fdfsolver_lmsder;
s = gsl_multifit_fdfsolver_alloc(T, n, p);
gsl_multifit_fdfsolver_set(s, &f, &x.vector);
gsl_vector *gradt = gsl_vector_alloc(p);
int iter = 0;
int status;
do
{
iter ++;
status = gsl_multifit_fdfsolver_iterate(s);
if (status)
{
// printf ("error: %s\n", gsl_strerror (status));
break;
}
gsl_multifit_gradient(s->J, s->f, gradt);
status = gsl_multifit_test_gradient(gradt, 1e-6);
} while (status == GSL_CONTINUE && iter < 10000);
#ifdef VECTOR_SOLVER
vN = Vector(
gsl_vector_get(s->x, 0),
gsl_vector_get(s->x, 1),
gsl_vector_get(s->x, 2)).getUnitVector();
fD = gsl_vector_get(s->x, 3);
#endif
#ifdef ANGLE_SOLVER
double theta = gsl_vector_get(s->x, 0);
double pi = gsl_vector_get(s->x, 1);
vN = Vector(sin(pi)*cos(theta), sin(pi)*sin(theta), cos(pi));
double d = gsl_vector_get(s->x, 2);
fD = - ((vN & vListener) + d);
#endif
#ifdef VECTOR2_SOLVER
vN = Vector(
gsl_vector_get(s->x, 0),
gsl_vector_get(s->x, 1),
gsl_vector_get(s->x, 2)).getUnitVector();
Vector vX1 = Vector(
gsl_vector_get(s->x, 3),
gsl_vector_get(s->x, 4),
gsl_vector_get(s->x, 5));
fD = -(vN & vX1);
#endif
#ifdef ANGLE2_SOLVER
double theta = gsl_vector_get(s->x, 0);
double pi = gsl_vector_get(s->x, 1);
vN = Vector(sin(pi)*cos(theta), sin(pi)*sin(theta), cos(pi));
Vector vX1 = Vector(
gsl_vector_get(s->x, 2),
gsl_vector_get(s->x, 3),
gsl_vector_get(s->x, 4));
fD = -(vN & vX1);
#endif
#if 0
for (double pp = 0; pp < M_PI; pp += 0.1)
{
for (double tt = 0; tt < M_PI; tt += 0.1)
{
Vector vNN = Vector(sin(pp)*cos(tt), sin(pp)*sin(tt), cos(pp));
printf("vN = ");
vNN.print();
double sum = 0;
for (size_t i = 0; i < data.size(); i++)
{
sum += pow(data[i].getF(tt, pp, fD), 2);
}
printf(" , sum = %f\n", sum);
}
}
#endif
#if 0
double sqrsum = 0;
for (size_t i = 0; i < data.size(); i++)
{
#ifdef VECTOR_SOLVER
sqrsum += pow(data[i].getF(vN, fD), 2);
#endif
#ifdef ANGLE_SOLVER
sqrsum += pow(data[i].getF(theta, pi, fD), 2);
#endif
#ifdef VECTOR2_SOLVER
sqrsum += pow(data[i].getF(vN, fD), 2);
#endif
// printf ("%f/", sqrsum);
}
vN.print();
printf(" %f %f\n", fD, sqrt(sqrsum));
#endif
// for (size_t i = 0; i < data.size(); i++)
// printf ("F_%d = %f\n", i, data[i].getF(vN));
gsl_vector_free(gradt);
gsl_multifit_fdfsolver_free(s);
vP.clear();
for (size_t i = 0; i < data.size(); i++)
{
vP.push_back(data[i].getP(vN));
}
}
// GICP cost function
double GICPOptimizer::f(const gsl_vector *x, void *params) {
GICPOptData *opt_data = (GICPOptData *)params;
double pt1[3];
double pt2[3];
double res[3]; // residual
double temp[3];
gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3);
gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3);
gsl_vector_view gsl_res = gsl_vector_view_array(res, 3);
gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3);
gsl_matrix_view gsl_M;
dgc_transform_t t;
// initialize the temp variable; if it happens to be NaN at start, bad things will happen in blas routines below
temp[0] = 0;
temp[1] = 0;
temp[2] = 0;
// take the base transformation
dgc_transform_copy(t, opt_data->base_t);
// apply the current state
apply_state(t, x);
double f = 0;
double temp_double = 0;
int N = opt_data->p1->Size();
int counter = 0;
for(int i = 0; i < N; i++) {
int j = opt_data->nn_indecies[i];
if(j != -1) {
// get point 1
pt1[0] = (*opt_data->p1)[i].x;
pt1[1] = (*opt_data->p1)[i].y;
pt1[2] = (*opt_data->p1)[i].z;
// get point 2
pt2[0] = (*opt_data->p2)[j].x;
pt2[1] = (*opt_data->p2)[j].y;
pt2[2] = (*opt_data->p2)[j].z;
//get M-matrix
gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3);
//transform point 1
dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t);
res[0] = pt1[0] - pt2[0];
res[1] = pt1[1] - pt2[1];
res[2] = pt1[2] - pt2[2];
//cout << "res: (" << res[0] << ", " <<res[1] <<", " << res[2] << ")" << endl;
// temp := M*res
gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector);
// temp_double := res'*temp = temp'*M*temp
gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double);
// increment total error
f += temp_double/(double)opt_data->num_matches;
//cout << "temp: " << temp_double << endl;
//cout << "f: " << f << "\t (" << opt_data->num_matches << ")" << endl;
//print_gsl_matrix(&gsl_M.matrix, "M");
counter++;
}
}
printf("counter %d\n",counter);
return f;
}
static void
test_fdf(const gsl_multifit_nlinear_type * T,
const gsl_multifit_nlinear_parameters * params,
const double xtol, const double gtol, const double ftol,
const double epsrel,
test_fdf_problem *problem,
const double *wts)
{
gsl_multifit_nlinear_fdf *fdf = problem->fdf;
const size_t n = fdf->n;
const size_t p = fdf->p;
const size_t max_iter = 2500;
gsl_vector *x0 = gsl_vector_alloc(p);
gsl_vector_view x0v = gsl_vector_view_array(problem->x0, p);
gsl_multifit_nlinear_workspace *w =
gsl_multifit_nlinear_alloc (T, params, n, p);
const char *pname = problem->name;
char buf[2048];
char sname[2048];
int status, info;
sprintf(buf, "%s/%s/scale=%s/solver=%s",
gsl_multifit_nlinear_name(w),
params->trs->name,
params->scale->name,
params->solver->name);
if (problem->fdf->df == NULL)
{
if (params->fdtype == GSL_MULTIFIT_NLINEAR_FWDIFF)
strcat(buf, "/fdjac,forward");
else
strcat(buf, "/fdjac,center");
}
if (problem->fdf->fvv == NULL)
{
strcat(buf, "/fdfvv");
}
strcpy(sname, buf);
gsl_vector_memcpy(x0, &x0v.vector);
if (wts)
{
gsl_vector_const_view wv = gsl_vector_const_view_array(wts, n);
gsl_multifit_nlinear_winit(x0, &wv.vector, fdf, w);
}
else
gsl_multifit_nlinear_init(x0, fdf, w);
status = gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,
NULL, NULL, &info, w);
gsl_test(status, "%s/%s did not converge, status=%s",
sname, pname, gsl_strerror(status));
/* check solution */
test_fdf_checksol(sname, pname, epsrel, w, problem);
if (wts == NULL)
{
/* test again with weighting matrix W = I */
gsl_vector *wv = gsl_vector_alloc(n);
sprintf(sname, "%s/weighted", buf);
gsl_vector_memcpy(x0, &x0v.vector);
gsl_vector_set_all(wv, 1.0);
gsl_multifit_nlinear_winit(x0, wv, fdf, w);
status = gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,
NULL, NULL, &info, w);
gsl_test(status, "%s/%s did not converge, status=%s",
sname, pname, gsl_strerror(status));
test_fdf_checksol(sname, pname, epsrel, w, problem);
gsl_vector_free(wv);
}
gsl_multifit_nlinear_free(w);
gsl_vector_free(x0);
}
int
main (void)
{
const gsl_multifit_fdfsolver_type *T = gsl_multifit_fdfsolver_lmsder;
gsl_multifit_fdfsolver *s;
int status, info;
size_t i;
const size_t n = N;
const size_t p = 3;
gsl_matrix *J = gsl_matrix_alloc(n, p);
gsl_matrix *covar = gsl_matrix_alloc (p, p);
double y[N], weights[N];
struct data d = { n, y };
gsl_multifit_function_fdf f;
double x_init[3] = { 1.0, 0.0, 0.0 };
gsl_vector_view x = gsl_vector_view_array (x_init, p);
gsl_vector_view w = gsl_vector_view_array(weights, n);
const gsl_rng_type * type;
gsl_rng * r;
gsl_vector *res_f;
double chi, chi0;
const double xtol = 1e-8;
const double gtol = 1e-8;
const double ftol = 0.0;
gsl_rng_env_setup();
type = gsl_rng_default;
r = gsl_rng_alloc (type);
f.f = &expb_f;
f.df = &expb_df; /* set to NULL for finite-difference Jacobian */
f.n = n;
f.p = p;
f.params = &d;
/* This is the data to be fitted */
for (i = 0; i < n; i++)
{
double t = i;
double yi = 1.0 + 5 * exp (-0.1 * t);
double si = 0.1 * yi;
double dy = gsl_ran_gaussian(r, si);
weights[i] = 1.0 / (si * si);
y[i] = yi + dy;
printf ("data: %zu %g %g\n", i, y[i], si);
};
s = gsl_multifit_fdfsolver_alloc (T, n, p);
/* initialize solver with starting point and weights */
gsl_multifit_fdfsolver_wset (s, &f, &x.vector, &w.vector);
/* compute initial residual norm */
res_f = gsl_multifit_fdfsolver_residual(s);
chi0 = gsl_blas_dnrm2(res_f);
/* solve the system with a maximum of 20 iterations */
status = gsl_multifit_fdfsolver_driver(s, 20, xtol, gtol, ftol, &info);
gsl_multifit_fdfsolver_jac(s, J);
gsl_multifit_covar (J, 0.0, covar);
/* compute final residual norm */
chi = gsl_blas_dnrm2(res_f);
#define FIT(i) gsl_vector_get(s->x, i)
#define ERR(i) sqrt(gsl_matrix_get(covar,i,i))
fprintf(stderr, "summary from method '%s'\n",
gsl_multifit_fdfsolver_name(s));
fprintf(stderr, "number of iterations: %zu\n",
gsl_multifit_fdfsolver_niter(s));
fprintf(stderr, "function evaluations: %zu\n", f.nevalf);
fprintf(stderr, "Jacobian evaluations: %zu\n", f.nevaldf);
fprintf(stderr, "reason for stopping: %s\n",
(info == 1) ? "small step size" : "small gradient");
fprintf(stderr, "initial |f(x)| = %g\n", chi0);
fprintf(stderr, "final |f(x)| = %g\n", chi);
{
double dof = n - p;
double c = GSL_MAX_DBL(1, chi / sqrt(dof));
fprintf(stderr, "chisq/dof = %g\n", pow(chi, 2.0) / dof);
fprintf (stderr, "A = %.5f +/- %.5f\n", FIT(0), c*ERR(0));
fprintf (stderr, "lambda = %.5f +/- %.5f\n", FIT(1), c*ERR(1));
fprintf (stderr, "b = %.5f +/- %.5f\n", FIT(2), c*ERR(2));
}
fprintf (stderr, "status = %s\n", gsl_strerror (status));
gsl_multifit_fdfsolver_free (s);
gsl_matrix_free (covar);
gsl_matrix_free (J);
gsl_rng_free (r);
return 0;
}
void pv_step2(int exists, double ** seeds, int seeds_size, double ** bg, struct data * d, int n_param)
{
//variables generales del programa
int i, j;
double H, eta;
double * shift_eta = vector_double_alloc((*d).numrings);
//variables del solver
int status = 0;
const gsl_multifit_fdfsolver_type * T;
gsl_multifit_fdfsolver * s;
gsl_multifit_function_fdf pv; //funcion a fitear
//gsl_matrix * covar = gsl_matrix_alloc (n_param, n_param);//matriz covariante
double * x_init = vector_double_alloc(n_param);
//printf("Inicializando los parametros\n");
j = 0;
for(i = 2; i < seeds_size; i += 4)
{
x_init[j] = seeds[1][i]; j++; //theta_0
x_init[j] = seeds[1][i + 1]; j++; //Intensity
x_init[j] = seeds[1][i + 2]; j++; //Shift_H
}
for(i = 0; i < (*d).n_bg; i++)
{
x_init[j] = bg[1][i]; j++;
}
gsl_vector_view x = gsl_vector_view_array (x_init, n_param); //inicializo el vector con los datos a fitear
//Estructura con los parametros fijos del fiteo
H = seeds[1][0];
eta = seeds[exists][1];
j = 0;
for(i = 2; i < seeds_size; i += 4)
{
shift_eta[j] = seeds[exists][i + 3]; j++;
}
data_s2 d2 = {*d, H, eta, shift_eta};
//inicializo la funcion pseudo-voigt
pv.f = &pv_f_step2; //definicion de la funcion
pv.df = NULL; //al apuntar la funcion con el jacobiano de la funcion a NULL, hago que la derivada de la funcion se calcule por el metodo de diferencias finitas
pv.fdf = NULL; //idem anterior
pv.n = (*d).n; //numero de puntos experimentales
pv.p = n_param; //variables a fitear (debe cumplir <= pv.n)
pv.params = &d2; //parametros fijos
//inicializo el solver
T = gsl_multifit_fdfsolver_lmsder;
s = gsl_multifit_fdfsolver_alloc (T, (*d).n, n_param);
gsl_multifit_fdfsolver_set (s, &pv, &x.vector);
solver_iterator(&status, s, T);
//printf("Salida de los resultados del paso 2\n");
j = 0;
for(i = 2; i < seeds_size; i += 4)
{
seeds[1][i] = gsl_vector_get(s -> x, j); j++; //theta_0
seeds[1][i + 1] = gsl_vector_get(s -> x, j); j++; //Intensity
seeds[1][i + 2] = gsl_vector_get(s -> x, j); j++; //Shift_H
}
for(i = 0; i < (*d).n_bg; i++)
{
bg[1][i] = gsl_vector_get(s -> x, j); j++;
}
free(x_init);
gsl_multifit_fdfsolver_free (s);
//printf("Final del paso 2\n");
}
