/*************************************************
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;
}
}
/*************************************************
* 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);
}
}
/*************************************************
* 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);
}
}
/*************************************************
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);
}
}
//===============================================================
// 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);
}