Пример #1
0
/*************************************************
Direction Cosine Matrix IMU: Theory
William Premerlani and Paul Bizard

Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5 
to approximations rather than identities. In effect, the axes in the two frames of reference no 
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a 
simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
*/
void 
AP_DCM::normalize(void)
{
	float error = 0;
	Vector3f	temporary[3];
	
	int problem = 0;
	
	error = _dcm_matrix.a * _dcm_matrix.b; 							// eq.18

	temporary[0] = _dcm_matrix.b;	
	temporary[1] = _dcm_matrix.a;		
	temporary[0] = _dcm_matrix.a - (temporary[0] * (0.5f * error));		// eq.19
	temporary[1] = _dcm_matrix.b - (temporary[1] * (0.5f * error));		// eq.19

	temporary[2] = temporary[0] % temporary[1];							// c= a x b // eq.20

	_dcm_matrix.a = renorm(temporary[0], problem);
	_dcm_matrix.b = renorm(temporary[1], problem);
	_dcm_matrix.c = renorm(temporary[2], problem);
	
	if (problem == 1) {		// Our solution is blowing up and we will force back to initial condition.	Hope we are not upside down!
		_dcm_matrix.a.x = 1.0f;
		_dcm_matrix.a.y = 0.0f;
		_dcm_matrix.a.z = 0.0f;
		_dcm_matrix.b.x = 0.0f;
		_dcm_matrix.b.y = 1.0f;
		_dcm_matrix.b.z = 0.0f;
		_dcm_matrix.c.x = 0.0f;
		_dcm_matrix.c.y = 0.0f;
		_dcm_matrix.c.z = 1.0f;
	}
}
Пример #2
0
/*************************************************
 *  Direction Cosine Matrix IMU: Theory
 *  William Premerlani and Paul Bizard
 *
 *  Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
 *  to approximations rather than identities. In effect, the axes in the two frames of reference no
 *  longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
 *  simple matter to stay ahead of it.
 *  We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
 */
void
AP_AHRS_DCM::normalize(void)
{
    const float error = _dcm_matrix.a * _dcm_matrix.b;                                 // eq.18

    const Vector3f t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error));              // eq.19
    const Vector3f t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error));              // eq.19
    const Vector3f t2 = t0 % t1;                                                       // c= a x b // eq.20

    if (!renorm(t0, _dcm_matrix.a) ||
            !renorm(t1, _dcm_matrix.b) ||
            !renorm(t2, _dcm_matrix.c)) {
        // Our solution is blowing up and we will force back
        // to last euler angles
        _last_failure_ms = AP_HAL::millis();
        AP_AHRS_DCM::reset(true);
    }
}
Пример #3
0
/*************************************************
 *  Direction Cosine Matrix IMU: Theory
 *  William Premerlani and Paul Bizard
 *
 *  Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
 *  to approximations rather than identities. In effect, the axes in the two frames of reference no
 *  longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
 *  simple matter to stay ahead of it.
 *  We call the process of enforcing the orthogonality conditions renormalization.
 */
void BC_AHRS::normalize(void) {		// ★チェックOK
	float error;
	Vector3f t0, t1, t2;
	
	error = _dcm_matrix.a * _dcm_matrix.b;                                              // eq.18
	
	t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error));              // eq.19
	t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error));              // eq.19
	t2 = t0 % t1;                                                       // c= a x b // eq.20
	
	if (!renorm(t0, _dcm_matrix.a) ||
		!renorm(t1, _dcm_matrix.b) ||
		!renorm(t2, _dcm_matrix.c)) {
		// Our solution is blowing up and we will force back
		// to last euler angles
		_last_failure_ms = millis();
		reset(true);
	}
}
Пример #4
0
/*************************************************
Direction Cosine Matrix IMU: Theory
William Premerlani and Paul Bizard

Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
to approximations rather than identities. In effect, the axes in the two frames of reference no
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions �renormalization�.
*/
void
AP_AHRS_DCM::normalize(void)
{
    float error;
    Vector3f t0, t1, t2;

    //setCurrentProfiledActivity(MATRIX_NORMALIZE1);

    error = _dcm_matrix.a * _dcm_matrix.b;                      // eq.18
    t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error));      // eq.19
    t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error));      // eq.19
    t2 = t0 % t1;

    //setCurrentProfiledActivity(MATRIX_NORMALIZE2);
    if (!renorm(t0, _dcm_matrix.a) ||
        !renorm(t1, _dcm_matrix.b) ||
        !renorm(t2, _dcm_matrix.c)) {
        // Our solution is blowing up and we will force back
        // to last euler angles
        reset(true);
    }
}
Пример #5
0
//===============================================================
//               MAIN Program
//===============================================================
int main(int argc, char *argv[])
{ 
printf("numbead %d \n",NUMBEAD);

  setbuf(stdout,NULL);

  //===============================================================
  // Declare variables and print to std output for reference
  //===============================================================
  //define the Config sturcts. Example: configOld[n].pos[i][j]
  //where n->Bead i->Particle j->dimension
  //configOld and configCurrent are switched between when doing MD
  config *configOld = calloc(NUMBEAD,sizeof(config));
  config *configCurrent = calloc(NUMBEAD,sizeof(config));
  config *configNew = calloc(NUMBEAD,sizeof(config));

  //Used to save positions for MHMC rejection
  position *savePos = calloc(NUMBEAD,sizeof(position));

  //averages
  averages *tubeAve = calloc(NUMBEAD,sizeof(averages));

  double doubleNUMu=(double)NUMu;
  double doubleNUMl=(double)NUMl;

  //Parameters
  double du=DU;
  double dt=PREDT*DU*DU;
  double h=sqrt(2.0l*dt);

  //Incrimenter Declarations
  int i,j,acc,rej;
  int MDloopi,MCloopi;
  int tau=0;

  //Vectors for doing the L Inverse
  double *vecdg = calloc(NUMl,sizeof(double));;
  double *veci0 = calloc(NUMl,sizeof(double));;
  double *veci1 = calloc(NUMl,sizeof(double));;
  //double veci1[NUMBEAD-2];
  //double veci0[NUMBEAD-2];

  // array to store rand nums in
  double *GaussRandArray = calloc(NUMu,sizeof(double));
  //double GaussRandArray[NUMu];

  // ratio for incrimenting the MH-MC test
  double ratio;

  // storage for brownian bridge
  double *bb = calloc(NUMu,sizeof(double));
  //double bb[NUMBEAD];

  // array plot of the average path
  //int xBinMax=300;
  //int yBinMax=200;
  int arrayPlot[300][200];
  for(i=0;i<300;i++){
    for(j=0;j<200;j++){
      arrayPlot[i][j]=0;
  } }

  //Print parameters for the run in stdout
  printf("=======================================================\n");
  printf("HMC method for 2D potentials \n");
  printf("=======================================================\n");
  printf("TEMPERATURE = %f \n",TEMP);
  printf("=======================================================\n");
  printf("Number of Metropolis Hastings steps: %i\n",NUMMC);
  printf("Number of MD steps: %i \n",NUMMD);
  printf("=======================================================\n");
  printf("Number of Dimensions: %i \n",NUMDIM);
  printf("Number of Beads: %i \n",NUMBEAD);
  printf("Path grid: du = %+.8e \n", DU);
  printf("Sampling Parameters: dt=%f \n",dt);
  printf("=======================================================\n");
  printf("MD step: h=%+.8e \n",h);
  printf("MD time (n*h): %+.8e \n",NUMMD*h);
  printf("=======================================================\n");

  //===============================================================
  // Reading the input configuration file into savepos
  //===============================================================
  //Input file to be read as first command line argument
  if(argv[1]==NULL) { 
    printf("No input file. Exiting!\n");
    exit(-1);
  }
  else {
    printf("Input Configuratrion File: %s\n",argv[1]);
  }
  int lineNum = 0;
  FILE *fptr = fopen(argv[1],"r");
  switch(NUMDIM){
    case 3:  //For 3 Dimensions
      while( EOF != fscanf(fptr,"%lf %lf %lf",
      &(savePos[lineNum].pos[0]),
      &(savePos[lineNum].pos[1]),
      &(savePos[lineNum].pos[2])) ) {
        lineNum++;
      }
      break;
    case 2:   //For 2 Dimensions 
      while( EOF != fscanf(fptr,"%lf %lf",
      &(savePos[lineNum].pos[0]),
      &(savePos[lineNum].pos[1])) ) {
        lineNum++;
      }
      break;
    case 1:  //For 1 Dimension
      while( EOF != fscanf(fptr,"%lf",
      &(savePos[lineNum].pos[0])) ) {
        lineNum++;
      }
      break;
    default:
      printf("ERROR: NUMDIM incorrectly defined. Exiting!\n");
      exit(-1);
  }


  //===============================================================
  // GNU Scientific Library Random Number Setup
  //===============================================================
  // Example shell command$ GSL_RNG_SEED=123 ./a.out
  printf("=======================================================\n");
  const gsl_rng_type * RanNumType;
  gsl_rng *RanNumPointer; 
  gsl_rng_env_setup();
  RanNumType = gsl_rng_default;
  RanNumPointer= gsl_rng_alloc (RanNumType);
  printf("Random Number Generator Type: %s \n", gsl_rng_name(RanNumPointer));
  printf("RNG Seed: %li \n", gsl_rng_default_seed);
  printf("=======================================================\n");

  double randUniform;

  renorm(savePos, DU, doubleNUMu);



  //===============================================================
  //     Start of HMC Loop (loops over Metropolis Hastings - MC steps)
  //===============================================================

  printf("START Hybrid Monte Carlo MAIN LOOP\n");
  printf("=======================================================\n");
  acc=0;
  rej=0;
  zeroAverages(tubeAve,&tau);

  for(MCloopi=1; MCloopi<=NUMMC; MCloopi++)
  {
    //zero ratio for MH MC test
    ratio=0.0l;
    //===============================================================
    //     Perform one SPDE step
    //===============================================================
  
    //store savePos.pos values to configCurrent.pos
    // savePos.pos stores the positions in case of rejection of the MHMC
    savePostoConfig(savePos, configCurrent);
 
    //(calculates potentials in config given the positions)
    #pragma omp parallel for
    for(i=0;i<NUMBEAD;i++) {calcPotentials(configCurrent,i);}
 
    //calculate LinvG for the config
    LInverse(configCurrent, doubleNUMl, du, vecdg, veci1, veci0);

    //do the preconditioned form of the SPDE
    preconditionSPDE(configCurrent, configNew, du, dt, doubleNUMu, bb, GaussRandArray, RanNumPointer);

    //(calculates potentials in config given the positions)
    #pragma omp parallel for
    for(i=0;i<NUMBEAD;i++) {calcPotentials(configNew,i);}

    //calculate LinvG for the config
    LInverse(configNew, doubleNUMl, du, vecdg, veci1, veci0);

    //acc ratio of newconfig
    ProbAccRatio(configCurrent, configNew, dt, du, &ratio);

    //calculate the averages for the tubes estimator
    accumulateAverages(tubeAve,configNew,&tau);
    accumulateArrayPlot(arrayPlot, configNew);

    printf("SPDE ratio: %+0.10f \n",ratio);
    //===============================================================
    //     Start of MD Loop
    //     This loop needs to be focused on for parallelization
    //===============================================================

    for(MDloopi=1;MDloopi<=NUMMD; MDloopi++)
    {
      //rotate the configuration        
      rotateConfig(&configOld, &configCurrent, &configNew);

      //do the MD position update
      MolecularDynamics(configOld, configCurrent, configNew, du, dt);
 
      //(calculates potentials in config given the positions)
      #pragma omp parallel for
      for(i=0;i<NUMBEAD;i++) {calcPotentials(configNew,i);}

      //calculate LinvG for the config
      LInverse(configNew, doubleNUMl, du, vecdg, veci1, veci0);

      //calculate the average distance moved in the step and print to std out
      if(MDloopi%WRITESTDOUT==0){
        printf("MDi: %.5d | MDi*h: %0.5f | MD ratio: %+0.5f | distance: ",MDloopi,MDloopi*sqrt(2*dt),ratio);
        printDistance(configNew, savePos);
      }

      //acc ratio of newconfig
      ProbAccRatio(configCurrent, configNew, dt, du, &ratio);
      //printf("%i  ProbAcc= %+.15e  QV Vel= %0.15e \n", MDloopi, ratio, qvvel);

      //calculate the averages for the tubes estimator
      accumulateAverages(tubeAve,configNew,&tau);
      accumulateArrayPlot(arrayPlot, configNew);
    }
    //===============================================================
    //Metropolis Hastings Monte-Carlo test
    //===============================================================
    randUniform = gsl_rng_uniform(RanNumPointer);
    if( exp(ratio/SIGMA2) > randUniform ){
      acc++;
      saveConfigtoPos(configNew, savePos);
    }
    else{
      rej++;
    }
    printf("rand=%+0.6f  Exp[ratio]=%+0.6f   dt= %+0.5e     acc= %i      rej= %i  \n",randUniform,exp(ratio/SIGMA2),dt,acc,rej);

    // Write the configuration to file
    if(MCloopi % WRITECONFIGS==0){
      normalizeAverages(tubeAve,&tau);
      writeConfig(configNew,tubeAve,MCloopi);
      zeroAverages(tubeAve,&tau);

      writeArrayPlot(arrayPlot, MCloopi);
      for(i=0;i<300;i++){
        for(j=0;j<200;j++){
          arrayPlot[i][j]=0;
      } }
    }

  }
  // GSL random number generator release memory
  gsl_rng_free (RanNumPointer);

  return(0);
}