Esempio n. 1
0
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;
}
Esempio n. 2
0
void printf_matrix(const char* filename, gsl_matrix * m)
{
    FILE* fileptr;
    fileptr = fopen(filename, "w");
    gsl_matrix_fprintf(fileptr, m, "%f");
    fclose(fileptr);
}
Esempio n. 3
0
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;
}
Esempio n. 4
0
File: io.c Progetto: diogro/evomod
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);
}
Esempio n. 6
0
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;
}
Esempio n. 7
0
void print_tilt()
{
	if (!initialized) return;
	gsl_matrix_fprintf(stdout, rm_ax_tilt, "%f");
}
Esempio n. 8
0
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);
}
Esempio n. 9
0
/*
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;
     }
Esempio n. 10
0
// for debugging
void dump_matrix(gsl_matrix *m, char *filename) {
  FILE *f = fopen(filename, "w");
  gsl_matrix_fprintf(f, m, "%.16g");
  fclose(f);
}
Esempio n. 11
0
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);
  
  }
}
Esempio n. 12
0
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;
}