// Copyright (c) 2014, Freescale Semiconductor, Inc. // All rights reserved. // vim: set ts=4: // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // * Neither the name of Freescale Semiconductor, Inc. nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // // This is the file that contains the fusion routines. It is STRONGLY RECOMMENDED // that the casual developer NOT TOUCH THIS FILE. The mathematics behind this file // is extremely complex, and it will be very easy (almost inevitable) that you screw // it up. // #include "NXPMotionSense.h" // kalman filter noise variances #define FQVA_9DOF_GBY_KALMAN 2E-6F // accelerometer noise g^2 so 1.4mg RMS #define FQVM_9DOF_GBY_KALMAN 0.1F // magnetometer noise uT^2 #define FQVG_9DOF_GBY_KALMAN 0.3F // gyro noise (deg/s)^2 #define FQWB_9DOF_GBY_KALMAN 1E-9F // gyro offset drift (deg/s)^2: 1E-9 implies 0.09deg/s max at 50Hz #define FQWA_9DOF_GBY_KALMAN 1E-4F // linear acceleration drift g^2 (increase slows convergence to g but reduces sensitivity to shake) #define FQWD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance drift uT^2 (increase slows convergence to B but reduces sensitivity to magnet) // initialization of Qw covariance matrix #define FQWINITTHTH_9DOF_GBY_KALMAN 2000E-5F // th_e * th_e terms #define FQWINITBB_9DOF_GBY_KALMAN 250E-3F // b_e * b_e terms #define FQWINITTHB_9DOF_GBY_KALMAN 0.0F // th_e * b_e terms #define FQWINITAA_9DOF_GBY_KALMAN 10E-5F // a_e * a_e terms (increase slows convergence to g but reduces sensitivity to shake) #define FQWINITDD_9DOF_GBY_KALMAN 600E-3F // d_e * d_e terms (increase slows convergence to B but reduces sensitivity to magnet) // linear acceleration and magnetic disturbance time constants #define FCA_9DOF_GBY_KALMAN 0.5F // linear acceleration decay factor #define FCD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance decay factor // maximum geomagnetic inclination angle tracked by Kalman filter #define SINDELTAMAX 0.9063078F // sin of max +ve geomagnetic inclination angle: here 65.0 deg #define COSDELTAMAX 0.4226183F // cos of max +ve geomagnetic inclination angle: here 65.0 deg #define DEFAULTB 50.0F // default geomagnetic field (uT) #define X 0 // vector components #define Y 1 #define Z 2 #define FDEGTORAD 0.01745329251994F // degrees to radians conversion = pi / 180 #define FRADTODEG 57.2957795130823F // radians to degrees conversion = 180 / pi #define ONEOVER48 0.02083333333F // 1 / 48 #define ONEOVER3840 0.0002604166667F // 1 / 3840 #define Quaternion_t NXPSensorFusion::Quaternion_t static void fqAeq1(Quaternion_t *pqA); static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[]); static void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg); static void fQuaternionFromRotationMatrix(float R[][3], Quaternion_t *pq); static void fQuaternionFromRotationVectorDeg(Quaternion_t *pq, const float rvecdeg[], float fscaling); static void fRotationMatrixFromQuaternion(float R[][3], const Quaternion_t *pq); static void fRotationVectorDegFromQuaternion(Quaternion_t *pq, float rvecdeg[]); static void qAeqAxB(Quaternion_t *pqA, const Quaternion_t *pqB); static void qAeqBxC(Quaternion_t *pqA, const Quaternion_t *pqB, const Quaternion_t *pqC); //static Quaternion_t qconjgAxB(const Quaternion_t *pqA, const Quaternion_t *pqB); static void fqAeqNormqA(Quaternion_t *pqA); static float fasin_deg(float x); static float facos_deg(float x); static float fatan_deg(float x); static float fatan2_deg(float y, float x); static float fatan_15deg(float x); extern "C"{ void f3x3matrixAeqI(float A[][3]); void fmatrixAeqI(float *A[], int16_t rc); void f3x3matrixAeqScalar(float A[][3], float Scalar); void f3x3matrixAeqInvSymB(float A[][3], float B[][3]); void f3x3matrixAeqAxScalar(float A[][3], float Scalar); void f3x3matrixAeqMinusA(float A[][3]); float f3x3matrixDetA(float A[][3]); void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n); void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[], int8_t iPivot[], int8_t isize); void fmatrixAeqRenormRotA(float A[][3]); } // initialize the 9DOF Kalman filter void NXPSensorFusion::begin(float sampleRate) { int8_t i, j; // reset the flag denoting that a first 9DOF orientation lock has been achieved FirstOrientationLock = 0; // compute and store useful product terms to save floating point calculations later Fastdeltat = 1.0f / sampleRate; deltat = Fastdeltat; deltatsq = deltat * deltat; casq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN; cdsq = FCD_9DOF_GBY_KALMAN * FCD_9DOF_GBY_KALMAN; QwbplusQvG = FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN; // initialize the fixed entries in the measurement matrix C for (i = 0; i < 6; i++) { for (j = 0; j < 12; j++) { C6x12[i][j]= 0.0F; } } C6x12[0][6] = C6x12[1][7] = C6x12[2][8] = 1.0F; C6x12[3][9] = C6x12[4][10] = C6x12[5][11] = -1.0F; // zero a posteriori orientation, error vector xe+ (thetae+, be+, de+, ae+) and b+ and inertial f3x3matrixAeqI(RPl); fqAeq1(&qPl); for (i = X; i <= Z; i++) { ThErrPl[i] = bErrPl[i] = aErrSePl[i] = dErrSePl[i] = bPl[i] = 0.0F; } // initialize the reference geomagnetic vector (uT, global frame) DeltaPl = 0.0F; // initialize NED geomagnetic vector to zero degrees inclination mGl[X] = DEFAULTB; mGl[Y] = 0.0F; mGl[Z] = 0.0F; // initialize noise variances for Qv and Qw matrix updates QvAA = FQVA_9DOF_GBY_KALMAN + FQWA_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * deltatsq * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN); QvMM = FQVM_9DOF_GBY_KALMAN + FQWD_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * deltatsq * DEFAULTB * DEFAULTB * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN); // initialize the 12x12 noise covariance matrix Qw of the a priori error vector xe- // Qw is then recursively updated as P+ = (1 - K * C) * P- = (1 - K * C) * Qw and Qw // updated from P+ // zero the matrix Qw for (i = 0; i < 12; i++) { for (j = 0; j < 12; j++) { Qw12x12[i][j] = 0.0F; } } // loop over non-zero values for (i = 0; i < 3; i++) { // theta_e * theta_e terms Qw12x12[i][i] = FQWINITTHTH_9DOF_GBY_KALMAN; // b_e * b_e terms Qw12x12[i + 3][i + 3] = FQWINITBB_9DOF_GBY_KALMAN; // th_e * b_e terms Qw12x12[i][i + 3] = Qw12x12[i + 3][i] = FQWINITTHB_9DOF_GBY_KALMAN; // a_e * a_e terms Qw12x12[i + 6][i + 6] = FQWINITAA_9DOF_GBY_KALMAN; // d_e * d_e terms Qw12x12[i + 9][i + 9] = FQWINITDD_9DOF_GBY_KALMAN; } // clear the reset flag resetflag = 0; } // 9DOF orientation function implemented using a 12 element Kalman filter //void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, //const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro, //const MagCalibration_t *MagCal) void NXPSensorFusion::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float Accel[3]={ax, ay, az}; // Accel float Yp[3]={gx, gy, gz}; // Gryo float Mag[3]={mx, my, mz}; // Mag // local scalars and arrays float fopp, fadj, fhyp; // opposite, adjacent and hypoteneuse float fsindelta, fcosdelta; // sin and cos of inclination angle delta float rvec[3]; // rotation vector float ftmp; // scratch variable float ftmpA12x6[12][6]; // scratch array int8_t i, j, k; // loop counters int8_t iMagJamming; // magnetic jamming flag int8_t ValidMagCal; // assorted array pointers float *pfPPlus12x12ij; float *pfPPlus12x12kj; float *pfQw12x12ij; float *pfQw12x12ik; float *pfQw12x12kj; float *pfK12x6ij; float *pfK12x6ik; float *pftmpA12x6ik; float *pftmpA12x6kj; float *pftmpA12x6ij; float *pfC6x12ik; float *pfC6x12jk; // working arrays for 6x6 matrix inversion float *pfRows[6]; int8_t iColInd[6]; int8_t iRowInd[6]; int8_t iPivot[6]; // do a reset and return if requested if (resetflag) { begin(1.0f / deltat); return; } // ********************************************************************************* // initial orientation lock to accelerometer and magnetometer eCompass orientation // ********************************************************************************* if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) { ValidMagCal = 1; } else { ValidMagCal = 0; } // do a once-only orientation lock after the first valid magnetic calibration if (ValidMagCal && !FirstOrientationLock) { // get the 6DOF orientation matrix and initial inclination angle feCompassNED(RPl, &DeltaPl, Mag, Accel); // get the orientation quaternion from the orientation matrix fQuaternionFromRotationMatrix(RPl, &qPl); // set the orientation lock flag so this initial alignment is only performed once FirstOrientationLock = 1; } // ********************************************************************************* // calculate a priori rotation matrix // ********************************************************************************* // compute the angular velocity from the averaged high frequency gyro reading. // omega[k] = yG[k] - b-[k] = yG[k] - b+[k-1] (deg/s) Omega[X] = Yp[X] - bPl[X]; Omega[Y] = Yp[Y] - bPl[Y]; Omega[Z] = Yp[Z] - bPl[Z]; // initialize the a priori orientation quaternion to the previous a posteriori estimate qMi = qPl; // integrate the buffered high frequency (typically 200Hz) gyro readings //for (j = 0; j < OVERSAMPLE_RATIO; j++) { // compute the incremental fast (typically 200Hz) rotation vector rvec (deg) for (i = X; i <= Z; i++) { rvec[i] = (Yp[i] - bPl[i]) * Fastdeltat; } // compute the incremental quaternion fDeltaq from the rotation vector fQuaternionFromRotationVectorDeg(&Deltaq, rvec, 1.0F); // incrementally rotate the a priori orientation quaternion fqMi // the a posteriori quaternion fqPl is re-normalized later so this update is stable qAeqAxB(&qMi, &Deltaq); //} // get the a priori rotation matrix from the a priori quaternion fRotationMatrixFromQuaternion(RMi, &qMi); // ********************************************************************************* // calculate a priori gyro, accelerometer and magnetometer estimates // of the gravity and geomagnetic vectors and errors // the most recent 'Fast' measurements are used to reduce phase errors // ********************************************************************************* for (i = X; i <= Z; i++) { // compute the a priori gyro estimate of the gravitational vector (g, sensor frame) // using an absolute rotation of the global frame gravity vector (with magnitude 1g) // NED gravity is along positive z axis gSeGyMi[i] = RMi[i][Z]; // compute a priori acceleration (a-) (g, sensor frame) from decayed a // posteriori estimate (g, sensor frame) aSeMi[i] = FCA_9DOF_GBY_KALMAN * aSePl[i]; // compute the a priori gravity error vector (accelerometer minus gyro estimates) // (g, sensor frame) // NED and Windows 8 have positive sign for gravity: y = g - a and g = y + a gErrSeMi[i] = Accel[i] + aSeMi[i] - gSeGyMi[i]; // compute the a priori gyro estimate of the geomagnetic vector (uT, sensor frame) // using an absolute rotation of the global frame geomagnetic vector (with magnitude B uT) // NED y component of geomagnetic vector in global frame is zero mSeGyMi[i] = RMi[i][X] * mGl[X] + RMi[i][Z] * mGl[Z]; // compute the a priori geomagnetic error vector (magnetometer minus gyro estimates) // (g, sensor frame) mErrSeMi[i] = Mag[i] - mSeGyMi[i]; } // ********************************************************************************* // update variable elements of measurement matrix C // ********************************************************************************* // update measurement matrix C with -alpha(g-)x and -alpha(m-)x from gyro (g, uT, sensor frame) C6x12[0][1] = FDEGTORAD * gSeGyMi[Z]; C6x12[0][2] = -FDEGTORAD * gSeGyMi[Y]; C6x12[1][2] = FDEGTORAD * gSeGyMi[X]; C6x12[1][0] = -C6x12[0][1]; C6x12[2][0] = -C6x12[0][2]; C6x12[2][1] = -C6x12[1][2]; C6x12[3][1] = FDEGTORAD * mSeGyMi[Z]; C6x12[3][2] = -FDEGTORAD * mSeGyMi[Y]; C6x12[4][2] = FDEGTORAD * mSeGyMi[X]; C6x12[4][0] = -C6x12[3][1]; C6x12[5][0] = -C6x12[3][2]; C6x12[5][1] = -C6x12[4][2]; C6x12[0][4] = -deltat * C6x12[0][1]; C6x12[0][5] = -deltat * C6x12[0][2]; C6x12[1][5] = -deltat * C6x12[1][2]; C6x12[1][3]= -C6x12[0][4]; C6x12[2][3]= -C6x12[0][5]; C6x12[2][4]= -C6x12[1][5]; C6x12[3][4] = -deltat * C6x12[3][1]; C6x12[3][5] = -deltat * C6x12[3][2]; C6x12[4][5] = -deltat * C6x12[4][2]; C6x12[4][3] = -C6x12[3][4]; C6x12[5][3] = -C6x12[3][5]; C6x12[5][4] = -C6x12[4][5]; // ********************************************************************************* // calculate the Kalman gain matrix K // K = P- * C^T * inv(C * P- * C^T + Qv) = Qw * C^T * inv(C * Qw * C^T + Qv) // Qw is used as a proxy for P- throughout the code // P+ is used here as a working array to reduce RAM usage and is re-computed later // ********************************************************************************* // set ftmpA12x6 = P- * C^T = Qw * C^T where Qw and C are both sparse // C also has a significant number of +1 and -1 entries // ftmpA12x6 is also sparse but not symmetric for (i = 0; i < 12; i++) { // loop over rows of ftmpA12x6 // initialize pftmpA12x6ij for current i, j=0 pftmpA12x6ij = ftmpA12x6[i]; for (j = 0; j < 6; j++) { // loop over columns of ftmpA12x6 // zero ftmpA12x6[i][j] *pftmpA12x6ij = 0.0F; // initialize pfC6x12jk for current j, k=0 pfC6x12jk = C6x12[j]; // initialize pfQw12x12ik for current i, k=0 pfQw12x12ik = Qw12x12[i]; // sum matrix products over inner loop over k for (k = 0; k < 12; k++) { if ((*pfQw12x12ik != 0.0F) && (*pfC6x12jk != 0.0F)) { if (*pfC6x12jk == 1.0F) *pftmpA12x6ij += *pfQw12x12ik; else if (*pfC6x12jk == -1.0F) *pftmpA12x6ij -= *pfQw12x12ik; else *pftmpA12x6ij += *pfQw12x12ik * *pfC6x12jk; } // increment pfC6x12jk and pfQw12x12ik for next iteration of k pfC6x12jk++; pfQw12x12ik++; } // increment pftmpA12x6ij for next iteration of j pftmpA12x6ij++; } } // set symmetric P+ (6x6 scratch sub-matrix) to C * P- * C^T + Qv // = C * (Qw * C^T) + Qv = C * ftmpA12x6 + Qv // both C and ftmpA12x6 are sparse but not symmetric for (i = 0; i < 6; i++) { // loop over rows of P+ // initialize pfPPlus12x12ij for current i, j=i pfPPlus12x12ij = PPlus12x12[i] + i; for (j = i; j < 6; j++) { // loop over above diagonal columns of P+ // zero P+[i][j] *pfPPlus12x12ij = 0.0F; // initialize pfC6x12ik for current i, k=0 pfC6x12ik = C6x12[i]; // initialize pftmpA12x6kj for current j, k=0 pftmpA12x6kj = *ftmpA12x6 + j; // sum matrix products over inner loop over k for (k = 0; k < 12; k++) { if ((*pfC6x12ik != 0.0F) && (*pftmpA12x6kj != 0.0F)) { if (*pfC6x12ik == 1.0F) *pfPPlus12x12ij += *pftmpA12x6kj; else if (*pfC6x12ik == -1.0F) *pfPPlus12x12ij -= *pftmpA12x6kj; else *pfPPlus12x12ij += *pfC6x12ik * *pftmpA12x6kj; } // update pfC6x12ik and pftmpA12x6kj for next iteration of k pfC6x12ik++; pftmpA12x6kj += 6; } // increment pfPPlus12x12ij for next iteration of j pfPPlus12x12ij++; } } // add in noise covariance terms to the diagonal PPlus12x12[0][0] += QvAA; PPlus12x12[1][1] += QvAA; PPlus12x12[2][2] += QvAA; PPlus12x12[3][3] += QvMM; PPlus12x12[4][4] += QvMM; PPlus12x12[5][5] += QvMM; // copy above diagonal elements of P+ (6x6 scratch sub-matrix) to below diagonal for (i = 1; i < 6; i++) for (j = 0; j < i; j++) PPlus12x12[i][j] = PPlus12x12[j][i]; // calculate inverse of P+ (6x6 scratch sub-matrix) = inv(C * P- * C^T + Qv) = inv(C * Qw * C^T + Qv) for (i = 0; i < 6; i++) { pfRows[i] = PPlus12x12[i]; } fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 3); // set K = P- * C^T * inv(C * P- * C^T + Qv) = Qw * C^T * inv(C * Qw * C^T + Qv) // = ftmpA12x6 * P+ (6x6 sub-matrix) // ftmpA12x6 = Qw * C^T is sparse but P+ (6x6 sub-matrix) is not // K is not symmetric because C is not symmetric for (i = 0; i < 12; i++) { // loop over rows of K12x6 // initialize pfK12x6ij for current i, j=0 pfK12x6ij = K12x6[i]; for (j = 0; j < 6; j++) { // loop over columns of K12x6 // zero the matrix element fK12x6[i][j] *pfK12x6ij = 0.0F; // initialize pftmpA12x6ik for current i, k=0 pftmpA12x6ik = ftmpA12x6[i]; // initialize pfPPlus12x12kj for current j, k=0 pfPPlus12x12kj = *PPlus12x12 + j; // sum matrix products over inner loop over k for (k = 0; k < 6; k++) { if (*pftmpA12x6ik != 0.0F) { *pfK12x6ij += *pftmpA12x6ik * *pfPPlus12x12kj; } // increment pftmpA12x6ik and pfPPlus12x12kj for next iteration of k pftmpA12x6ik++; pfPPlus12x12kj += 12; } // increment pfK12x6ij for the next iteration of j pfK12x6ij++; } } // ********************************************************************************* // calculate a posteriori error estimate: xe+ = K * ze- // ********************************************************************************* // first calculate all four error vector components using accelerometer error component only // for fThErrPl, fbErrPl, faErrSePl but also magnetometer for fdErrSePl for (i = X; i <= Z; i++) { ThErrPl[i] = K12x6[i][0] * gErrSeMi[X] + K12x6[i][1] * gErrSeMi[Y] + K12x6[i][2] * gErrSeMi[Z]; bErrPl[i] = K12x6[i + 3][0] * gErrSeMi[X] + K12x6[i + 3][1] * gErrSeMi[Y] + K12x6[i + 3][2] * gErrSeMi[Z]; aErrSePl[i] = K12x6[i + 6][0] * gErrSeMi[X] + K12x6[i + 6][1] * gErrSeMi[Y] + K12x6[i + 6][2] * gErrSeMi[Z]; dErrSePl[i] = K12x6[i + 9][0] * gErrSeMi[X] + K12x6[i + 9][1] * gErrSeMi[Y] + K12x6[i + 9][2] * gErrSeMi[Z] + K12x6[i + 9][3] * mErrSeMi[X] + K12x6[i + 9][4] * mErrSeMi[Y] + K12x6[i + 9][5] * mErrSeMi[Z]; } // set the magnetic jamming flag if there is a significant magnetic error power after calibration ftmp = dErrSePl[X] * dErrSePl[X] + dErrSePl[Y] * dErrSePl[Y] + dErrSePl[Z] * dErrSePl[Z]; //iMagJamming = (ValidMagCal) && (ftmp > MagCal->FourBsq); iMagJamming = (ValidMagCal) && (ftmp > (DEFAULTB*DEFAULTB*4.0f)); // TODO: FourBsq.... // add the remaining magnetic error terms if there is calibration and no magnetic jamming if (ValidMagCal && !iMagJamming) { for (i = X; i <= Z; i++) { ThErrPl[i] += K12x6[i][3] * mErrSeMi[X] + K12x6[i][4] * mErrSeMi[Y] + K12x6[i][5] * mErrSeMi[Z]; bErrPl[i] += K12x6[i + 3][3] * mErrSeMi[X] + K12x6[i + 3][4] * mErrSeMi[Y] + K12x6[i + 3][5] * mErrSeMi[Z]; aErrSePl[i] += K12x6[i + 6][3] * mErrSeMi[X] + K12x6[i + 6][4] * mErrSeMi[Y] + K12x6[i + 6][5] * mErrSeMi[Z]; } } // ********************************************************************************* // apply the a posteriori error corrections to the a posteriori state vector // ********************************************************************************* // get the a posteriori delta quaternion fQuaternionFromRotationVectorDeg(&Deltaq, ThErrPl, -1.0F); // compute the a posteriori orientation quaternion fqPl = fqMi * Deltaq(-thetae+) // the resulting quaternion may have negative scalar component q0 qAeqBxC(&qPl, &qMi, &Deltaq); // normalize the a posteriori orientation quaternion to stop error propagation // the renormalization function ensures that the scalar component q0 is non-negative fqAeqNormqA(&qPl); // compute the a posteriori rotation matrix from the a posteriori quaternion fRotationMatrixFromQuaternion(RPl, &qPl); // compute the rotation vector from the a posteriori quaternion fRotationVectorDegFromQuaternion(&qPl, RVecPl); // update the a posteriori gyro offset vector b+ and // assign the entire linear acceleration error vector to update the linear acceleration for (i = X; i <= Z; i++) { // b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s) bPl[i] -= bErrPl[i]; // a+ = a- - ae+ (g, sensor frame) aSePl[i] = aSeMi[i] - aErrSePl[i]; } // compute the linear acceleration in the global frame from the accelerometer measurement (sensor frame). // de-rotate the accelerometer measurement from the sensor to global frame using the inverse (transpose) // of the a posteriori rotation matrix aGlPl[X] = RPl[X][X] * Accel[X] + RPl[Y][X] * Accel[Y] + RPl[Z][X] * Accel[Z]; aGlPl[Y] = RPl[X][Y] * Accel[X] + RPl[Y][Y] * Accel[Y] + RPl[Z][Y] * Accel[Z]; aGlPl[Z] = RPl[X][Z] * Accel[X] + RPl[Y][Z] * Accel[Y] + RPl[Z][Z] * Accel[Z]; // remove gravity and correct the sign if the coordinate system is gravity positive / acceleration negative // gravity positive NED aGlPl[X] = -aGlPl[X]; aGlPl[Y] = -aGlPl[Y]; aGlPl[Z] = -(aGlPl[Z] - 1.0F); // update the reference geomagnetic vector using magnetic disturbance error if valid calibration and no jamming if (ValidMagCal && !iMagJamming) { // de-rotate the NED magnetic disturbance error de+ from the sensor to the global reference frame // using the inverse (transpose) of the a posteriori rotation matrix dErrGlPl[X] = RPl[X][X] * dErrSePl[X] + RPl[Y][X] * dErrSePl[Y] + RPl[Z][X] * dErrSePl[Z]; dErrGlPl[Z] = RPl[X][Z] * dErrSePl[X] + RPl[Y][Z] * dErrSePl[Y] + RPl[Z][Z] * dErrSePl[Z]; // compute components of the new geomagnetic vector // the north pointing component fadj must always be non-negative fopp = mGl[Z] - dErrGlPl[Z]; fadj = mGl[X] - dErrGlPl[X]; if (fadj < 0.0F) { fadj = 0.0F; } fhyp = sqrtf(fopp * fopp + fadj * fadj); // check for the pathological condition of zero geomagnetic field if (fhyp != 0.0F) { // compute the sine and cosine of the new geomagnetic vector ftmp = 1.0F / fhyp; fsindelta = fopp * ftmp; fcosdelta = fadj * ftmp; // limit the inclination angle between limits to prevent runaway if (fsindelta > SINDELTAMAX) { fsindelta = SINDELTAMAX; fcosdelta = COSDELTAMAX; } else if (fsindelta < -SINDELTAMAX) { fsindelta = -SINDELTAMAX; fcosdelta = COSDELTAMAX; } // compute the new geomagnetic vector (always north pointing) DeltaPl = fasin_deg(fsindelta); //mGl[X] = MagCal->B * fcosdelta; // TODO: MagCal->B //mGl[Z] = MagCal->B * fsindelta; mGl[X] = DEFAULTB * fcosdelta; // TODO: MagCal->B mGl[Z] = DEFAULTB * fsindelta; } // end hyp == 0.0F } // end ValidMagCal // ********************************************************************************* // compute the a posteriori Euler angles from the orientation matrix // ********************************************************************************* // calculate the NED Euler angles fNEDAnglesDegFromRotationMatrix(RPl, &PhiPl, &ThePl, &PsiPl, &RhoPl, &ChiPl); // *********************************************************************************** // calculate (symmetric) a posteriori error covariance matrix P+ // P+ = (I12 - K * C) * P- = (I12 - K * C) * Qw = Qw - K * (C * Qw) // both Qw and P+ are used as working arrays in this section // at the end of this section, P+ is valid but Qw is over-written // *********************************************************************************** // set P+ (6x12 scratch sub-matrix) to the product C (6x12) * Qw (12x12) // where both C and Qw are sparse and C has a significant number of +1 and -1 entries // the resulting matrix is sparse but not symmetric for (i = 0; i < 6; i++) { // initialize pfPPlus12x12ij for current i, j=0 pfPPlus12x12ij = PPlus12x12[i]; for (j = 0; j < 12; j++) { // zero P+[i][j] *pfPPlus12x12ij = 0.0F; // initialize pfC6x12ik for current i, k=0 pfC6x12ik = C6x12[i]; // initialize pfQw12x12kj for current j, k=0 pfQw12x12kj = &Qw12x12[0][j]; // sum matrix products over inner loop over k for (k = 0; k < 12; k++) { if ((*pfC6x12ik != 0.0F) && (*pfQw12x12kj != 0.0F)) { if (*pfC6x12ik == 1.0F) *pfPPlus12x12ij += *pfQw12x12kj; else if (*pfC6x12ik == -1.0F) *pfPPlus12x12ij -= *pfQw12x12kj; else *pfPPlus12x12ij += *pfC6x12ik * *pfQw12x12kj; } // update pfC6x12ik and pfQw12x12kj for next iteration of k pfC6x12ik++; pfQw12x12kj += 12; } // increment pfPPlus12x12ij for next iteration of j pfPPlus12x12ij++; } } // compute P+ = (I12 - K * C) * Qw = Qw - K * (C * Qw) = Qw - K * P+ (6x12 sub-matrix) // storing result P+ in Qw and over-writing Qw which is OK since Qw is later computed from P+ // where working array P+ (6x12 sub-matrix) is sparse but K is not sparse // only on and above diagonal terms of P+ are computed since P+ is symmetric for (i = 0; i < 12; i++) { // initialize pfQw12x12ij for current i, j=i pfQw12x12ij = Qw12x12[i] + i; for (j = i; j < 12; j++) { // initialize pfK12x6ik for current i, k=0 pfK12x6ik = K12x6[i]; // initialize pfPPlus12x12kj for current j, k=0 pfPPlus12x12kj = *PPlus12x12 + j; // compute on and above diagonal matrix entry for (k = 0; k < 6; k++) { // check for non-zero values since P+ (6x12 scratch sub-matrix) is sparse if (*pfPPlus12x12kj != 0.0F) { *pfQw12x12ij -= *pfK12x6ik * *pfPPlus12x12kj; } // increment pfK12x6ik and pfPPlus12x12kj for next iteration of k pfK12x6ik++; pfPPlus12x12kj += 12; } // increment pfQw12x12ij for next iteration of j pfQw12x12ij++; } } // Qw now holds the on and above diagonal elements of P+ // so perform a simple copy to the all elements of P+ // after execution of this code P+ is valid but Qw remains invalid for (i = 0; i < 12; i++) { // initialize pfPPlus12x12ij and pfQw12x12ij for current i, j=i pfPPlus12x12ij = PPlus12x12[i] + i; pfQw12x12ij = Qw12x12[i] + i; // copy the on-diagonal elements and increment pointers to enter loop at j=i+1 *(pfPPlus12x12ij++) = *(pfQw12x12ij++); // loop over above diagonal columns j copying to below-diagonal elements for (j = i + 1; j < 12; j++) { *(pfPPlus12x12ij++)= PPlus12x12[j][i] = *(pfQw12x12ij++); } } // ********************************************************************************* // re-create the noise covariance matrix Qw=fn(P+) for the next iteration // using the elements of P+ which are now valid // Qw was over-written earlier but is here recomputed (all elements) // ********************************************************************************* // zero the matrix Qw for (i = 0; i < 12; i++) { for (j = 0; j < 12; j++) { Qw12x12[i][j] = 0.0F; } } // update the covariance matrix components for (i = 0; i < 3; i++) { // Qw[th-th-] = Qw[0-2][0-2] = E[th-(th-)^T] = Q[th+th+] + deltat^2 * (Q[b+b+] + (Qwb + QvG) * I) Qw12x12[i][i] = PPlus12x12[i][i] + deltatsq * (PPlus12x12[i + 3][i + 3] + QwbplusQvG); // Qw[b-b-] = Qw[3-5][3-5] = E[b-(b-)^T] = Q[b+b+] + Qwb * I Qw12x12[i + 3][i + 3] = PPlus12x12[i + 3][i + 3] + FQWB_9DOF_GBY_KALMAN; // Qw[th-b-] = Qw[0-2][3-5] = E[th-(b-)^T] = -deltat * (Q[b+b+] + Qwb * I) = -deltat * Qw[b-b-] Qw12x12[i][i + 3] = Qw12x12[i + 3][i] = -deltat * Qw12x12[i + 3][i + 3]; // Qw[a-a-] = Qw[6-8][6-8] = E[a-(a-)^T] = ca^2 * Q[a+a+] + Qwa * I Qw12x12[i + 6][i + 6] = casq * PPlus12x12[i + 6][i + 6] + FQWA_9DOF_GBY_KALMAN; // Qw[d-d-] = Qw[9-11][9-11] = E[d-(d-)^T] = cd^2 * Q[d+d+] + Qwd * I Qw12x12[i + 9][i + 9] = cdsq * PPlus12x12[i + 9][i + 9] + FQWD_9DOF_GBY_KALMAN; } } // compile time constants that are private to this file #define SMALLQ0 0.01F // limit of quaternion scalar component requiring special algorithm #define CORRUPTQUAT 0.001F // threshold for deciding rotation quaternion is corrupt #define SMALLMODULUS 0.01F // limit where rounding errors may appear // Aerospace NED accelerometer 3DOF tilt function computing rotation matrix fR void f3DOFTiltNED(float fR[][3], float fGp[]) { // the NED self-consistency twist occurs at 90 deg pitch // local variables int16_t i; // counter float fmodGxyz; // modulus of the x, y, z accelerometer readings float fmodGyz; // modulus of the y, z accelerometer readings float frecipmodGxyz; // reciprocal of modulus float ftmp; // scratch variable // compute the accelerometer squared magnitudes fmodGyz = fGp[Y] * fGp[Y] + fGp[Z] * fGp[Z]; fmodGxyz = fmodGyz + fGp[X] * fGp[X]; // check for freefall special case where no solution is possible if (fmodGxyz == 0.0F) { f3x3matrixAeqI(fR); return; } // check for vertical up or down gimbal lock case if (fmodGyz == 0.0F) { f3x3matrixAeqScalar(fR, 0.0F); fR[Y][Y] = 1.0F; if (fGp[X] >= 0.0F) { fR[X][Z] = 1.0F; fR[Z][X] = -1.0F; } else { fR[X][Z] = -1.0F; fR[Z][X] = 1.0F; } return; } // compute moduli for the general case fmodGyz = sqrtf(fmodGyz); fmodGxyz = sqrtf(fmodGxyz); frecipmodGxyz = 1.0F / fmodGxyz; ftmp = fmodGxyz / fmodGyz; // normalize the accelerometer reading into the z column for (i = X; i <= Z; i++) { fR[i][Z] = fGp[i] * frecipmodGxyz; } // construct x column of orientation matrix fR[X][X] = fmodGyz * frecipmodGxyz; fR[Y][X] = -fR[X][Z] * fR[Y][Z] * ftmp; fR[Z][X] = -fR[X][Z] * fR[Z][Z] * ftmp; // // construct y column of orientation matrix fR[X][Y] = 0.0F; fR[Y][Y] = fR[Z][Z] * ftmp; fR[Z][Y] = -fR[Y][Z] * ftmp; } // Aerospace NED magnetometer 3DOF flat eCompass function computing rotation matrix fR void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[]) { // local variables float fmodBxy; // modulus of the x, y magnetometer readings // compute the magnitude of the horizontal (x and y) magnetometer reading fmodBxy = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y]); // check for zero field special case where no solution is possible if (fmodBxy == 0.0F) { f3x3matrixAeqI(fR); return; } // define the fixed entries in the z row and column fR[Z][X] = fR[Z][Y] = fR[X][Z] = fR[Y][Z] = 0.0F; fR[Z][Z] = 1.0F; // define the remaining entries fR[X][X] = fR[Y][Y] = fBc[X] / fmodBxy; fR[Y][X] = fBc[Y] / fmodBxy; fR[X][Y] = -fR[Y][X]; } // NED: 6DOF e-Compass function computing rotation matrix fR static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[]) { // local variables float fmod[3]; // column moduli float fmodBc; // modulus of Bc float fGdotBc; // dot product of vectors G.Bc float ftmp; // scratch variable int8_t i, j; // loop counters // set the inclination angle to zero in case it is not computed later *pfDelta = 0.0F; // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and x axes for (i = X; i <= Z; i++) { fR[i][Z] = fGp[i]; fR[i][X] = fBc[i]; } // set y vector to vector product of z and x vectors fR[X][Y] = fR[Y][Z] * fR[Z][X] - fR[Z][Z] * fR[Y][X]; fR[Y][Y] = fR[Z][Z] * fR[X][X] - fR[X][Z] * fR[Z][X]; fR[Z][Y] = fR[X][Z] * fR[Y][X] - fR[Y][Z] * fR[X][X]; // set x vector to vector product of y and z vectors fR[X][X] = fR[Y][Y] * fR[Z][Z] - fR[Z][Y] * fR[Y][Z]; fR[Y][X] = fR[Z][Y] * fR[X][Z] - fR[X][Y] * fR[Z][Z]; fR[Z][X] = fR[X][Y] * fR[Y][Z] - fR[Y][Y] * fR[X][Z]; // calculate the rotation matrix column moduli fmod[X] = sqrtf(fR[X][X] * fR[X][X] + fR[Y][X] * fR[Y][X] + fR[Z][X] * fR[Z][X]); fmod[Y] = sqrtf(fR[X][Y] * fR[X][Y] + fR[Y][Y] * fR[Y][Y] + fR[Z][Y] * fR[Z][Y]); fmod[Z] = sqrtf(fR[X][Z] * fR[X][Z] + fR[Y][Z] * fR[Y][Z] + fR[Z][Z] * fR[Z][Z]); // normalize the rotation matrix columns if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F))) { // loop over columns j for (j = X; j <= Z; j++) { ftmp = 1.0F / fmod[j]; // loop over rows i for (i = X; i <= Z; i++) { // normalize by the column modulus fR[i][j] *= ftmp; } } } else { // no solution is possible to set rotation to identity matrix f3x3matrixAeqI(fR); return; } // compute the geomagnetic inclination angle fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]); fGdotBc = fGp[X] * fBc[X] + fGp[Y] * fBc[Y] + fGp[Z] * fBc[Z]; if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F))) { *pfDelta = fasin_deg(fGdotBc / (fmod[Z] * fmodBc)); } } // extract the NED angles in degrees from the NED rotation matrix static void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg) { // calculate the pitch angle -90.0 <= Theta <= 90.0 deg *pfTheDeg = fasin_deg(-R[X][Z]); // calculate the roll angle range -180.0 <= Phi < 180.0 deg *pfPhiDeg = fatan2_deg(R[Y][Z], R[Z][Z]); // map +180 roll onto the functionally equivalent -180 deg roll if (*pfPhiDeg == 180.0F) { *pfPhiDeg = -180.0F; } // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg if (*pfTheDeg == 90.0F) { // vertical upwards gimbal lock case *pfPsiDeg = fatan2_deg(R[Z][Y], R[Y][Y]) + *pfPhiDeg; } else if (*pfTheDeg == -90.0F) { // vertical downwards gimbal lock case *pfPsiDeg = fatan2_deg(-R[Z][Y], R[Y][Y]) - *pfPhiDeg; } else { // general case *pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]); } // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg if (*pfPsiDeg < 0.0F) { *pfPsiDeg += 360.0F; } // check for rounding errors mapping small negative angle to 360 deg if (*pfPsiDeg >= 360.0F) { *pfPsiDeg = 0.0F; } // for NED, the compass heading Rho equals the yaw angle Psi *pfRhoDeg = *pfPsiDeg; // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg) *pfChiDeg = facos_deg(R[Z][Z]); return; } // computes normalized rotation quaternion from a rotation vector (deg) static void fQuaternionFromRotationVectorDeg(Quaternion_t *pq, const float rvecdeg[], float fscaling) { float fetadeg; // rotation angle (deg) float fetarad; // rotation angle (rad) float fetarad2; // eta (rad)^2 float fetarad4; // eta (rad)^4 float sinhalfeta; // sin(eta/2) float fvecsq; // q1^2+q2^2+q3^2 float ftmp; // scratch variable // compute the scaled rotation angle eta (deg) which can be both positve or negative fetadeg = fscaling * sqrtf(rvecdeg[X] * rvecdeg[X] + rvecdeg[Y] * rvecdeg[Y] + rvecdeg[Z] * rvecdeg[Z]); fetarad = fetadeg * FDEGTORAD; fetarad2 = fetarad * fetarad; // calculate the sine and cosine using small angle approximations or exact // angles under sqrt(0.02)=0.141 rad is 8.1 deg and 1620 deg/s (=936deg/s in 3 axes) at 200Hz and 405 deg/s at 50Hz if (fetarad2 <= 0.02F) { // use MacLaurin series up to and including third order sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2); } else if (fetarad2 <= 0.06F) { // use MacLaurin series up to and including fifth order // angles under sqrt(0.06)=0.245 rad is 14.0 deg and 2807 deg/s (=1623deg/s in 3 axes) at 200Hz and 703 deg/s at 50Hz fetarad4 = fetarad2 * fetarad2; sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2 + ONEOVER3840 * fetarad4); } else { // use exact calculation sinhalfeta = (float)sinf(0.5F * fetarad); } // compute the vector quaternion components q1, q2, q3 if (fetadeg != 0.0F) { // general case with non-zero rotation angle ftmp = fscaling * sinhalfeta / fetadeg; pq->q1 = rvecdeg[X] * ftmp; // q1 = nx * sin(eta/2) pq->q2 = rvecdeg[Y] * ftmp; // q2 = ny * sin(eta/2) pq->q3 = rvecdeg[Z] * ftmp; // q3 = nz * sin(eta/2) } else { // zero rotation angle giving zero vector component pq->q1 = pq->q2 = pq->q3 = 0.0F; } // compute the scalar quaternion component q0 by explicit normalization // taking care to avoid rounding errors giving negative operand to sqrt fvecsq = pq->q1 * pq->q1 + pq->q2 * pq->q2 + pq->q3 * pq->q3; if (fvecsq <= 1.0F) { // normal case pq->q0 = sqrtf(1.0F - fvecsq); } else { // rounding errors are present pq->q0 = 0.0F; } } // compute the orientation quaternion from a 3x3 rotation matrix static void fQuaternionFromRotationMatrix(float R[][3], Quaternion_t *pq) { float fq0sq; // q0^2 float recip4q0; // 1/4q0 // the quaternion is not explicitly normalized in this function on the assumption that it // is supplied with a normalized rotation matrix. if the rotation matrix is normalized then // the quaternion will also be normalized even if the case of small q0 // get q0^2 and q0 fq0sq = 0.25F * (1.0F + R[X][X] + R[Y][Y] + R[Z][Z]); pq->q0 = sqrtf(fabs(fq0sq)); // normal case when q0 is not small meaning rotation angle not near 180 deg if (pq->q0 > SMALLQ0) { // calculate q1 to q3 (general case) recip4q0 = 0.25F / pq->q0; pq->q1 = recip4q0 * (R[Y][Z] - R[Z][Y]); pq->q2 = recip4q0 * (R[Z][X] - R[X][Z]); pq->q3 = recip4q0 * (R[X][Y] - R[Y][X]); } else { // special case of near 180 deg corresponds to nearly symmetric matrix // which is not numerically well conditioned for division by small q0 // instead get absolute values of q1 to q3 from leading diagonal pq->q1 = sqrtf(fabs(0.5F * (1.0F + R[X][X]) - fq0sq)); pq->q2 = sqrtf(fabs(0.5F * (1.0F + R[Y][Y]) - fq0sq)); pq->q3 = sqrtf(fabs(0.5F * (1.0F + R[Z][Z]) - fq0sq)); // correct the signs of q1 to q3 by examining the signs of differenced off-diagonal terms if ((R[Y][Z] - R[Z][Y]) < 0.0F) pq->q1 = -pq->q1; if ((R[Z][X] - R[X][Z]) < 0.0F) pq->q2 = -pq->q2; if ((R[X][Y] - R[Y][X]) < 0.0F) pq->q3 = -pq->q3; } } // compute the rotation matrix from an orientation quaternion static void fRotationMatrixFromQuaternion(float R[][3], const Quaternion_t *pq) { float f2q; float f2q0q0, f2q0q1, f2q0q2, f2q0q3; float f2q1q1, f2q1q2, f2q1q3; float f2q2q2, f2q2q3; float f2q3q3; // calculate products f2q = 2.0F * pq->q0; f2q0q0 = f2q * pq->q0; f2q0q1 = f2q * pq->q1; f2q0q2 = f2q * pq->q2; f2q0q3 = f2q * pq->q3; f2q = 2.0F * pq->q1; f2q1q1 = f2q * pq->q1; f2q1q2 = f2q * pq->q2; f2q1q3 = f2q * pq->q3; f2q = 2.0F * pq->q2; f2q2q2 = f2q * pq->q2; f2q2q3 = f2q * pq->q3; f2q3q3 = 2.0F * pq->q3 * pq->q3; // calculate the rotation matrix assuming the quaternion is normalized R[X][X] = f2q0q0 + f2q1q1 - 1.0F; R[X][Y] = f2q1q2 + f2q0q3; R[X][Z] = f2q1q3 - f2q0q2; R[Y][X] = f2q1q2 - f2q0q3; R[Y][Y] = f2q0q0 + f2q2q2 - 1.0F; R[Y][Z] = f2q2q3 + f2q0q1; R[Z][X] = f2q1q3 + f2q0q2; R[Z][Y] = f2q2q3 - f2q0q1; R[Z][Z] = f2q0q0 + f2q3q3 - 1.0F; } // function calculate the rotation vector from a rotation matrix void fRotationVectorDegFromRotationMatrix(float R[][3], float rvecdeg[]) { float ftrace; // trace of the rotation matrix float fetadeg; // rotation angle eta (deg) float fmodulus; // modulus of axis * angle vector = 2|sin(eta)| float ftmp; // scratch variable // calculate the trace of the rotation matrix = 1+2cos(eta) in range -1 to +3 // and eta (deg) in range 0 to 180 deg inclusive // checking for rounding errors that might take the trace outside this range ftrace = R[X][X] + R[Y][Y] + R[Z][Z]; if (ftrace >= 3.0F) { fetadeg = 0.0F; } else if (ftrace <= -1.0F) { fetadeg = 180.0F; } else { fetadeg = acosf(0.5F * (ftrace - 1.0F)) * FRADTODEG; } // set the rvecdeg vector to differences across the diagonal = 2*n*sin(eta) // and calculate its modulus equal to 2|sin(eta)| // the modulus approaches zero near 0 and 180 deg (when sin(eta) approaches zero) rvecdeg[X] = R[Y][Z] - R[Z][Y]; rvecdeg[Y] = R[Z][X] - R[X][Z]; rvecdeg[Z] = R[X][Y] - R[Y][X]; fmodulus = sqrtf(rvecdeg[X] * rvecdeg[X] + rvecdeg[Y] * rvecdeg[Y] + rvecdeg[Z] * rvecdeg[Z]); // normalize the rotation vector for general, 0 deg and 180 deg rotation cases if (fmodulus > SMALLMODULUS) { // general case away from 0 and 180 deg rotation ftmp = fetadeg / fmodulus; rvecdeg[X] *= ftmp; // set x component to eta(deg) * nx rvecdeg[Y] *= ftmp; // set y component to eta(deg) * ny rvecdeg[Z] *= ftmp; // set z component to eta(deg) * nz } else if (ftrace >= 0.0F) { // near 0 deg rotation (trace = 3): matrix is nearly identity matrix // R[Y][Z]-R[Z][Y]=2*nx*eta(rad) and similarly for other components ftmp = 0.5F * FRADTODEG; rvecdeg[X] *= ftmp; rvecdeg[Y] *= ftmp; rvecdeg[Z] *= ftmp; } else { // near 180 deg (trace = -1): matrix is nearly symmetric // calculate the absolute value of the components of the axis-angle vector rvecdeg[X] = 180.0F * sqrtf(fabs(0.5F * (R[X][X] + 1.0F))); rvecdeg[Y] = 180.0F * sqrtf(fabs(0.5F * (R[Y][Y] + 1.0F))); rvecdeg[Z] = 180.0F * sqrtf(fabs(0.5F * (R[Z][Z] + 1.0F))); // correct the signs of the three components by examining the signs of differenced off-diagonal terms if ((R[Y][Z] - R[Z][Y]) < 0.0F) rvecdeg[X] = -rvecdeg[X]; if ((R[Z][X] - R[X][Z]) < 0.0F) rvecdeg[Y] = -rvecdeg[Y]; if ((R[X][Y] - R[Y][X]) < 0.0F) rvecdeg[Z] = -rvecdeg[Z]; } } // computes rotation vector (deg) from rotation quaternion static void fRotationVectorDegFromQuaternion(Quaternion_t *pq, float rvecdeg[]) { float fetarad; // rotation angle (rad) float fetadeg; // rotation angle (deg) float sinhalfeta; // sin(eta/2) float ftmp; // scratch variable // calculate the rotation angle in the range 0 <= eta < 360 deg if ((pq->q0 >= 1.0F) || (pq->q0 <= -1.0F)) { // rotation angle is 0 deg or 2*180 deg = 360 deg = 0 deg fetarad = 0.0F; fetadeg = 0.0F; } else { // general case returning 0 < eta < 360 deg fetarad = 2.0F * acosf(pq->q0); fetadeg = fetarad * FRADTODEG; } // map the rotation angle onto the range -180 deg <= eta < 180 deg if (fetadeg >= 180.0F) { fetadeg -= 360.0F; fetarad = fetadeg * FDEGTORAD; } // calculate sin(eta/2) which will be in the range -1 to +1 sinhalfeta = (float)sinf(0.5F * fetarad); // calculate the rotation vector (deg) if (sinhalfeta == 0.0F) { // the rotation angle eta is zero and the axis is irrelevant rvecdeg[X] = rvecdeg[Y] = rvecdeg[Z] = 0.0F; } else { // general case with non-zero rotation angle ftmp = fetadeg / sinhalfeta; rvecdeg[X] = pq->q1 * ftmp; rvecdeg[Y] = pq->q2 * ftmp; rvecdeg[Z] = pq->q3 * ftmp; } } #if 0 // function low pass filters an orientation quaternion and computes virtual gyro rotation rate void fLPFOrientationQuaternion(Quaternion_t *pq, Quaternion_t *pLPq, float flpf, float fdeltat, float fOmega[], int32_t loopcounter) { // local variables Quaternion_t fdeltaq; // delta rotation quaternion float rvecdeg[3]; // rotation vector (deg) float ftmp; // scratch variable // initialize delay line on first pass: LPq[n]=q[n] if (loopcounter == 0) { *pLPq = *pq; } // set fdeltaqn to the delta rotation quaternion conjg(fLPq[n-1) . fqn fdeltaq = qconjgAxB(pLPq, pq); if (fdeltaq.q0 < 0.0F) { fdeltaq.q0 = -fdeltaq.q0; fdeltaq.q1 = -fdeltaq.q1; fdeltaq.q2 = -fdeltaq.q2; fdeltaq.q3 = -fdeltaq.q3; } // set ftmp to a scaled lpf value which equals flpf in the limit of small rotations (q0=1) // but which rises as the delta rotation angle increases (q0 tends to zero) ftmp = flpf + 0.75F * (1.0F - fdeltaq.q0); if (ftmp > 1.0F) { ftmp = 1.0F; } // scale the delta rotation by the corrected lpf value fdeltaq.q1 *= ftmp; fdeltaq.q2 *= ftmp; fdeltaq.q3 *= ftmp; // compute the scalar component q0 ftmp = fdeltaq.q1 * fdeltaq.q1 + fdeltaq.q2 * fdeltaq.q2 + fdeltaq.q3 * fdeltaq.q3; if (ftmp <= 1.0F) { // normal case fdeltaq.q0 = sqrtf(1.0F - ftmp); } else { // rounding errors present so simply set scalar component to 0 fdeltaq.q0 = 0.0F; } // calculate the delta rotation vector from fdeltaqn and the virtual gyro angular velocity (deg/s) fRotationVectorDegFromQuaternion(&fdeltaq, rvecdeg); ftmp = 1.0F / fdeltat; fOmega[X] = rvecdeg[X] * ftmp; fOmega[Y] = rvecdeg[Y] * ftmp; fOmega[Z] = rvecdeg[Z] * ftmp; // set LPq[n] = LPq[n-1] . deltaq[n] qAeqAxB(pLPq, &fdeltaq); // renormalize the low pass filtered quaternion to prevent error accumulation // the renormalization function ensures that q0 is non-negative fqAeqNormqA(pLPq); } // function low pass filters a scalar void fLPFScalar(float *pfS, float *pfLPS, float flpf, int32_t loopcounter) { // set S[LP,n]=S[n] on first pass if (loopcounter == 0) { *pfLPS = *pfS; } // apply the exponential low pass filter *pfLPS += flpf * (*pfS - *pfLPS); } #endif // function compute the quaternion product qA * qB static void qAeqBxC(Quaternion_t *pqA, const Quaternion_t *pqB, const Quaternion_t *pqC) { pqA->q0 = pqB->q0 * pqC->q0 - pqB->q1 * pqC->q1 - pqB->q2 * pqC->q2 - pqB->q3 * pqC->q3; pqA->q1 = pqB->q0 * pqC->q1 + pqB->q1 * pqC->q0 + pqB->q2 * pqC->q3 - pqB->q3 * pqC->q2; pqA->q2 = pqB->q0 * pqC->q2 - pqB->q1 * pqC->q3 + pqB->q2 * pqC->q0 + pqB->q3 * pqC->q1; pqA->q3 = pqB->q0 * pqC->q3 + pqB->q1 * pqC->q2 - pqB->q2 * pqC->q1 + pqB->q3 * pqC->q0; } // function compute the quaternion product qA = qA * qB static void qAeqAxB(Quaternion_t *pqA, const Quaternion_t *pqB) { Quaternion_t qProd; // perform the quaternion product qProd.q0 = pqA->q0 * pqB->q0 - pqA->q1 * pqB->q1 - pqA->q2 * pqB->q2 - pqA->q3 * pqB->q3; qProd.q1 = pqA->q0 * pqB->q1 + pqA->q1 * pqB->q0 + pqA->q2 * pqB->q3 - pqA->q3 * pqB->q2; qProd.q2 = pqA->q0 * pqB->q2 - pqA->q1 * pqB->q3 + pqA->q2 * pqB->q0 + pqA->q3 * pqB->q1; qProd.q3 = pqA->q0 * pqB->q3 + pqA->q1 * pqB->q2 - pqA->q2 * pqB->q1 + pqA->q3 * pqB->q0; // copy the result back into qA *pqA = qProd; } #if 0 // function compute the quaternion product conjg(qA) * qB static Quaternion_t qconjgAxB(const Quaternion_t *pqA, const Quaternion_t *pqB) { Quaternion_t qProd; qProd.q0 = pqA->q0 * pqB->q0 + pqA->q1 * pqB->q1 + pqA->q2 * pqB->q2 + pqA->q3 * pqB->q3; qProd.q1 = pqA->q0 * pqB->q1 - pqA->q1 * pqB->q0 - pqA->q2 * pqB->q3 + pqA->q3 * pqB->q2; qProd.q2 = pqA->q0 * pqB->q2 + pqA->q1 * pqB->q3 - pqA->q2 * pqB->q0 - pqA->q3 * pqB->q1; qProd.q3 = pqA->q0 * pqB->q3 - pqA->q1 * pqB->q2 + pqA->q2 * pqB->q1 - pqA->q3 * pqB->q0; return qProd; } #endif // function normalizes a rotation quaternion and ensures q0 is non-negative static void fqAeqNormqA(Quaternion_t *pqA) { float fNorm; // quaternion Norm // calculate the quaternion Norm fNorm = sqrtf(pqA->q0 * pqA->q0 + pqA->q1 * pqA->q1 + pqA->q2 * pqA->q2 + pqA->q3 * pqA->q3); if (fNorm > CORRUPTQUAT) { // general case fNorm = 1.0F / fNorm; pqA->q0 *= fNorm; pqA->q1 *= fNorm; pqA->q2 *= fNorm; pqA->q3 *= fNorm; } else { // return with identity quaternion since the quaternion is corrupted pqA->q0 = 1.0F; pqA->q1 = pqA->q2 = pqA->q3 = 0.0F; } // correct a negative scalar component if the function was called with negative q0 if (pqA->q0 < 0.0F) { pqA->q0 = -pqA->q0; pqA->q1 = -pqA->q1; pqA->q2 = -pqA->q2; pqA->q3 = -pqA->q3; } } // set a quaternion to the unit quaternion static void fqAeq1(Quaternion_t *pqA) { pqA->q0 = 1.0F; pqA->q1 = pqA->q2 = pqA->q3 = 0.0F; } // function returns an approximation to angle(deg)=asin(x) for x in the range -1 <= x <= 1 // and returns -90 <= angle <= 90 deg // maximum error is 10.29E-6 deg static float fasin_deg(float x) { // for robustness, check for invalid argument if (x >= 1.0F) return 90.0F; if (x <= -1.0F) return -90.0F; // call the atan which will return an angle in the correct range -90 to 90 deg // this line cannot fail from division by zero or negative square root since |x| < 1 return (fatan_deg(x / sqrtf(1.0F - x * x))); } // function returns an approximation to angle(deg)=acos(x) for x in the range -1 <= x <= 1 // and returns 0 <= angle <= 180 deg // maximum error is 14.67E-6 deg static float facos_deg(float x) { // for robustness, check for invalid arguments if (x >= 1.0F) return 0.0F; if (x <= -1.0F) return 180.0F; // call the atan which will return an angle in the incorrect range -90 to 90 deg // these lines cannot fail from division by zero or negative square root if (x == 0.0F) return 90.0F; if (x > 0.0F) return fatan_deg((sqrtf(1.0F - x * x) / x)); return 180.0F + fatan_deg((sqrtf(1.0F - x * x) / x)); } // function returns angle in range -90 to 90 deg // maximum error is 9.84E-6 deg static float fatan_deg(float x) { float fangledeg; // compute computed (deg) int8_t ixisnegative; // argument x is negative int8_t ixexceeds1; // argument x is greater than 1.0 int8_t ixmapped; // argument in range tan(15 deg) to tan(45 deg)=1.0 #define TAN15DEG 0.26794919243F // tan(15 deg) = 2 - sqrt(3) #define TAN30DEG 0.57735026919F // tan(30 deg) = 1/sqrt(3) // reset all flags ixisnegative = ixexceeds1 = ixmapped = 0; // test for negative argument to allow use of tan(-x)=-tan(x) if (x < 0.0F) { x = -x; ixisnegative = 1; } // test for argument above 1 to allow use of atan(x)=pi/2-atan(1/x) if (x > 1.0F) { x = 1.0F / x; ixexceeds1 = 1; } // at this point, x is in the range 0 to 1 inclusive // map argument onto range -tan(15 deg) to tan(15 deg) // using tan(angle-30deg) = (tan(angle)-tan(30deg)) / (1 + tan(angle)tan(30deg)) // tan(15deg) maps to tan(-15 deg) = -tan(15 deg) // 1. maps to (sqrt(3) - 1) / (sqrt(3) + 1) = 2 - sqrt(3) = tan(15 deg) if (x > TAN15DEG) { x = (x - TAN30DEG)/(1.0F + TAN30DEG * x); ixmapped = 1; } // call the atan estimator to obtain -15 deg <= angle <= 15 deg fangledeg = fatan_15deg(x); // undo the distortions applied earlier to obtain -90 deg <= angle <= 90 deg if (ixmapped) fangledeg += 30.0F; if (ixexceeds1) fangledeg = 90.0F - fangledeg; if (ixisnegative) fangledeg = -fangledeg; return (fangledeg); } // function returns approximate atan2 angle in range -180 to 180 deg // maximum error is 14.58E-6 deg static float fatan2_deg(float y, float x) { // check for zero x to avoid division by zero if (x == 0.0F) { // return 90 deg for positive y if (y > 0.0F) return 90.0F; // return -90 deg for negative y if (y < 0.0F) return -90.0F; // otherwise y= 0.0 and return 0 deg (invalid arguments) return 0.0F; } // from here onwards, x is guaranteed to be non-zero // compute atan2 for quadrant 1 (0 to 90 deg) and quadrant 4 (-90 to 0 deg) if (x > 0.0F) return (fatan_deg(y / x)); // compute atan2 for quadrant 2 (90 to 180 deg) if ((x < 0.0F) && (y > 0.0F)) return (180.0F + fatan_deg(y / x)); // compute atan2 for quadrant 3 (-180 to -90 deg) return (-180.0F + fatan_deg(y / x)); } // approximation to inverse tan function (deg) for x in range // -tan(15 deg) to tan(15 deg) giving an output -15 deg <= angle <= 15 deg // using modified Pade[3/2] approximation static float fatan_15deg(float x) { float x2; // x^2 #define PADE_A 96.644395816F // theoretical Pade[3/2] value is 5/3*180/PI=95.49296 #define PADE_B 25.086941612F // theoretical Pade[3/2] value is 4/9*180/PI=25.46479 #define PADE_C 1.6867633134F // theoretical Pade[3/2] value is 5/3=1.66667 // compute the approximation to the inverse tangent // the function is anti-symmetric as required for positive and negative arguments x2 = x * x; return (x * (PADE_A + x2 * PADE_B) / (PADE_C + x2)); }