void xmi_inverse_matrix(double x[3], double y[3], double z[3], double **inverseF) {
gsl_matrix *m = gsl_matrix_alloc(3,3);
gsl_matrix *inverse = gsl_matrix_alloc(3,3);
gsl_permutation *p = gsl_permutation_calloc(3);
int signum;
double *rv;
int i,j,k;
gsl_matrix_set(m,0,0, x[0]);
gsl_matrix_set(m,1,0, x[1]);
gsl_matrix_set(m,2,0, x[2]);
gsl_matrix_set(m,0,1, y[0]);
gsl_matrix_set(m,1,1, y[1]);
gsl_matrix_set(m,2,1, y[2]);
gsl_matrix_set(m,0,2, z[0]);
gsl_matrix_set(m,1,2, z[1]);
gsl_matrix_set(m,2,2, z[2]);
#if DEBUG == 2
fprintf(stdout,"input matrix\n");
gsl_matrix_fprintf(stdout,m , "%g");
#endif
//invert the sucker
gsl_linalg_LU_decomp(m,p,&signum);
gsl_linalg_LU_invert(m,p,inverse);
#if DEBUG == 2
fprintf(stdout,"inverted matrix\n");
gsl_matrix_fprintf(stdout,inverse , "%g");
#endif
gsl_matrix_transpose(inverse);
#if DEBUG == 2
fprintf(stdout,"transposed inverted matrix\n");
gsl_matrix_fprintf(stdout,inverse , "%g");
#endif
rv = (double *) malloc(9*sizeof(double));
k = 0;
for (i = 0 ; i < 3 ; i++)
for (j = 0 ; j < 3 ; j++)
rv[k++] = gsl_matrix_get(inverse, i,j);
gsl_matrix_free(m);
gsl_matrix_free(inverse);
gsl_permutation_free(p);
*inverseF = rv;
}
void printf_matrix(const char* filename, gsl_matrix * m)
{
FILE* fileptr;
fileptr = fopen(filename, "w");
gsl_matrix_fprintf(fileptr, m, "%f");
fclose(fileptr);
}
int
CRebuildGraph::calculateCommunicability(const char *argv[]){
CFuncTrace lFuncTrace(false,"fCalculateConnectivity");
gslGraph *targetGraph=NULL;
// targetGraph=GetGraphfromFile(argv[1]);
int graphOrder=targetGraph->getOrder();
lFuncTrace.trace(CTrace::TRACE_DEBUG,"Graph Order %d",graphOrder);
// Get Numpy Matrix // Matriu d'adjacencia
gsl_matrix *A1=gsl_matrix_calloc(graphOrder,graphOrder);
//targetGraph->printGraph();
targetGraph->graphToGsl(A1);
lFuncTrace.trace(CTrace::TRACE_DEBUG,"Printing Home made Matrix\n");
printGslMatrix(A1);
lFuncTrace.trace(CTrace::TRACE_DEBUG,"Printing with gsl function\n");
gsl_matrix_fprintf(stdout, A1, "%g");
gsl_vector_complex *eval = calculateEgeinval(A1);
/*gsl_vector *evalexp = */ calculateExp(eval);
return RESULT_OK;
}
void population_fprintf (const Population *pop, FILE *stream)
{
int i;
for (i = 0; i < pop->n_e; i++){
gsl_vector_fprintf(stream, pop->y[i], "%f");
gsl_matrix_fprintf(stream, pop->b[i], "%f");
}
fprintf(stream, "%d", pop->current_gen);
}
void mtx_fprintf(const char* filename, const gsl_matrix * m) {
// outlog( "writing %ld x %ld matrix to %s",
// m->size1, m->size2, filename);
FILE* fileptr;
fileptr = fopen(filename, "w");
if (!fileptr) {
outlog("Error opening file: %s", filename);
exit(1);
}
gsl_matrix_fprintf(fileptr, m, "%20.17e");
fclose(fileptr);
}
static int
iterate (void *vstate, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
{
lmder_state_t *state = (lmder_state_t *) vstate;
gsl_matrix *r = state->r;
gsl_vector *tau = state->tau;
gsl_vector *diag = state->diag;
gsl_vector *qtf = state->qtf;
gsl_vector *x_trial = state->x_trial;
gsl_vector *f_trial = state->f_trial;
gsl_vector *rptdx = state->rptdx;
gsl_vector *newton = state->newton;
gsl_vector *gradient = state->gradient;
gsl_vector *sdiag = state->sdiag;
gsl_vector *w = state->w;
gsl_vector *work1 = state->work1;
gsl_permutation *perm = state->perm;
double prered, actred;
double pnorm, fnorm1, fnorm1p, gnorm;
double ratio;
double dirder;
int iter = 0;
double p1 = 0.1, p25 = 0.25, p5 = 0.5, p75 = 0.75, p0001 = 0.0001;
if (state->fnorm == 0.0)
{
return GSL_SUCCESS;
}
/* Compute qtf = Q^T f */
gsl_vector_memcpy (qtf, f);
gsl_linalg_QR_QTvec (r, tau, qtf);
/* Compute norm of scaled gradient */
compute_gradient_direction (r, perm, qtf, diag, gradient);
{
size_t iamax = gsl_blas_idamax (gradient);
gnorm = fabs(gsl_vector_get (gradient, iamax) / state->fnorm);
}
/* Determine the Levenberg-Marquardt parameter */
lm_iteration:
iter++ ;
{
int status = lmpar (r, perm, qtf, diag, state->delta, &(state->par), newton, gradient, sdiag, dx, w);
if (status)
return status;
}
/* Take a trial step */
gsl_vector_scale (dx, -1.0); /* reverse the step to go downhill */
compute_trial_step (x, dx, state->x_trial);
pnorm = scaled_enorm (diag, dx);
if (state->iter == 1)
{
if (pnorm < state->delta)
{
#ifdef DEBUG
printf("set delta = pnorm = %g\n" , pnorm);
#endif
state->delta = pnorm;
}
}
/* Evaluate function at x + p */
/* return immediately if evaluation raised error */
{
int status = GSL_MULTIFIT_FN_EVAL_F (fdf, x_trial, f_trial);
if (status)
return status;
}
fnorm1 = enorm (f_trial);
/* Compute the scaled actual reduction */
actred = compute_actual_reduction (state->fnorm, fnorm1);
#ifdef DEBUG
printf("lmiterate: fnorm = %g fnorm1 = %g actred = %g\n", state->fnorm, fnorm1, actred);
printf("r = "); gsl_matrix_fprintf(stdout, r, "%g");
printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
printf("dx = "); gsl_vector_fprintf(stdout, dx, "%g");
#endif
/* Compute rptdx = R P^T dx, noting that |J dx| = |R P^T dx| */
compute_rptdx (r, perm, dx, rptdx);
#ifdef DEBUG
printf("rptdx = "); gsl_vector_fprintf(stdout, rptdx, "%g");
#endif
fnorm1p = enorm (rptdx);
/* Compute the scaled predicted reduction = |J dx|^2 + 2 par |D dx|^2 */
{
double t1 = fnorm1p / state->fnorm;
double t2 = (sqrt(state->par) * pnorm) / state->fnorm;
prered = t1 * t1 + t2 * t2 / p5;
dirder = -(t1 * t1 + t2 * t2);
}
/* compute the ratio of the actual to predicted reduction */
if (prered > 0)
{
ratio = actred / prered;
}
else
{
ratio = 0;
}
#ifdef DEBUG
printf("lmiterate: prered = %g dirder = %g ratio = %g\n", prered, dirder,ratio);
#endif
/* update the step bound */
if (ratio > p25)
{
#ifdef DEBUG
printf("ratio > p25\n");
#endif
if (state->par == 0 || ratio >= p75)
{
state->delta = pnorm / p5;
state->par *= p5;
#ifdef DEBUG
printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par);
#endif
}
}
else
{
double temp = (actred >= 0) ? p5 : p5*dirder / (dirder + p5 * actred);
#ifdef DEBUG
printf("ratio < p25\n");
#endif
if (p1 * fnorm1 >= state->fnorm || temp < p1 )
{
temp = p1;
}
state->delta = temp * GSL_MIN_DBL (state->delta, pnorm/p1);
state->par /= temp;
#ifdef DEBUG
printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par);
#endif
}
/* test for successful iteration, termination and stringent tolerances */
if (ratio >= p0001)
{
gsl_vector_memcpy (x, x_trial);
gsl_vector_memcpy (f, f_trial);
/* return immediately if evaluation raised error */
{
int status;
if (fdf->df)
status = GSL_MULTIFIT_FN_EVAL_DF (fdf, x_trial, J);
else
status = gsl_multifit_fdfsolver_dif_df(x_trial, fdf, f_trial, J);
if (status)
return status;
}
/* wa2_j = diag_j * x_j */
state->xnorm = scaled_enorm(diag, x);
state->fnorm = fnorm1;
state->iter++;
/* Rescale if necessary */
if (scale)
{
update_diag (J, diag);
}
{
int signum;
gsl_matrix_memcpy (r, J);
gsl_linalg_QRPT_decomp (r, tau, perm, &signum, work1);
}
return GSL_SUCCESS;
}
else if (fabs(actred) <= GSL_DBL_EPSILON && prered <= GSL_DBL_EPSILON
&& p5 * ratio <= 1.0)
{
return GSL_ETOLF ;
}
else if (state->delta <= GSL_DBL_EPSILON * state->xnorm)
{
return GSL_ETOLX;
}
else if (gnorm <= GSL_DBL_EPSILON)
{
return GSL_ETOLG;
}
else if (iter < 10)
{
/* Repeat inner loop if unsuccessful */
goto lm_iteration;
}
return GSL_ENOPROG;
}
void print_tilt()
{
if (!initialized) return;
gsl_matrix_fprintf(stdout, rm_ax_tilt, "%f");
}
void mtx_fprintf(const char* filename, const gsl_matrix * m) {
FILE* fileptr;
fileptr = fopen(filename, "w");
gsl_matrix_fprintf(fileptr, m, "%20.17e");
fclose(fileptr);
}
/*
in matlab or octave
orig = [2.580000 -3.100000 4.250000
> 3.821000 4.440000 5.656000
> 1.820000 7.410000 3.330000]
orig =
2.5800 -3.1000 4.2500
3.8210 4.4400 5.6560
1.8200 7.4100 3.3300
octave:4> trans = orig'
trans =
2.5800 3.8210 1.8200
-3.1000 4.4400 7.4100
4.2500 5.6560 3.3300
*/
int main (void) {
int i, j;
FILE *opointer;
opointer = fopen("initial.txt","w");
FILE *tpointer;
tpointer = fopen("transpose.txt","w");
char *ofmt;
ofmt = "%f";
printf("The output file format ofmt %s \n",ofmt);
printf("will be used in gsl_matrix_fprintf (opointer, m, ofmt)\n");
gsl_matrix * m = gsl_matrix_alloc (3, 3);
gsl_matrix_set (m, 0, 0, 2.58);
gsl_matrix_set (m, 0, 1, -3.1);
gsl_matrix_set (m, 0, 2, 4.25);
gsl_matrix_set (m, 1, 0, 3.821);
gsl_matrix_set (m, 1, 1, 4.44);
gsl_matrix_set (m, 1, 2, 5.656);
gsl_matrix_set (m, 2, 0, 1.82);
gsl_matrix_set (m, 2, 1, 7.41);
gsl_matrix_set (m, 2, 2, 3.33);
/*
Function: int gsl_matrix_fprintf (FILE * stream, const gsl_matrix * m, const char * format)
This function writes the elements of the matrix m line-by-line to the stream stream using the format specifier format, which should be one of the %g, %e or %f formats for floating point numbers and %d for integers. The function returns 0 for success and GSL_EFAILED if there was a problem writing to the file.
*/
gsl_matrix_fprintf (opointer, m, ofmt);
printf ("Initial test matrice \n");
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
printf ("m(%d,%d) = %g\n", i, j,
gsl_matrix_get (m, i, j));
}
}
printf ("transpose of initial matrice \n");
printf ("the matrice needs to be square\n");
printf ("%x\n",*m);
printf ("sizeof of struct m %d \n",sizeof(*m));
printf("num of rows %d\n",m->size1);
printf("num of cols %d\n",m->size2);
if (m->size1 == m->size2) {
gsl_matrix_transpose (m);
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
printf ("m(%d,%d) = %g\n", i, j,
gsl_matrix_get (m, i, j));
}
}
gsl_matrix_fprintf (tpointer, m, ofmt);
}
else printf("the matrice is not square can not transpose\n");
gsl_matrix_set_identity (m);
printf ("The identity matrice \n");
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
printf ("m(%d,%d) = %g\n", i, j,
gsl_matrix_get (m, i, j));
}
}
gsl_matrix_free (m);
return 0;
}
// for debugging
void dump_matrix(gsl_matrix *m, char *filename) {
FILE *f = fopen(filename, "w");
gsl_matrix_fprintf(f, m, "%.16g");
fclose(f);
}
int main() {
srand(time(NULL));
int states = 6;
int unfiltered_states = 3;
gsl_matrix *state_mean = gsl_matrix_calloc(states,1);
gsl_matrix_set(state_mean, 0,0, 3);
gsl_matrix_set(state_mean, 1,0, 3);
gsl_matrix *state_covariance = gsl_matrix_calloc(states,states);
gsl_matrix *observation_mean = gsl_matrix_calloc(states,1);
gsl_matrix *observation_covariance = gsl_matrix_calloc(states,states);
//gsl_matrix_set(observation_covariance, 0, 0, hdop);
//gsl_matrix_set(observation_covariance, 1, 1, hdop);
//gsl_matrix_set(observation_covariance, 2, 2, vert_err);
//gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2));
gsl_matrix *observation_transformation = gsl_matrix_calloc(states,states);
gsl_matrix *estimate_mean = gsl_matrix_calloc(states,1);
gsl_matrix *estimate_covariance = gsl_matrix_calloc(states,states);
gsl_matrix *kalman_gain = gsl_matrix_calloc(states,states);
gsl_matrix *temp21a = gsl_matrix_calloc(states,1);
gsl_matrix *temp21b = gsl_matrix_calloc(states,1);
gsl_matrix *temp22a = gsl_matrix_calloc(states,states);
gsl_matrix *temp22b = gsl_matrix_calloc(states,states);
gsl_matrix *predict = gsl_matrix_calloc(states,states);
//gsl_matrix_set(predict, 0, 0, 1);
//gsl_matrix_set(predict, 1, 1, 1);
//gsl_matrix_set(predict, 2, 2, 1);
gsl_matrix_set(predict, 3, 3, 1);
gsl_matrix_set(predict, 4, 4, 1);
gsl_matrix_set(predict, 5, 5, 1);
//gsl_matrix_set(predict, 0, 3, timestep);
//gsl_matrix_set(predict, 1, 4, timestep);
//gsl_matrix_set(predict, 2, 5, timestep);
gsl_matrix *control = gsl_matrix_calloc(states, unfiltered_states);
//gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2));
//gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2));
//gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2));
//gsl_matrix_set(control, 3, 0, timestep);
//gsl_matrix_set(control, 4, 1, timestep);
//gsl_matrix_set(control, 5, 2, timestep);
gsl_vector *acceleration = gsl_vector_calloc(unfiltered_states);
gsl_vector *velocity = gsl_vector_calloc(unfiltered_states);
gsl_vector *location = gsl_vector_calloc(unfiltered_states);
gsl_vector *delta_location = gsl_vector_calloc(unfiltered_states);
gsl_vector *temp_location = gsl_vector_calloc(unfiltered_states);
double timestep;
int hdop;
int vert_err;
int s;
double error;
for (int t = 1; t < 1000000; t++) {
timestep = 1;
gsl_vector_set(acceleration, 0, (rand()%101)-52);
gsl_vector_set(acceleration, 1, (rand()%101)-46);
gsl_vector_set(acceleration, 2, (rand()%101)-54);
gsl_vector_add(velocity, acceleration);
gsl_vector_add(location, velocity);
gsl_vector_memcpy(delta_location, location);
gsl_vector_sub(delta_location, temp_location);
gsl_vector_memcpy(temp_location, location);
gsl_matrix_set(predict, 0, 3, timestep);
gsl_matrix_set(predict, 1, 4, timestep);
gsl_matrix_set(predict, 2, 5, timestep);
gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2));
gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2));
gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2));
gsl_matrix_set(control, 3, 0, timestep);
gsl_matrix_set(control, 4, 1, timestep);
gsl_matrix_set(control, 5, 2, timestep);
hdop = 5;
vert_err = 5;
gsl_matrix_set(observation_covariance, 0, 0, hdop);
gsl_matrix_set(observation_covariance, 1, 1, hdop);
gsl_matrix_set(observation_covariance, 2, 2, vert_err);
gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2));
//observation_transformation = observation_mean[k] * pseudoinverse(state_mean[k-1])
gsl_matrix_set_zero(temp21a);
gsl_matrix_set(temp21a, 0, 0, gsl_vector_get(delta_location,0));
gsl_matrix_set(temp21a, 1, 0, gsl_vector_get(delta_location,1));
gsl_matrix_set(temp21a, 2, 0, gsl_vector_get(delta_location,2));
gsl_matrix_set(temp21a, 3, 0, gsl_vector_get(velocity,0));
gsl_matrix_set(temp21a, 4, 0, gsl_vector_get(velocity,1));
gsl_matrix_set(temp21a, 5, 0, gsl_vector_get(velocity,2));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, temp21a, pseudo_inverse(state_mean), 0, observation_transformation);
//observation_mean[k] = observation_transformation * state_mean[k-1]
gsl_matrix_memcpy(temp21a, state_mean);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp21a, 0, observation_mean);
//observation_covariance[k] = observation_transformation * state_covariance * transpose(observation_transformation)
/* gsl_matrix_set_zero(temp22a);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, observation_transformation, 0, temp22a); //notice observation_transformation is transposed
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp22a, 0, observation_covariance);
*/
gsl_matrix_set(observation_covariance, 0, 3, timestep*3);
gsl_matrix_set(observation_covariance, 3, 0, timestep*3);
gsl_matrix_set(observation_covariance, 1, 4, timestep*5);
gsl_matrix_set(observation_covariance, 4, 1, timestep*5);
gsl_matrix_set(observation_covariance, 2, 5, timestep*6);
gsl_matrix_set(observation_covariance, 5, 2, timestep*6);
//estimate_mean = predict * state_mean + control * acceleration;
gsl_matrix_set_zero(estimate_mean);
gsl_matrix_set_zero(temp21a);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, state_mean, 0, estimate_mean);
gsl_matrix_view test = gsl_matrix_view_vector(acceleration, unfiltered_states, 1);
gsl_matrix * tmp = &test.matrix;
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, control, tmp, 0, temp21a);
gsl_matrix_add(estimate_mean, temp21a);
//estimate_covariance = predict * state_covariance * transpose(predict) + noise;
gsl_matrix_set_zero(estimate_covariance);
gsl_matrix_set_zero(temp22a);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, predict, 0, temp22a); //notice predict is transposed
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, temp22a, 0, estimate_covariance);
gsl_matrix_add_constant(estimate_covariance, 0.1); //adxl345 noise, this is completely wrong
//kalman_gain = estimate_covariance * pseudoinverse(estimate_covariance + observation_covariance);
gsl_matrix_set_zero(kalman_gain);
gsl_matrix_set_zero(temp22a);
gsl_matrix_memcpy(temp22a, observation_covariance);
gsl_matrix_add(temp22a, estimate_covariance);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, estimate_covariance, pseudo_inverse(temp22a), 0, kalman_gain);
//state_mean = estimate_mean + kalman_gain * ( observation_mean - estimate_mean );
gsl_matrix_set_zero(state_mean);
gsl_matrix_set_zero(temp21a);
gsl_matrix_memcpy(temp21a, observation_mean);
gsl_matrix_sub(temp21a, estimate_mean);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, temp21a, 0, state_mean);
gsl_matrix_add(state_mean, estimate_mean);
//state_covariance = estimate_covariance - kalman_gain * ( estimate_covariance );
gsl_matrix_set_zero(state_covariance);
gsl_matrix_set_zero(temp22a);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, estimate_covariance, 0, temp22a);
gsl_matrix_add(state_covariance, estimate_covariance);
gsl_matrix_sub(state_covariance, temp22a);
printf("state_mean:");
gsl_matrix_fprintf(stdout, state_mean, "%f");
//printf("state_covariance:");
//gsl_matrix_fprintf(stdout, state_covariance, "%f");
printf("observation_mean:");
gsl_matrix_fprintf(stdout, observation_mean, "%f");
//printf("observation_covariance:");
//gsl_matrix_fprintf(stdout, observation_covariance, "%f");
printf("estimate_mean:");
gsl_matrix_fprintf(stdout, estimate_mean, "%f");
//printf("estimate_covariance:");
//gsl_matrix_fprintf(stdout, estimate_covariance, "%f");
gsl_matrix_set_zero(temp21a);
gsl_matrix_memcpy(temp21a, observation_mean);
gsl_matrix_sub(temp21a, state_mean);
gsl_matrix_div_elements(temp21a, observation_mean);
gsl_matrix_mul_elements(temp21a, temp21a);
for (int i = states-1; i >= 0; i--) {
error += gsl_matrix_get(temp21a, i, 0);
}
printf("error: %f\n", error);
printf("error/time: %f\n", error/t);
printf("\n");
usleep(1000000);
}
}
static int
set (void *vstate, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
{
lmder_state_t *state = (lmder_state_t *) vstate;
gsl_matrix *r = state->r;
gsl_vector *tau = state->tau;
gsl_vector *diag = state->diag;
gsl_vector *work1 = state->work1;
gsl_permutation *perm = state->perm;
int signum;
/* Evaluate function at x */
/* return immediately if evaluation raised error */
{
int status = GSL_MULTIFIT_FN_EVAL_F_DF (fdf, x, f, J);
if (status)
return status;
}
state->par = 0;
state->iter = 1;
state->fnorm = enorm (f);
gsl_vector_set_all (dx, 0.0);
/* store column norms in diag */
if (scale)
{
compute_diag (J, diag);
}
else
{
gsl_vector_set_all (diag, 1.0);
}
/* set delta to 100 |D x| or to 100 if |D x| is zero */
state->xnorm = scaled_enorm (diag, x);
state->delta = compute_delta (diag, x);
/* Factorize J into QR decomposition */
gsl_matrix_memcpy (r, J);
gsl_linalg_QRPT_decomp (r, tau, perm, &signum, work1);
gsl_vector_set_zero (state->rptdx);
gsl_vector_set_zero (state->w);
/* Zero the trial vector, as in the alloc function */
gsl_vector_set_zero (state->f_trial);
#ifdef DEBUG
printf("r = "); gsl_matrix_fprintf(stdout, r, "%g");
printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
printf("tau = "); gsl_vector_fprintf(stdout, tau, "%g");
#endif
return GSL_SUCCESS;
}