double sensorFusion::getTiltProportionalA() { double T_total = RAD2DEG((acos(cos(RPY(0))*cos(RPY(1))))); T_total = constrainn<double>(T_total, 0.0f, SFUS_COMPF_RP_LIM_TILT); return mapp<double>(T_total, 0.0f, SFUS_COMPF_RP_LIM_TILT, rollPitchA_minimum, rollPitchA_maximum); }
double sensorFusion::getEulerYawFromMagne(Vector3d magneBodyFrame) { // tilt compensation without yaw // Source: http://www.chrobotics.com/library/understanding-euler-angles // Source: http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf Vector3d magneEarthFrame = getVectorFromBody2EarthFrame(magneBodyFrame, Vector3d(RPY(0), RPY(1), 0.0f)); // Source: https://www.adafruit.com/datasheets/AN203_Compass_Heading_Using_Magnetometers.pdf double orientation_tmp; if(magneEarthFrame(1) == 0.0) { if(magneEarthFrame(0) > 0.0) orientation_tmp = 0.0; else if(magneEarthFrame(0) < 0.0) orientation_tmp = Pi; } else if(magneEarthFrame(1) > 0.0) { orientation_tmp = Pi/2.0 - atan(magneEarthFrame(0)/magneEarthFrame(1)); } else if(magneEarthFrame(2) < 0.0) { orientation_tmp = 1.5*Pi - atan(magneEarthFrame(0)/magneEarthFrame(1)); } // transform from 0-2*Pi to +/-Pi return wrap_Pi(-orientation_tmp); }
void sensorFusion::writeData(Vector3d accel, Vector3d gyros, Vector3d magne, Vector3d barom) { // compensate chip tilt for all sensors accel = getVectorFromBody2EarthFrame(accel, sensorTiltCalib); gyros = getRatesFromBody2EarthFrame(gyros-gyrosCalib, sensorTiltCalib); magne = getVectorFromBody2EarthFrame(magne, sensorTiltCalib); // push tilt compensated values to container accelData.pushNewElem(accel); gyrosData.pushNewElem(gyros); magneData.pushNewElem(magne); baromData.pushNewElem(barom); // transform input from sensors into earth frame Vector3d accelEulertmp = getEulerAnglesFromAccel(accel); Vector3d gyrosEulerRatestmp = getRatesFromBody2EarthFrame(gyros, RPY); Vector3d accelAccelerationtmp = getVectorFromBody2EarthFrame(accel, RPY); // adjust complementary filter double currentA = getTiltProportionalA(); cf[ROLL].setTauViaA(currentA); cf[PITCH].setTauViaA(currentA); // filtering for roll and pitch RPY(0) = cf[ROLL].getCombinedEstimation(accelEulertmp(0), gyrosEulerRatestmp(0)); RPY(1) = cf[PITCH].getCombinedEstimation(accelEulertmp(1), gyrosEulerRatestmp(1)); RPY(2) = cf[YAW].getCombinedEstimation(getEulerYawFromMagne(magne), gyrosEulerRatestmp(2)); // yaw is +/- Pi RPY(2) = wrap_Pi(RPY(2)); cf[YAW].setCombinedEstimation(RPY(2)); // rotation rates from calibrated gyroscope RPYDot = gyrosEulerRatestmp; // apply moving average filter for height height = 0.0; double height_old = 0.0; for(int i=0; i<SFUS_MA_HEIGHT; i++) { Vector3d tmp; baromData.getNthElem(tmp, i); height += tmp(2); baromData.getNthElem(tmp, i+1); height_old += tmp(2); } height /= SFUS_MA_HEIGHT; height_old /= SFUS_MA_HEIGHT; // vertical speed heightDot = cf[HEIGHTDOT].getCombinedEstimation((height - height_old)/dT, accelAccelerationtmp(2) - GRAVITY); // vertical acceleration heightDotDot = accelAccelerationtmp(2) - GRAVITY; }
/*---------------------------------------------------------------------------*/ int CGFMMmrhs(MKL_INT *solution, double *ShellSphs, double *rhs, double *nRhs, MKL_INT n) { /*---------------------------------------------------------------------------*/ /* Define arrays for the upper triangle of the coefficient matrix and rhs vector */ /* Compressed sparse row storage is used for sparse representation */ /*---------------------------------------------------------------------------*/ MKL_INT rci_request, expected_itercount = 20, i, j; MKL_INT itercount[nRhs]; /* Fill all arrays containing matrix data. */ /*---------------------------------------------------------------------------*/ /* Allocate storage for the solver ?par and temporary storage tmp */ /*---------------------------------------------------------------------------*/ MKL_INT length = 128, method = 1; /*---------------------------------------------------------------------------*/ /* Some additional variables to use with the RCI (P)CG solver */ /*---------------------------------------------------------------------------*/ MKL_INT ipar[128 + 2 * nRhs]; double euclidean_norm, dpar[128 + 2 * nRhs]; double *tmp; tmp = (double*)calloc(n * (3 + nRhs),sizeof(double)); double eone = -1.E0; MKL_INT ione = 1; /*---------------------------------------------------------------------------*/ /* Initialize the initial guess */ /*---------------------------------------------------------------------------*/ for (i = 0; i < n*nRhs; i++) solution[i] = 1.E0; /*---------------------------------------------------------------------------*/ /* Initialize the solver */ /*---------------------------------------------------------------------------*/ for (i = 0; i < (length + 2 * nRhs); i++) ipar[i] = 0; for (i = 0; i < (length + 2 * nRhs); i++) dpar[i] = 0.E0; dcgmrhs_init (&n, solution, &nRhs, rhs, &method, &rci_request, ipar, dpar, tmp); if (rci_request != 0) goto failure; /*---------------------------------------------------------------------------*/ /* Set the desired parameters: */ /* LOGICAL parameters: */ /* do residual stopping test */ /* do not request for the user defined stopping test */ /* DOUBLE parameters */ /* set the relative tolerance to 1.0D-5 instead of default value 1.0D-6 */ /*---------------------------------------------------------------------------*/ ipar[8] = 1; ipar[9] = 0; dpar[0] = 1.E-5; /*---------------------------------------------------------------------------*/ /* Compute the solution by RCI (P)CG solver without preconditioning */ /* Reverse Communications starts here */ /*---------------------------------------------------------------------------*/ rci:dcgmrhs (&n, solution, &nRhs, rhs, &rci_request, ipar, dpar, tmp); /*---------------------------------------------------------------------------*/ /* If rci_request=0, then the solution was found with the required precision */ /*---------------------------------------------------------------------------*/ if (rci_request == 0) goto getsln; /*---------------------------------------------------------------------------*/ /* If rci_request=1, then compute the vector A*tmp[0] */ /* and put the result in vector tmp[n] */ /*---------------------------------------------------------------------------*/ if (rci_request == 1) { //mkl_dcsrsymv (&tr, &n, a, ia, ja, tmp, &tmp[n]); //debug RPY(n, ShellSphs,tmp); // SPMV by 4 calls of FMM goto rci; } /*---------------------------------------------------------------------------*/ /* If rci_request=anything else, then dcg subroutine failed */ /* to compute the solution vector: solution[n] */ /*---------------------------------------------------------------------------*/ goto failure; /*---------------------------------------------------------------------------*/ /* Reverse Communication ends here */ /* Get the current iteration number into itercount */ /*---------------------------------------------------------------------------*/ getsln:dcgmrhs_get (&n, solution, &nRhs, rhs, &rci_request, ipar, dpar, tmp, itercount); /*---------------------------------------------------------------------------*/ /* Print solution vector: solution[n] and number of iterations: itercount */ /*---------------------------------------------------------------------------*/ printf ("The system has been solved\n"); printf ("The following solution obtained\n"); for (i = 0; i < n / 2; i++) printf ("%6.3f ", solution[i]); printf ("\n"); for (i = n / 2; i < n; i++) printf ("%6.3f ", solution[i]); printf ("\n"); for (i = 0; i < n / 2; i++) printf ("%6.3f ", solution[n + i]); printf ("\n"); for (i = n / 2; i < n; i++) printf ("%6.3f ", solution[n + i]); printf ("\nExpected solution is\n"); for (i = 0; i < n / 2; i++) { printf ("%6.3f ", expected_sol[i]); expected_sol[i] -= solution[i]; } printf ("\n"); for (i = n / 2; i < n; i++) { printf ("%6.3f ", expected_sol[i]); expected_sol[i] -= solution[i]; } printf ("\n"); for (i = 0; i < n / 2; i++) { printf ("%6.3f ", expected_sol[n + i]); expected_sol[n + i] -= solution[n + i]; } printf ("\n"); for (i = n / 2; i < n; i++) { printf ("%6.3f ", expected_sol[n + i]); expected_sol[n + i] -= solution[n + i]; } printf ("\n"); i = 1; j = n * nRhs; euclidean_norm = dnrm2 (&j, expected_sol, &i); /*-------------------------------------------------------------------------*/ /* Release internal MKL memory that might be used for computations */ /* NOTE: It is important to call the routine below to avoid memory leaks */ /* unless you disable MKL Memory Manager */ /*-------------------------------------------------------------------------*/ MKL_Free_Buffers (); if (euclidean_norm < 1.0e-12) { printf ("This example has successfully PASSED through all steps of computation!\n"); return 0; } else { printf ("This example may have FAILED as the computed solution differs\n"); printf ("much from the expected solution (Euclidean norm is %e).\n", euclidean_norm); return 1; } /*-------------------------------------------------------------------------*/ /* Release internal MKL memory that might be used for computations */ /* NOTE: It is important to call the routine below to avoid memory leaks */ /* unless you disable MKL Memory Manager */ /*-------------------------------------------------------------------------*/ failure:printf ("This example FAILED as the solver has returned the ERROR code %d", rci_request); MKL_Free_Buffers (); return 1; }
bool sensorFusion::writeInitData(Vector3d accel, Vector3d gyros, Vector3d magne, Vector3d barom) { initAccelReadsCnt++; if(initAccelReadsCnt > SFUS_INIT_ACCEL_DROPS && initAccelReadsCnt <= (SFUS_INIT_ACCEL_DROPS + SFUS_INIT_ACCEL_READS)) initAccelSum += accel; initGyrosReadsCnt++; if(initGyrosReadsCnt > SFUS_INIT_GYROS_DROPS && initGyrosReadsCnt <= (SFUS_INIT_GYROS_DROPS + SFUS_INIT_GYROS_READS)) initGyrosSum += gyros; initMagneReadsCnt++; if(initMagneReadsCnt > SFUS_INIT_MAGNE_DROPS && initMagneReadsCnt <= (SFUS_INIT_MAGNE_DROPS + SFUS_INIT_MAGNE_READS)) initMagneSum += magne; initBaromReadsCnt++; if(initBaromReadsCnt > SFUS_INIT_BAROM_DROPS && initBaromReadsCnt <= (SFUS_INIT_BAROM_DROPS + SFUS_INIT_BAROM_READS)) initBaromSum += barom; // data collection finished if(initAccelReadsCnt >= (SFUS_INIT_ACCEL_DROPS + SFUS_INIT_ACCEL_READS) && initGyrosReadsCnt >= (SFUS_INIT_GYROS_DROPS + SFUS_INIT_GYROS_READS) && initMagneReadsCnt >= (SFUS_INIT_MAGNE_DROPS + SFUS_INIT_MAGNE_READS) && initBaromReadsCnt >= (SFUS_INIT_BAROM_DROPS + SFUS_INIT_BAROM_READS)) { initAccelSum /= SFUS_INIT_ACCEL_READS; initGyrosSum /= SFUS_INIT_GYROS_READS; initMagneSum /= SFUS_INIT_MAGNE_READS; initBaromSum /= SFUS_INIT_BAROM_READS; // compensate chip tilt initAccelSum = getVectorFromBody2EarthFrame(initAccelSum, sensorTiltCalib); initMagneSum = getVectorFromBody2EarthFrame(initMagneSum, sensorTiltCalib); // determine initial values RPY = getEulerAnglesFromAccel(initAccelSum); RPY(2) = getEulerYawFromMagne(initMagneSum); RPYDot = Vector3d(0.0, 0.0, 0.0); height = initBaromSum(2); heightDot = 0.0; heightDotDot = 0.0; #ifdef QS_DEBUG_WITH_CONSOLE printf("*********************************************\n"); printf("Initial attitude:\n"); printf("roll: %f\n", RAD2DEG(RPY(0))); printf("pitch: %f\n", RAD2DEG(RPY(1))); printf("yaw: %f\n\n", RAD2DEG(RPY(2))); #endif // the gyroscope is recalibrated at each start up #ifdef SFUS_USE_GYROS_CALIB gyrosCalib = initGyrosSum; #else gyrosCalib = Vector3d(0.0, 0.0, 0.0); #endif #ifdef QS_DEBUG_WITH_CONSOLE printf("*********************************************\n"); printf("Gyroscope calibrated in sensor fusion:\n"); printf("gyros x offset: %f\n", RAD2DEG(gyrosCalib(0))); printf("gyros x offset: %f\n", RAD2DEG(gyrosCalib(1))); printf("gyros x offset: %f\n\n", RAD2DEG(gyrosCalib(2))); #endif // push values into buffers for(int i=0; i<SFUS_ACCEL_BUF; i++) accelData.pushNewElem(initAccelSum); for(int i=0; i<SFUS_GYROS_BUF; i++) gyrosData.pushNewElem(initGyrosSum); for(int i=0; i<SFUS_MAGNE_BUF; i++) magneData.pushNewElem(initMagneSum); for(int i=0; i<SFUS_BAROM_BUF; i++) baromData.pushNewElem(initBaromSum); // init complementary filter A/Tau factor float currentA = getTiltProportionalA(); cf[ROLL].setTauViaA(currentA); cf[PITCH].setTauViaA(currentA); // init complementary filter angles cf[ROLL].setCombinedEstimation(RPY(0)); cf[PITCH].setCombinedEstimation(RPY(1)); cf[YAW].setCombinedEstimation(RPY(2)); cf[HEIGHTDOT].setCombinedEstimation(heightDot); return true; } return false; }
//! Gives back a rotation matrix specified with EulerZYX convention : //! First rotate around Z with alfa, //! then around the new Y with beta, then around //! new X with gamma. //! //! closely related to RPY-convention inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) { return RPY(Gamma,Beta,Alfa); }