PlatformIO package of the Teensy core framework compatible with GCC 10 & C++20
Вы не можете выбрать более 25 тем Темы должны начинаться с буквы или цифры, могут содержать дефисы(-) и должны содержать не более 35 символов.

3 лет назад
1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441
  1. // Copyright (c) 2014, Freescale Semiconductor, Inc.
  2. // All rights reserved.
  3. // vim: set ts=4:
  4. //
  5. // Redistribution and use in source and binary forms, with or without
  6. // modification, are permitted provided that the following conditions are met:
  7. // * Redistributions of source code must retain the above copyright
  8. // notice, this list of conditions and the following disclaimer.
  9. // * Redistributions in binary form must reproduce the above copyright
  10. // notice, this list of conditions and the following disclaimer in the
  11. // documentation and/or other materials provided with the distribution.
  12. // * Neither the name of Freescale Semiconductor, Inc. nor the
  13. // names of its contributors may be used to endorse or promote products
  14. // derived from this software without specific prior written permission.
  15. //
  16. // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  17. // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  18. // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19. // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
  20. // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  21. // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  22. // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  23. // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24. // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25. // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26. //
  27. // This is the file that contains the fusion routines. It is STRONGLY RECOMMENDED
  28. // that the casual developer NOT TOUCH THIS FILE. The mathematics behind this file
  29. // is extremely complex, and it will be very easy (almost inevitable) that you screw
  30. // it up.
  31. //
  32. #include "NXPMotionSense.h"
  33. // kalman filter noise variances
  34. #define FQVA_9DOF_GBY_KALMAN 2E-6F // accelerometer noise g^2 so 1.4mg RMS
  35. #define FQVM_9DOF_GBY_KALMAN 0.1F // magnetometer noise uT^2
  36. #define FQVG_9DOF_GBY_KALMAN 0.3F // gyro noise (deg/s)^2
  37. #define FQWB_9DOF_GBY_KALMAN 1E-9F // gyro offset drift (deg/s)^2: 1E-9 implies 0.09deg/s max at 50Hz
  38. #define FQWA_9DOF_GBY_KALMAN 1E-4F // linear acceleration drift g^2 (increase slows convergence to g but reduces sensitivity to shake)
  39. #define FQWD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance drift uT^2 (increase slows convergence to B but reduces sensitivity to magnet)
  40. // initialization of Qw covariance matrix
  41. #define FQWINITTHTH_9DOF_GBY_KALMAN 2000E-5F // th_e * th_e terms
  42. #define FQWINITBB_9DOF_GBY_KALMAN 250E-3F // b_e * b_e terms
  43. #define FQWINITTHB_9DOF_GBY_KALMAN 0.0F // th_e * b_e terms
  44. #define FQWINITAA_9DOF_GBY_KALMAN 10E-5F // a_e * a_e terms (increase slows convergence to g but reduces sensitivity to shake)
  45. #define FQWINITDD_9DOF_GBY_KALMAN 600E-3F // d_e * d_e terms (increase slows convergence to B but reduces sensitivity to magnet)
  46. // linear acceleration and magnetic disturbance time constants
  47. #define FCA_9DOF_GBY_KALMAN 0.5F // linear acceleration decay factor
  48. #define FCD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance decay factor
  49. // maximum geomagnetic inclination angle tracked by Kalman filter
  50. #define SINDELTAMAX 0.9063078F // sin of max +ve geomagnetic inclination angle: here 65.0 deg
  51. #define COSDELTAMAX 0.4226183F // cos of max +ve geomagnetic inclination angle: here 65.0 deg
  52. #define DEFAULTB 50.0F // default geomagnetic field (uT)
  53. #define X 0 // vector components
  54. #define Y 1
  55. #define Z 2
  56. #define FDEGTORAD 0.01745329251994F // degrees to radians conversion = pi / 180
  57. #define FRADTODEG 57.2957795130823F // radians to degrees conversion = 180 / pi
  58. #define ONEOVER48 0.02083333333F // 1 / 48
  59. #define ONEOVER3840 0.0002604166667F // 1 / 3840
  60. #define Quaternion_t NXPSensorFusion::Quaternion_t
  61. static void fqAeq1(Quaternion_t *pqA);
  62. static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[]);
  63. static void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg,
  64. float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg);
  65. static void fQuaternionFromRotationMatrix(float R[][3], Quaternion_t *pq);
  66. static void fQuaternionFromRotationVectorDeg(Quaternion_t *pq, const float rvecdeg[], float fscaling);
  67. static void fRotationMatrixFromQuaternion(float R[][3], const Quaternion_t *pq);
  68. static void fRotationVectorDegFromQuaternion(Quaternion_t *pq, float rvecdeg[]);
  69. static void qAeqAxB(Quaternion_t *pqA, const Quaternion_t *pqB);
  70. static void qAeqBxC(Quaternion_t *pqA, const Quaternion_t *pqB, const Quaternion_t *pqC);
  71. //static Quaternion_t qconjgAxB(const Quaternion_t *pqA, const Quaternion_t *pqB);
  72. static void fqAeqNormqA(Quaternion_t *pqA);
  73. static float fasin_deg(float x);
  74. static float facos_deg(float x);
  75. static float fatan_deg(float x);
  76. static float fatan2_deg(float y, float x);
  77. static float fatan_15deg(float x);
  78. extern "C"{
  79. void f3x3matrixAeqI(float A[][3]);
  80. void fmatrixAeqI(float *A[], int16_t rc);
  81. void f3x3matrixAeqScalar(float A[][3], float Scalar);
  82. void f3x3matrixAeqInvSymB(float A[][3], float B[][3]);
  83. void f3x3matrixAeqAxScalar(float A[][3], float Scalar);
  84. void f3x3matrixAeqMinusA(float A[][3]);
  85. float f3x3matrixDetA(float A[][3]);
  86. void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n);
  87. void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[], int8_t iPivot[], int8_t isize);
  88. void fmatrixAeqRenormRotA(float A[][3]);
  89. }
  90. // initialize the 9DOF Kalman filter
  91. void NXPSensorFusion::begin(float sampleRate)
  92. {
  93. int8_t i, j;
  94. // reset the flag denoting that a first 9DOF orientation lock has been achieved
  95. FirstOrientationLock = 0;
  96. // compute and store useful product terms to save floating point calculations later
  97. Fastdeltat = 1.0f / sampleRate;
  98. deltat = Fastdeltat;
  99. deltatsq = deltat * deltat;
  100. casq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN;
  101. cdsq = FCD_9DOF_GBY_KALMAN * FCD_9DOF_GBY_KALMAN;
  102. QwbplusQvG = FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN;
  103. // initialize the fixed entries in the measurement matrix C
  104. for (i = 0; i < 6; i++) {
  105. for (j = 0; j < 12; j++) {
  106. C6x12[i][j]= 0.0F;
  107. }
  108. }
  109. C6x12[0][6] = C6x12[1][7] = C6x12[2][8] = 1.0F;
  110. C6x12[3][9] = C6x12[4][10] = C6x12[5][11] = -1.0F;
  111. // zero a posteriori orientation, error vector xe+ (thetae+, be+, de+, ae+) and b+ and inertial
  112. f3x3matrixAeqI(RPl);
  113. fqAeq1(&qPl);
  114. for (i = X; i <= Z; i++) {
  115. ThErrPl[i] = bErrPl[i] = aErrSePl[i] = dErrSePl[i] = bPl[i] = 0.0F;
  116. }
  117. // initialize the reference geomagnetic vector (uT, global frame)
  118. DeltaPl = 0.0F;
  119. // initialize NED geomagnetic vector to zero degrees inclination
  120. mGl[X] = DEFAULTB;
  121. mGl[Y] = 0.0F;
  122. mGl[Z] = 0.0F;
  123. // initialize noise variances for Qv and Qw matrix updates
  124. QvAA = FQVA_9DOF_GBY_KALMAN + FQWA_9DOF_GBY_KALMAN +
  125. FDEGTORAD * FDEGTORAD * deltatsq * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
  126. QvMM = FQVM_9DOF_GBY_KALMAN + FQWD_9DOF_GBY_KALMAN +
  127. FDEGTORAD * FDEGTORAD * deltatsq * DEFAULTB * DEFAULTB *
  128. (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
  129. // initialize the 12x12 noise covariance matrix Qw of the a priori error vector xe-
  130. // Qw is then recursively updated as P+ = (1 - K * C) * P- = (1 - K * C) * Qw and Qw
  131. // updated from P+
  132. // zero the matrix Qw
  133. for (i = 0; i < 12; i++) {
  134. for (j = 0; j < 12; j++) {
  135. Qw12x12[i][j] = 0.0F;
  136. }
  137. }
  138. // loop over non-zero values
  139. for (i = 0; i < 3; i++) {
  140. // theta_e * theta_e terms
  141. Qw12x12[i][i] = FQWINITTHTH_9DOF_GBY_KALMAN;
  142. // b_e * b_e terms
  143. Qw12x12[i + 3][i + 3] = FQWINITBB_9DOF_GBY_KALMAN;
  144. // th_e * b_e terms
  145. Qw12x12[i][i + 3] = Qw12x12[i + 3][i] = FQWINITTHB_9DOF_GBY_KALMAN;
  146. // a_e * a_e terms
  147. Qw12x12[i + 6][i + 6] = FQWINITAA_9DOF_GBY_KALMAN;
  148. // d_e * d_e terms
  149. Qw12x12[i + 9][i + 9] = FQWINITDD_9DOF_GBY_KALMAN;
  150. }
  151. // clear the reset flag
  152. resetflag = 0;
  153. }
  154. // 9DOF orientation function implemented using a 12 element Kalman filter
  155. //void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
  156. //const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
  157. //const MagCalibration_t *MagCal)
  158. void NXPSensorFusion::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
  159. {
  160. float Accel[3]={ax, ay, az}; // Accel
  161. float Yp[3]={gx, gy, gz}; // Gryo
  162. float Mag[3]={mx, my, mz}; // Mag
  163. // local scalars and arrays
  164. float fopp, fadj, fhyp; // opposite, adjacent and hypoteneuse
  165. float fsindelta, fcosdelta; // sin and cos of inclination angle delta
  166. float rvec[3]; // rotation vector
  167. float ftmp; // scratch variable
  168. float ftmpA12x6[12][6]; // scratch array
  169. int8_t i, j, k; // loop counters
  170. int8_t iMagJamming; // magnetic jamming flag
  171. int8_t ValidMagCal;
  172. // assorted array pointers
  173. float *pfPPlus12x12ij;
  174. float *pfPPlus12x12kj;
  175. float *pfQw12x12ij;
  176. float *pfQw12x12ik;
  177. float *pfQw12x12kj;
  178. float *pfK12x6ij;
  179. float *pfK12x6ik;
  180. float *pftmpA12x6ik;
  181. float *pftmpA12x6kj;
  182. float *pftmpA12x6ij;
  183. float *pfC6x12ik;
  184. float *pfC6x12jk;
  185. // working arrays for 6x6 matrix inversion
  186. float *pfRows[6];
  187. int8_t iColInd[6];
  188. int8_t iRowInd[6];
  189. int8_t iPivot[6];
  190. // do a reset and return if requested
  191. if (resetflag) {
  192. begin(1.0f / deltat);
  193. return;
  194. }
  195. // *********************************************************************************
  196. // initial orientation lock to accelerometer and magnetometer eCompass orientation
  197. // *********************************************************************************
  198. if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {
  199. ValidMagCal = 1;
  200. } else {
  201. ValidMagCal = 0;
  202. }
  203. // do a once-only orientation lock after the first valid magnetic calibration
  204. if (ValidMagCal && !FirstOrientationLock) {
  205. // get the 6DOF orientation matrix and initial inclination angle
  206. feCompassNED(RPl, &DeltaPl, Mag, Accel);
  207. // get the orientation quaternion from the orientation matrix
  208. fQuaternionFromRotationMatrix(RPl, &qPl);
  209. // set the orientation lock flag so this initial alignment is only performed once
  210. FirstOrientationLock = 1;
  211. }
  212. // *********************************************************************************
  213. // calculate a priori rotation matrix
  214. // *********************************************************************************
  215. // compute the angular velocity from the averaged high frequency gyro reading.
  216. // omega[k] = yG[k] - b-[k] = yG[k] - b+[k-1] (deg/s)
  217. Omega[X] = Yp[X] - bPl[X];
  218. Omega[Y] = Yp[Y] - bPl[Y];
  219. Omega[Z] = Yp[Z] - bPl[Z];
  220. // initialize the a priori orientation quaternion to the previous a posteriori estimate
  221. qMi = qPl;
  222. // integrate the buffered high frequency (typically 200Hz) gyro readings
  223. //for (j = 0; j < OVERSAMPLE_RATIO; j++) {
  224. // compute the incremental fast (typically 200Hz) rotation vector rvec (deg)
  225. for (i = X; i <= Z; i++) {
  226. rvec[i] = (Yp[i] - bPl[i]) * Fastdeltat;
  227. }
  228. // compute the incremental quaternion fDeltaq from the rotation vector
  229. fQuaternionFromRotationVectorDeg(&Deltaq, rvec, 1.0F);
  230. // incrementally rotate the a priori orientation quaternion fqMi
  231. // the a posteriori quaternion fqPl is re-normalized later so this update is stable
  232. qAeqAxB(&qMi, &Deltaq);
  233. //}
  234. // get the a priori rotation matrix from the a priori quaternion
  235. fRotationMatrixFromQuaternion(RMi, &qMi);
  236. // *********************************************************************************
  237. // calculate a priori gyro, accelerometer and magnetometer estimates
  238. // of the gravity and geomagnetic vectors and errors
  239. // the most recent 'Fast' measurements are used to reduce phase errors
  240. // *********************************************************************************
  241. for (i = X; i <= Z; i++) {
  242. // compute the a priori gyro estimate of the gravitational vector (g, sensor frame)
  243. // using an absolute rotation of the global frame gravity vector (with magnitude 1g)
  244. // NED gravity is along positive z axis
  245. gSeGyMi[i] = RMi[i][Z];
  246. // compute a priori acceleration (a-) (g, sensor frame) from decayed a
  247. // posteriori estimate (g, sensor frame)
  248. aSeMi[i] = FCA_9DOF_GBY_KALMAN * aSePl[i];
  249. // compute the a priori gravity error vector (accelerometer minus gyro estimates)
  250. // (g, sensor frame)
  251. // NED and Windows 8 have positive sign for gravity: y = g - a and g = y + a
  252. gErrSeMi[i] = Accel[i] + aSeMi[i] - gSeGyMi[i];
  253. // compute the a priori gyro estimate of the geomagnetic vector (uT, sensor frame)
  254. // using an absolute rotation of the global frame geomagnetic vector (with magnitude B uT)
  255. // NED y component of geomagnetic vector in global frame is zero
  256. mSeGyMi[i] = RMi[i][X] * mGl[X] + RMi[i][Z] * mGl[Z];
  257. // compute the a priori geomagnetic error vector (magnetometer minus gyro estimates)
  258. // (g, sensor frame)
  259. mErrSeMi[i] = Mag[i] - mSeGyMi[i];
  260. }
  261. // *********************************************************************************
  262. // update variable elements of measurement matrix C
  263. // *********************************************************************************
  264. // update measurement matrix C with -alpha(g-)x and -alpha(m-)x from gyro (g, uT, sensor frame)
  265. C6x12[0][1] = FDEGTORAD * gSeGyMi[Z];
  266. C6x12[0][2] = -FDEGTORAD * gSeGyMi[Y];
  267. C6x12[1][2] = FDEGTORAD * gSeGyMi[X];
  268. C6x12[1][0] = -C6x12[0][1];
  269. C6x12[2][0] = -C6x12[0][2];
  270. C6x12[2][1] = -C6x12[1][2];
  271. C6x12[3][1] = FDEGTORAD * mSeGyMi[Z];
  272. C6x12[3][2] = -FDEGTORAD * mSeGyMi[Y];
  273. C6x12[4][2] = FDEGTORAD * mSeGyMi[X];
  274. C6x12[4][0] = -C6x12[3][1];
  275. C6x12[5][0] = -C6x12[3][2];
  276. C6x12[5][1] = -C6x12[4][2];
  277. C6x12[0][4] = -deltat * C6x12[0][1];
  278. C6x12[0][5] = -deltat * C6x12[0][2];
  279. C6x12[1][5] = -deltat * C6x12[1][2];
  280. C6x12[1][3]= -C6x12[0][4];
  281. C6x12[2][3]= -C6x12[0][5];
  282. C6x12[2][4]= -C6x12[1][5];
  283. C6x12[3][4] = -deltat * C6x12[3][1];
  284. C6x12[3][5] = -deltat * C6x12[3][2];
  285. C6x12[4][5] = -deltat * C6x12[4][2];
  286. C6x12[4][3] = -C6x12[3][4];
  287. C6x12[5][3] = -C6x12[3][5];
  288. C6x12[5][4] = -C6x12[4][5];
  289. // *********************************************************************************
  290. // calculate the Kalman gain matrix K
  291. // K = P- * C^T * inv(C * P- * C^T + Qv) = Qw * C^T * inv(C * Qw * C^T + Qv)
  292. // Qw is used as a proxy for P- throughout the code
  293. // P+ is used here as a working array to reduce RAM usage and is re-computed later
  294. // *********************************************************************************
  295. // set ftmpA12x6 = P- * C^T = Qw * C^T where Qw and C are both sparse
  296. // C also has a significant number of +1 and -1 entries
  297. // ftmpA12x6 is also sparse but not symmetric
  298. for (i = 0; i < 12; i++) { // loop over rows of ftmpA12x6
  299. // initialize pftmpA12x6ij for current i, j=0
  300. pftmpA12x6ij = ftmpA12x6[i];
  301. for (j = 0; j < 6; j++) { // loop over columns of ftmpA12x6
  302. // zero ftmpA12x6[i][j]
  303. *pftmpA12x6ij = 0.0F;
  304. // initialize pfC6x12jk for current j, k=0
  305. pfC6x12jk = C6x12[j];
  306. // initialize pfQw12x12ik for current i, k=0
  307. pfQw12x12ik = Qw12x12[i];
  308. // sum matrix products over inner loop over k
  309. for (k = 0; k < 12; k++) {
  310. if ((*pfQw12x12ik != 0.0F) && (*pfC6x12jk != 0.0F)) {
  311. if (*pfC6x12jk == 1.0F)
  312. *pftmpA12x6ij += *pfQw12x12ik;
  313. else if (*pfC6x12jk == -1.0F)
  314. *pftmpA12x6ij -= *pfQw12x12ik;
  315. else
  316. *pftmpA12x6ij += *pfQw12x12ik * *pfC6x12jk;
  317. }
  318. // increment pfC6x12jk and pfQw12x12ik for next iteration of k
  319. pfC6x12jk++;
  320. pfQw12x12ik++;
  321. }
  322. // increment pftmpA12x6ij for next iteration of j
  323. pftmpA12x6ij++;
  324. }
  325. }
  326. // set symmetric P+ (6x6 scratch sub-matrix) to C * P- * C^T + Qv
  327. // = C * (Qw * C^T) + Qv = C * ftmpA12x6 + Qv
  328. // both C and ftmpA12x6 are sparse but not symmetric
  329. for (i = 0; i < 6; i++) { // loop over rows of P+
  330. // initialize pfPPlus12x12ij for current i, j=i
  331. pfPPlus12x12ij = PPlus12x12[i] + i;
  332. for (j = i; j < 6; j++) { // loop over above diagonal columns of P+
  333. // zero P+[i][j]
  334. *pfPPlus12x12ij = 0.0F;
  335. // initialize pfC6x12ik for current i, k=0
  336. pfC6x12ik = C6x12[i];
  337. // initialize pftmpA12x6kj for current j, k=0
  338. pftmpA12x6kj = *ftmpA12x6 + j;
  339. // sum matrix products over inner loop over k
  340. for (k = 0; k < 12; k++) {
  341. if ((*pfC6x12ik != 0.0F) && (*pftmpA12x6kj != 0.0F)) {
  342. if (*pfC6x12ik == 1.0F)
  343. *pfPPlus12x12ij += *pftmpA12x6kj;
  344. else if (*pfC6x12ik == -1.0F)
  345. *pfPPlus12x12ij -= *pftmpA12x6kj;
  346. else
  347. *pfPPlus12x12ij += *pfC6x12ik * *pftmpA12x6kj;
  348. }
  349. // update pfC6x12ik and pftmpA12x6kj for next iteration of k
  350. pfC6x12ik++;
  351. pftmpA12x6kj += 6;
  352. }
  353. // increment pfPPlus12x12ij for next iteration of j
  354. pfPPlus12x12ij++;
  355. }
  356. }
  357. // add in noise covariance terms to the diagonal
  358. PPlus12x12[0][0] += QvAA;
  359. PPlus12x12[1][1] += QvAA;
  360. PPlus12x12[2][2] += QvAA;
  361. PPlus12x12[3][3] += QvMM;
  362. PPlus12x12[4][4] += QvMM;
  363. PPlus12x12[5][5] += QvMM;
  364. // copy above diagonal elements of P+ (6x6 scratch sub-matrix) to below diagonal
  365. for (i = 1; i < 6; i++)
  366. for (j = 0; j < i; j++)
  367. PPlus12x12[i][j] = PPlus12x12[j][i];
  368. // calculate inverse of P+ (6x6 scratch sub-matrix) = inv(C * P- * C^T + Qv) = inv(C * Qw * C^T + Qv)
  369. for (i = 0; i < 6; i++) {
  370. pfRows[i] = PPlus12x12[i];
  371. }
  372. fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 3);
  373. // set K = P- * C^T * inv(C * P- * C^T + Qv) = Qw * C^T * inv(C * Qw * C^T + Qv)
  374. // = ftmpA12x6 * P+ (6x6 sub-matrix)
  375. // ftmpA12x6 = Qw * C^T is sparse but P+ (6x6 sub-matrix) is not
  376. // K is not symmetric because C is not symmetric
  377. for (i = 0; i < 12; i++) { // loop over rows of K12x6
  378. // initialize pfK12x6ij for current i, j=0
  379. pfK12x6ij = K12x6[i];
  380. for (j = 0; j < 6; j++) { // loop over columns of K12x6
  381. // zero the matrix element fK12x6[i][j]
  382. *pfK12x6ij = 0.0F;
  383. // initialize pftmpA12x6ik for current i, k=0
  384. pftmpA12x6ik = ftmpA12x6[i];
  385. // initialize pfPPlus12x12kj for current j, k=0
  386. pfPPlus12x12kj = *PPlus12x12 + j;
  387. // sum matrix products over inner loop over k
  388. for (k = 0; k < 6; k++) {
  389. if (*pftmpA12x6ik != 0.0F) {
  390. *pfK12x6ij += *pftmpA12x6ik * *pfPPlus12x12kj;
  391. }
  392. // increment pftmpA12x6ik and pfPPlus12x12kj for next iteration of k
  393. pftmpA12x6ik++;
  394. pfPPlus12x12kj += 12;
  395. }
  396. // increment pfK12x6ij for the next iteration of j
  397. pfK12x6ij++;
  398. }
  399. }
  400. // *********************************************************************************
  401. // calculate a posteriori error estimate: xe+ = K * ze-
  402. // *********************************************************************************
  403. // first calculate all four error vector components using accelerometer error component only
  404. // for fThErrPl, fbErrPl, faErrSePl but also magnetometer for fdErrSePl
  405. for (i = X; i <= Z; i++) {
  406. ThErrPl[i] = K12x6[i][0] * gErrSeMi[X] +
  407. K12x6[i][1] * gErrSeMi[Y] +
  408. K12x6[i][2] * gErrSeMi[Z];
  409. bErrPl[i] = K12x6[i + 3][0] * gErrSeMi[X] +
  410. K12x6[i + 3][1] * gErrSeMi[Y] +
  411. K12x6[i + 3][2] * gErrSeMi[Z];
  412. aErrSePl[i] = K12x6[i + 6][0] * gErrSeMi[X] +
  413. K12x6[i + 6][1] * gErrSeMi[Y] +
  414. K12x6[i + 6][2] * gErrSeMi[Z];
  415. dErrSePl[i] = K12x6[i + 9][0] * gErrSeMi[X] +
  416. K12x6[i + 9][1] * gErrSeMi[Y] +
  417. K12x6[i + 9][2] * gErrSeMi[Z] +
  418. K12x6[i + 9][3] * mErrSeMi[X] +
  419. K12x6[i + 9][4] * mErrSeMi[Y] +
  420. K12x6[i + 9][5] * mErrSeMi[Z];
  421. }
  422. // set the magnetic jamming flag if there is a significant magnetic error power after calibration
  423. ftmp = dErrSePl[X] * dErrSePl[X] + dErrSePl[Y] * dErrSePl[Y] +
  424. dErrSePl[Z] * dErrSePl[Z];
  425. //iMagJamming = (ValidMagCal) && (ftmp > MagCal->FourBsq);
  426. iMagJamming = (ValidMagCal) && (ftmp > (DEFAULTB*DEFAULTB*4.0f)); // TODO: FourBsq....
  427. // add the remaining magnetic error terms if there is calibration and no magnetic jamming
  428. if (ValidMagCal && !iMagJamming) {
  429. for (i = X; i <= Z; i++) {
  430. ThErrPl[i] += K12x6[i][3] * mErrSeMi[X] +
  431. K12x6[i][4] * mErrSeMi[Y] +
  432. K12x6[i][5] * mErrSeMi[Z];
  433. bErrPl[i] += K12x6[i + 3][3] * mErrSeMi[X] +
  434. K12x6[i + 3][4] * mErrSeMi[Y] +
  435. K12x6[i + 3][5] * mErrSeMi[Z];
  436. aErrSePl[i] += K12x6[i + 6][3] * mErrSeMi[X] +
  437. K12x6[i + 6][4] * mErrSeMi[Y] +
  438. K12x6[i + 6][5] * mErrSeMi[Z];
  439. }
  440. }
  441. // *********************************************************************************
  442. // apply the a posteriori error corrections to the a posteriori state vector
  443. // *********************************************************************************
  444. // get the a posteriori delta quaternion
  445. fQuaternionFromRotationVectorDeg(&Deltaq, ThErrPl, -1.0F);
  446. // compute the a posteriori orientation quaternion fqPl = fqMi * Deltaq(-thetae+)
  447. // the resulting quaternion may have negative scalar component q0
  448. qAeqBxC(&qPl, &qMi, &Deltaq);
  449. // normalize the a posteriori orientation quaternion to stop error propagation
  450. // the renormalization function ensures that the scalar component q0 is non-negative
  451. fqAeqNormqA(&qPl);
  452. // compute the a posteriori rotation matrix from the a posteriori quaternion
  453. fRotationMatrixFromQuaternion(RPl, &qPl);
  454. // compute the rotation vector from the a posteriori quaternion
  455. fRotationVectorDegFromQuaternion(&qPl, RVecPl);
  456. // update the a posteriori gyro offset vector b+ and
  457. // assign the entire linear acceleration error vector to update the linear acceleration
  458. for (i = X; i <= Z; i++) {
  459. // b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s)
  460. bPl[i] -= bErrPl[i];
  461. // a+ = a- - ae+ (g, sensor frame)
  462. aSePl[i] = aSeMi[i] - aErrSePl[i];
  463. }
  464. // compute the linear acceleration in the global frame from the accelerometer measurement (sensor frame).
  465. // de-rotate the accelerometer measurement from the sensor to global frame using the inverse (transpose)
  466. // of the a posteriori rotation matrix
  467. aGlPl[X] = RPl[X][X] * Accel[X] + RPl[Y][X] * Accel[Y] + RPl[Z][X] * Accel[Z];
  468. aGlPl[Y] = RPl[X][Y] * Accel[X] + RPl[Y][Y] * Accel[Y] + RPl[Z][Y] * Accel[Z];
  469. aGlPl[Z] = RPl[X][Z] * Accel[X] + RPl[Y][Z] * Accel[Y] + RPl[Z][Z] * Accel[Z];
  470. // remove gravity and correct the sign if the coordinate system is gravity positive / acceleration negative
  471. // gravity positive NED
  472. aGlPl[X] = -aGlPl[X];
  473. aGlPl[Y] = -aGlPl[Y];
  474. aGlPl[Z] = -(aGlPl[Z] - 1.0F);
  475. // update the reference geomagnetic vector using magnetic disturbance error if valid calibration and no jamming
  476. if (ValidMagCal && !iMagJamming) {
  477. // de-rotate the NED magnetic disturbance error de+ from the sensor to the global reference frame
  478. // using the inverse (transpose) of the a posteriori rotation matrix
  479. dErrGlPl[X] = RPl[X][X] * dErrSePl[X] +
  480. RPl[Y][X] * dErrSePl[Y] +
  481. RPl[Z][X] * dErrSePl[Z];
  482. dErrGlPl[Z] = RPl[X][Z] * dErrSePl[X] +
  483. RPl[Y][Z] * dErrSePl[Y] +
  484. RPl[Z][Z] * dErrSePl[Z];
  485. // compute components of the new geomagnetic vector
  486. // the north pointing component fadj must always be non-negative
  487. fopp = mGl[Z] - dErrGlPl[Z];
  488. fadj = mGl[X] - dErrGlPl[X];
  489. if (fadj < 0.0F) {
  490. fadj = 0.0F;
  491. }
  492. fhyp = sqrtf(fopp * fopp + fadj * fadj);
  493. // check for the pathological condition of zero geomagnetic field
  494. if (fhyp != 0.0F) {
  495. // compute the sine and cosine of the new geomagnetic vector
  496. ftmp = 1.0F / fhyp;
  497. fsindelta = fopp * ftmp;
  498. fcosdelta = fadj * ftmp;
  499. // limit the inclination angle between limits to prevent runaway
  500. if (fsindelta > SINDELTAMAX) {
  501. fsindelta = SINDELTAMAX;
  502. fcosdelta = COSDELTAMAX;
  503. } else if (fsindelta < -SINDELTAMAX) {
  504. fsindelta = -SINDELTAMAX;
  505. fcosdelta = COSDELTAMAX;
  506. }
  507. // compute the new geomagnetic vector (always north pointing)
  508. DeltaPl = fasin_deg(fsindelta);
  509. //mGl[X] = MagCal->B * fcosdelta; // TODO: MagCal->B
  510. //mGl[Z] = MagCal->B * fsindelta;
  511. mGl[X] = DEFAULTB * fcosdelta; // TODO: MagCal->B
  512. mGl[Z] = DEFAULTB * fsindelta;
  513. } // end hyp == 0.0F
  514. } // end ValidMagCal
  515. // *********************************************************************************
  516. // compute the a posteriori Euler angles from the orientation matrix
  517. // *********************************************************************************
  518. // calculate the NED Euler angles
  519. fNEDAnglesDegFromRotationMatrix(RPl, &PhiPl, &ThePl, &PsiPl, &RhoPl, &ChiPl);
  520. // ***********************************************************************************
  521. // calculate (symmetric) a posteriori error covariance matrix P+
  522. // P+ = (I12 - K * C) * P- = (I12 - K * C) * Qw = Qw - K * (C * Qw)
  523. // both Qw and P+ are used as working arrays in this section
  524. // at the end of this section, P+ is valid but Qw is over-written
  525. // ***********************************************************************************
  526. // set P+ (6x12 scratch sub-matrix) to the product C (6x12) * Qw (12x12)
  527. // where both C and Qw are sparse and C has a significant number of +1 and -1 entries
  528. // the resulting matrix is sparse but not symmetric
  529. for (i = 0; i < 6; i++) {
  530. // initialize pfPPlus12x12ij for current i, j=0
  531. pfPPlus12x12ij = PPlus12x12[i];
  532. for (j = 0; j < 12; j++) {
  533. // zero P+[i][j]
  534. *pfPPlus12x12ij = 0.0F;
  535. // initialize pfC6x12ik for current i, k=0
  536. pfC6x12ik = C6x12[i];
  537. // initialize pfQw12x12kj for current j, k=0
  538. pfQw12x12kj = &Qw12x12[0][j];
  539. // sum matrix products over inner loop over k
  540. for (k = 0; k < 12; k++) {
  541. if ((*pfC6x12ik != 0.0F) && (*pfQw12x12kj != 0.0F)) {
  542. if (*pfC6x12ik == 1.0F)
  543. *pfPPlus12x12ij += *pfQw12x12kj;
  544. else if (*pfC6x12ik == -1.0F)
  545. *pfPPlus12x12ij -= *pfQw12x12kj;
  546. else
  547. *pfPPlus12x12ij += *pfC6x12ik * *pfQw12x12kj;
  548. }
  549. // update pfC6x12ik and pfQw12x12kj for next iteration of k
  550. pfC6x12ik++;
  551. pfQw12x12kj += 12;
  552. }
  553. // increment pfPPlus12x12ij for next iteration of j
  554. pfPPlus12x12ij++;
  555. }
  556. }
  557. // compute P+ = (I12 - K * C) * Qw = Qw - K * (C * Qw) = Qw - K * P+ (6x12 sub-matrix)
  558. // storing result P+ in Qw and over-writing Qw which is OK since Qw is later computed from P+
  559. // where working array P+ (6x12 sub-matrix) is sparse but K is not sparse
  560. // only on and above diagonal terms of P+ are computed since P+ is symmetric
  561. for (i = 0; i < 12; i++) {
  562. // initialize pfQw12x12ij for current i, j=i
  563. pfQw12x12ij = Qw12x12[i] + i;
  564. for (j = i; j < 12; j++) {
  565. // initialize pfK12x6ik for current i, k=0
  566. pfK12x6ik = K12x6[i];
  567. // initialize pfPPlus12x12kj for current j, k=0
  568. pfPPlus12x12kj = *PPlus12x12 + j;
  569. // compute on and above diagonal matrix entry
  570. for (k = 0; k < 6; k++) {
  571. // check for non-zero values since P+ (6x12 scratch sub-matrix) is sparse
  572. if (*pfPPlus12x12kj != 0.0F) {
  573. *pfQw12x12ij -= *pfK12x6ik * *pfPPlus12x12kj;
  574. }
  575. // increment pfK12x6ik and pfPPlus12x12kj for next iteration of k
  576. pfK12x6ik++;
  577. pfPPlus12x12kj += 12;
  578. }
  579. // increment pfQw12x12ij for next iteration of j
  580. pfQw12x12ij++;
  581. }
  582. }
  583. // Qw now holds the on and above diagonal elements of P+
  584. // so perform a simple copy to the all elements of P+
  585. // after execution of this code P+ is valid but Qw remains invalid
  586. for (i = 0; i < 12; i++) {
  587. // initialize pfPPlus12x12ij and pfQw12x12ij for current i, j=i
  588. pfPPlus12x12ij = PPlus12x12[i] + i;
  589. pfQw12x12ij = Qw12x12[i] + i;
  590. // copy the on-diagonal elements and increment pointers to enter loop at j=i+1
  591. *(pfPPlus12x12ij++) = *(pfQw12x12ij++);
  592. // loop over above diagonal columns j copying to below-diagonal elements
  593. for (j = i + 1; j < 12; j++) {
  594. *(pfPPlus12x12ij++)= PPlus12x12[j][i] = *(pfQw12x12ij++);
  595. }
  596. }
  597. // *********************************************************************************
  598. // re-create the noise covariance matrix Qw=fn(P+) for the next iteration
  599. // using the elements of P+ which are now valid
  600. // Qw was over-written earlier but is here recomputed (all elements)
  601. // *********************************************************************************
  602. // zero the matrix Qw
  603. for (i = 0; i < 12; i++) {
  604. for (j = 0; j < 12; j++) {
  605. Qw12x12[i][j] = 0.0F;
  606. }
  607. }
  608. // update the covariance matrix components
  609. for (i = 0; i < 3; i++) {
  610. // Qw[th-th-] = Qw[0-2][0-2] = E[th-(th-)^T] = Q[th+th+] + deltat^2 * (Q[b+b+] + (Qwb + QvG) * I)
  611. Qw12x12[i][i] = PPlus12x12[i][i] + deltatsq * (PPlus12x12[i + 3][i + 3] + QwbplusQvG);
  612. // Qw[b-b-] = Qw[3-5][3-5] = E[b-(b-)^T] = Q[b+b+] + Qwb * I
  613. Qw12x12[i + 3][i + 3] = PPlus12x12[i + 3][i + 3] + FQWB_9DOF_GBY_KALMAN;
  614. // Qw[th-b-] = Qw[0-2][3-5] = E[th-(b-)^T] = -deltat * (Q[b+b+] + Qwb * I) = -deltat * Qw[b-b-]
  615. Qw12x12[i][i + 3] = Qw12x12[i + 3][i] = -deltat * Qw12x12[i + 3][i + 3];
  616. // Qw[a-a-] = Qw[6-8][6-8] = E[a-(a-)^T] = ca^2 * Q[a+a+] + Qwa * I
  617. Qw12x12[i + 6][i + 6] = casq * PPlus12x12[i + 6][i + 6] + FQWA_9DOF_GBY_KALMAN;
  618. // Qw[d-d-] = Qw[9-11][9-11] = E[d-(d-)^T] = cd^2 * Q[d+d+] + Qwd * I
  619. Qw12x12[i + 9][i + 9] = cdsq * PPlus12x12[i + 9][i + 9] + FQWD_9DOF_GBY_KALMAN;
  620. }
  621. }
  622. // compile time constants that are private to this file
  623. #define SMALLQ0 0.01F // limit of quaternion scalar component requiring special algorithm
  624. #define CORRUPTQUAT 0.001F // threshold for deciding rotation quaternion is corrupt
  625. #define SMALLMODULUS 0.01F // limit where rounding errors may appear
  626. // Aerospace NED accelerometer 3DOF tilt function computing rotation matrix fR
  627. void f3DOFTiltNED(float fR[][3], float fGp[])
  628. {
  629. // the NED self-consistency twist occurs at 90 deg pitch
  630. // local variables
  631. int16_t i; // counter
  632. float fmodGxyz; // modulus of the x, y, z accelerometer readings
  633. float fmodGyz; // modulus of the y, z accelerometer readings
  634. float frecipmodGxyz; // reciprocal of modulus
  635. float ftmp; // scratch variable
  636. // compute the accelerometer squared magnitudes
  637. fmodGyz = fGp[Y] * fGp[Y] + fGp[Z] * fGp[Z];
  638. fmodGxyz = fmodGyz + fGp[X] * fGp[X];
  639. // check for freefall special case where no solution is possible
  640. if (fmodGxyz == 0.0F) {
  641. f3x3matrixAeqI(fR);
  642. return;
  643. }
  644. // check for vertical up or down gimbal lock case
  645. if (fmodGyz == 0.0F) {
  646. f3x3matrixAeqScalar(fR, 0.0F);
  647. fR[Y][Y] = 1.0F;
  648. if (fGp[X] >= 0.0F) {
  649. fR[X][Z] = 1.0F;
  650. fR[Z][X] = -1.0F;
  651. } else {
  652. fR[X][Z] = -1.0F;
  653. fR[Z][X] = 1.0F;
  654. }
  655. return;
  656. }
  657. // compute moduli for the general case
  658. fmodGyz = sqrtf(fmodGyz);
  659. fmodGxyz = sqrtf(fmodGxyz);
  660. frecipmodGxyz = 1.0F / fmodGxyz;
  661. ftmp = fmodGxyz / fmodGyz;
  662. // normalize the accelerometer reading into the z column
  663. for (i = X; i <= Z; i++) {
  664. fR[i][Z] = fGp[i] * frecipmodGxyz;
  665. }
  666. // construct x column of orientation matrix
  667. fR[X][X] = fmodGyz * frecipmodGxyz;
  668. fR[Y][X] = -fR[X][Z] * fR[Y][Z] * ftmp;
  669. fR[Z][X] = -fR[X][Z] * fR[Z][Z] * ftmp;
  670. // // construct y column of orientation matrix
  671. fR[X][Y] = 0.0F;
  672. fR[Y][Y] = fR[Z][Z] * ftmp;
  673. fR[Z][Y] = -fR[Y][Z] * ftmp;
  674. }
  675. // Aerospace NED magnetometer 3DOF flat eCompass function computing rotation matrix fR
  676. void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
  677. {
  678. // local variables
  679. float fmodBxy; // modulus of the x, y magnetometer readings
  680. // compute the magnitude of the horizontal (x and y) magnetometer reading
  681. fmodBxy = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y]);
  682. // check for zero field special case where no solution is possible
  683. if (fmodBxy == 0.0F) {
  684. f3x3matrixAeqI(fR);
  685. return;
  686. }
  687. // define the fixed entries in the z row and column
  688. fR[Z][X] = fR[Z][Y] = fR[X][Z] = fR[Y][Z] = 0.0F;
  689. fR[Z][Z] = 1.0F;
  690. // define the remaining entries
  691. fR[X][X] = fR[Y][Y] = fBc[X] / fmodBxy;
  692. fR[Y][X] = fBc[Y] / fmodBxy;
  693. fR[X][Y] = -fR[Y][X];
  694. }
  695. // NED: 6DOF e-Compass function computing rotation matrix fR
  696. static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[])
  697. {
  698. // local variables
  699. float fmod[3]; // column moduli
  700. float fmodBc; // modulus of Bc
  701. float fGdotBc; // dot product of vectors G.Bc
  702. float ftmp; // scratch variable
  703. int8_t i, j; // loop counters
  704. // set the inclination angle to zero in case it is not computed later
  705. *pfDelta = 0.0F;
  706. // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and x axes
  707. for (i = X; i <= Z; i++) {
  708. fR[i][Z] = fGp[i];
  709. fR[i][X] = fBc[i];
  710. }
  711. // set y vector to vector product of z and x vectors
  712. fR[X][Y] = fR[Y][Z] * fR[Z][X] - fR[Z][Z] * fR[Y][X];
  713. fR[Y][Y] = fR[Z][Z] * fR[X][X] - fR[X][Z] * fR[Z][X];
  714. fR[Z][Y] = fR[X][Z] * fR[Y][X] - fR[Y][Z] * fR[X][X];
  715. // set x vector to vector product of y and z vectors
  716. fR[X][X] = fR[Y][Y] * fR[Z][Z] - fR[Z][Y] * fR[Y][Z];
  717. fR[Y][X] = fR[Z][Y] * fR[X][Z] - fR[X][Y] * fR[Z][Z];
  718. fR[Z][X] = fR[X][Y] * fR[Y][Z] - fR[Y][Y] * fR[X][Z];
  719. // calculate the rotation matrix column moduli
  720. fmod[X] = sqrtf(fR[X][X] * fR[X][X] + fR[Y][X] * fR[Y][X] + fR[Z][X] * fR[Z][X]);
  721. fmod[Y] = sqrtf(fR[X][Y] * fR[X][Y] + fR[Y][Y] * fR[Y][Y] + fR[Z][Y] * fR[Z][Y]);
  722. fmod[Z] = sqrtf(fR[X][Z] * fR[X][Z] + fR[Y][Z] * fR[Y][Z] + fR[Z][Z] * fR[Z][Z]);
  723. // normalize the rotation matrix columns
  724. if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F))) {
  725. // loop over columns j
  726. for (j = X; j <= Z; j++) {
  727. ftmp = 1.0F / fmod[j];
  728. // loop over rows i
  729. for (i = X; i <= Z; i++) {
  730. // normalize by the column modulus
  731. fR[i][j] *= ftmp;
  732. }
  733. }
  734. } else {
  735. // no solution is possible to set rotation to identity matrix
  736. f3x3matrixAeqI(fR);
  737. return;
  738. }
  739. // compute the geomagnetic inclination angle
  740. fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]);
  741. fGdotBc = fGp[X] * fBc[X] + fGp[Y] * fBc[Y] + fGp[Z] * fBc[Z];
  742. if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F))) {
  743. *pfDelta = fasin_deg(fGdotBc / (fmod[Z] * fmodBc));
  744. }
  745. }
  746. // extract the NED angles in degrees from the NED rotation matrix
  747. static void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg,
  748. float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
  749. {
  750. // calculate the pitch angle -90.0 <= Theta <= 90.0 deg
  751. *pfTheDeg = fasin_deg(-R[X][Z]);
  752. // calculate the roll angle range -180.0 <= Phi < 180.0 deg
  753. *pfPhiDeg = fatan2_deg(R[Y][Z], R[Z][Z]);
  754. // map +180 roll onto the functionally equivalent -180 deg roll
  755. if (*pfPhiDeg == 180.0F) {
  756. *pfPhiDeg = -180.0F;
  757. }
  758. // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
  759. if (*pfTheDeg == 90.0F) {
  760. // vertical upwards gimbal lock case
  761. *pfPsiDeg = fatan2_deg(R[Z][Y], R[Y][Y]) + *pfPhiDeg;
  762. } else if (*pfTheDeg == -90.0F) {
  763. // vertical downwards gimbal lock case
  764. *pfPsiDeg = fatan2_deg(-R[Z][Y], R[Y][Y]) - *pfPhiDeg;
  765. } else {
  766. // general case
  767. *pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]);
  768. }
  769. // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
  770. if (*pfPsiDeg < 0.0F) {
  771. *pfPsiDeg += 360.0F;
  772. }
  773. // check for rounding errors mapping small negative angle to 360 deg
  774. if (*pfPsiDeg >= 360.0F) {
  775. *pfPsiDeg = 0.0F;
  776. }
  777. // for NED, the compass heading Rho equals the yaw angle Psi
  778. *pfRhoDeg = *pfPsiDeg;
  779. // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
  780. *pfChiDeg = facos_deg(R[Z][Z]);
  781. return;
  782. }
  783. // computes normalized rotation quaternion from a rotation vector (deg)
  784. static void fQuaternionFromRotationVectorDeg(Quaternion_t *pq, const float rvecdeg[], float fscaling)
  785. {
  786. float fetadeg; // rotation angle (deg)
  787. float fetarad; // rotation angle (rad)
  788. float fetarad2; // eta (rad)^2
  789. float fetarad4; // eta (rad)^4
  790. float sinhalfeta; // sin(eta/2)
  791. float fvecsq; // q1^2+q2^2+q3^2
  792. float ftmp; // scratch variable
  793. // compute the scaled rotation angle eta (deg) which can be both positve or negative
  794. fetadeg = fscaling * sqrtf(rvecdeg[X] * rvecdeg[X] + rvecdeg[Y] * rvecdeg[Y] + rvecdeg[Z] * rvecdeg[Z]);
  795. fetarad = fetadeg * FDEGTORAD;
  796. fetarad2 = fetarad * fetarad;
  797. // calculate the sine and cosine using small angle approximations or exact
  798. // 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
  799. if (fetarad2 <= 0.02F) {
  800. // use MacLaurin series up to and including third order
  801. sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2);
  802. } else if (fetarad2 <= 0.06F) {
  803. // use MacLaurin series up to and including fifth order
  804. // 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
  805. fetarad4 = fetarad2 * fetarad2;
  806. sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2 + ONEOVER3840 * fetarad4);
  807. } else {
  808. // use exact calculation
  809. sinhalfeta = (float)sinf(0.5F * fetarad);
  810. }
  811. // compute the vector quaternion components q1, q2, q3
  812. if (fetadeg != 0.0F) {
  813. // general case with non-zero rotation angle
  814. ftmp = fscaling * sinhalfeta / fetadeg;
  815. pq->q1 = rvecdeg[X] * ftmp; // q1 = nx * sin(eta/2)
  816. pq->q2 = rvecdeg[Y] * ftmp; // q2 = ny * sin(eta/2)
  817. pq->q3 = rvecdeg[Z] * ftmp; // q3 = nz * sin(eta/2)
  818. } else {
  819. // zero rotation angle giving zero vector component
  820. pq->q1 = pq->q2 = pq->q3 = 0.0F;
  821. }
  822. // compute the scalar quaternion component q0 by explicit normalization
  823. // taking care to avoid rounding errors giving negative operand to sqrt
  824. fvecsq = pq->q1 * pq->q1 + pq->q2 * pq->q2 + pq->q3 * pq->q3;
  825. if (fvecsq <= 1.0F) {
  826. // normal case
  827. pq->q0 = sqrtf(1.0F - fvecsq);
  828. } else {
  829. // rounding errors are present
  830. pq->q0 = 0.0F;
  831. }
  832. }
  833. // compute the orientation quaternion from a 3x3 rotation matrix
  834. static void fQuaternionFromRotationMatrix(float R[][3], Quaternion_t *pq)
  835. {
  836. float fq0sq; // q0^2
  837. float recip4q0; // 1/4q0
  838. // the quaternion is not explicitly normalized in this function on the assumption that it
  839. // is supplied with a normalized rotation matrix. if the rotation matrix is normalized then
  840. // the quaternion will also be normalized even if the case of small q0
  841. // get q0^2 and q0
  842. fq0sq = 0.25F * (1.0F + R[X][X] + R[Y][Y] + R[Z][Z]);
  843. pq->q0 = sqrtf(fabs(fq0sq));
  844. // normal case when q0 is not small meaning rotation angle not near 180 deg
  845. if (pq->q0 > SMALLQ0) {
  846. // calculate q1 to q3 (general case)
  847. recip4q0 = 0.25F / pq->q0;
  848. pq->q1 = recip4q0 * (R[Y][Z] - R[Z][Y]);
  849. pq->q2 = recip4q0 * (R[Z][X] - R[X][Z]);
  850. pq->q3 = recip4q0 * (R[X][Y] - R[Y][X]);
  851. } else {
  852. // special case of near 180 deg corresponds to nearly symmetric matrix
  853. // which is not numerically well conditioned for division by small q0
  854. // instead get absolute values of q1 to q3 from leading diagonal
  855. pq->q1 = sqrtf(fabs(0.5F * (1.0F + R[X][X]) - fq0sq));
  856. pq->q2 = sqrtf(fabs(0.5F * (1.0F + R[Y][Y]) - fq0sq));
  857. pq->q3 = sqrtf(fabs(0.5F * (1.0F + R[Z][Z]) - fq0sq));
  858. // correct the signs of q1 to q3 by examining the signs of differenced off-diagonal terms
  859. if ((R[Y][Z] - R[Z][Y]) < 0.0F) pq->q1 = -pq->q1;
  860. if ((R[Z][X] - R[X][Z]) < 0.0F) pq->q2 = -pq->q2;
  861. if ((R[X][Y] - R[Y][X]) < 0.0F) pq->q3 = -pq->q3;
  862. }
  863. }
  864. // compute the rotation matrix from an orientation quaternion
  865. static void fRotationMatrixFromQuaternion(float R[][3], const Quaternion_t *pq)
  866. {
  867. float f2q;
  868. float f2q0q0, f2q0q1, f2q0q2, f2q0q3;
  869. float f2q1q1, f2q1q2, f2q1q3;
  870. float f2q2q2, f2q2q3;
  871. float f2q3q3;
  872. // calculate products
  873. f2q = 2.0F * pq->q0;
  874. f2q0q0 = f2q * pq->q0;
  875. f2q0q1 = f2q * pq->q1;
  876. f2q0q2 = f2q * pq->q2;
  877. f2q0q3 = f2q * pq->q3;
  878. f2q = 2.0F * pq->q1;
  879. f2q1q1 = f2q * pq->q1;
  880. f2q1q2 = f2q * pq->q2;
  881. f2q1q3 = f2q * pq->q3;
  882. f2q = 2.0F * pq->q2;
  883. f2q2q2 = f2q * pq->q2;
  884. f2q2q3 = f2q * pq->q3;
  885. f2q3q3 = 2.0F * pq->q3 * pq->q3;
  886. // calculate the rotation matrix assuming the quaternion is normalized
  887. R[X][X] = f2q0q0 + f2q1q1 - 1.0F;
  888. R[X][Y] = f2q1q2 + f2q0q3;
  889. R[X][Z] = f2q1q3 - f2q0q2;
  890. R[Y][X] = f2q1q2 - f2q0q3;
  891. R[Y][Y] = f2q0q0 + f2q2q2 - 1.0F;
  892. R[Y][Z] = f2q2q3 + f2q0q1;
  893. R[Z][X] = f2q1q3 + f2q0q2;
  894. R[Z][Y] = f2q2q3 - f2q0q1;
  895. R[Z][Z] = f2q0q0 + f2q3q3 - 1.0F;
  896. }
  897. // function calculate the rotation vector from a rotation matrix
  898. void fRotationVectorDegFromRotationMatrix(float R[][3], float rvecdeg[])
  899. {
  900. float ftrace; // trace of the rotation matrix
  901. float fetadeg; // rotation angle eta (deg)
  902. float fmodulus; // modulus of axis * angle vector = 2|sin(eta)|
  903. float ftmp; // scratch variable
  904. // calculate the trace of the rotation matrix = 1+2cos(eta) in range -1 to +3
  905. // and eta (deg) in range 0 to 180 deg inclusive
  906. // checking for rounding errors that might take the trace outside this range
  907. ftrace = R[X][X] + R[Y][Y] + R[Z][Z];
  908. if (ftrace >= 3.0F) {
  909. fetadeg = 0.0F;
  910. } else if (ftrace <= -1.0F) {
  911. fetadeg = 180.0F;
  912. } else {
  913. fetadeg = acosf(0.5F * (ftrace - 1.0F)) * FRADTODEG;
  914. }
  915. // set the rvecdeg vector to differences across the diagonal = 2*n*sin(eta)
  916. // and calculate its modulus equal to 2|sin(eta)|
  917. // the modulus approaches zero near 0 and 180 deg (when sin(eta) approaches zero)
  918. rvecdeg[X] = R[Y][Z] - R[Z][Y];
  919. rvecdeg[Y] = R[Z][X] - R[X][Z];
  920. rvecdeg[Z] = R[X][Y] - R[Y][X];
  921. fmodulus = sqrtf(rvecdeg[X] * rvecdeg[X] + rvecdeg[Y] * rvecdeg[Y] + rvecdeg[Z] * rvecdeg[Z]);
  922. // normalize the rotation vector for general, 0 deg and 180 deg rotation cases
  923. if (fmodulus > SMALLMODULUS) {
  924. // general case away from 0 and 180 deg rotation
  925. ftmp = fetadeg / fmodulus;
  926. rvecdeg[X] *= ftmp; // set x component to eta(deg) * nx
  927. rvecdeg[Y] *= ftmp; // set y component to eta(deg) * ny
  928. rvecdeg[Z] *= ftmp; // set z component to eta(deg) * nz
  929. } else if (ftrace >= 0.0F) {
  930. // near 0 deg rotation (trace = 3): matrix is nearly identity matrix
  931. // R[Y][Z]-R[Z][Y]=2*nx*eta(rad) and similarly for other components
  932. ftmp = 0.5F * FRADTODEG;
  933. rvecdeg[X] *= ftmp;
  934. rvecdeg[Y] *= ftmp;
  935. rvecdeg[Z] *= ftmp;
  936. } else {
  937. // near 180 deg (trace = -1): matrix is nearly symmetric
  938. // calculate the absolute value of the components of the axis-angle vector
  939. rvecdeg[X] = 180.0F * sqrtf(fabs(0.5F * (R[X][X] + 1.0F)));
  940. rvecdeg[Y] = 180.0F * sqrtf(fabs(0.5F * (R[Y][Y] + 1.0F)));
  941. rvecdeg[Z] = 180.0F * sqrtf(fabs(0.5F * (R[Z][Z] + 1.0F)));
  942. // correct the signs of the three components by examining the signs of differenced off-diagonal terms
  943. if ((R[Y][Z] - R[Z][Y]) < 0.0F) rvecdeg[X] = -rvecdeg[X];
  944. if ((R[Z][X] - R[X][Z]) < 0.0F) rvecdeg[Y] = -rvecdeg[Y];
  945. if ((R[X][Y] - R[Y][X]) < 0.0F) rvecdeg[Z] = -rvecdeg[Z];
  946. }
  947. }
  948. // computes rotation vector (deg) from rotation quaternion
  949. static void fRotationVectorDegFromQuaternion(Quaternion_t *pq, float rvecdeg[])
  950. {
  951. float fetarad; // rotation angle (rad)
  952. float fetadeg; // rotation angle (deg)
  953. float sinhalfeta; // sin(eta/2)
  954. float ftmp; // scratch variable
  955. // calculate the rotation angle in the range 0 <= eta < 360 deg
  956. if ((pq->q0 >= 1.0F) || (pq->q0 <= -1.0F)) {
  957. // rotation angle is 0 deg or 2*180 deg = 360 deg = 0 deg
  958. fetarad = 0.0F;
  959. fetadeg = 0.0F;
  960. } else {
  961. // general case returning 0 < eta < 360 deg
  962. fetarad = 2.0F * acosf(pq->q0);
  963. fetadeg = fetarad * FRADTODEG;
  964. }
  965. // map the rotation angle onto the range -180 deg <= eta < 180 deg
  966. if (fetadeg >= 180.0F) {
  967. fetadeg -= 360.0F;
  968. fetarad = fetadeg * FDEGTORAD;
  969. }
  970. // calculate sin(eta/2) which will be in the range -1 to +1
  971. sinhalfeta = (float)sinf(0.5F * fetarad);
  972. // calculate the rotation vector (deg)
  973. if (sinhalfeta == 0.0F) {
  974. // the rotation angle eta is zero and the axis is irrelevant
  975. rvecdeg[X] = rvecdeg[Y] = rvecdeg[Z] = 0.0F;
  976. } else {
  977. // general case with non-zero rotation angle
  978. ftmp = fetadeg / sinhalfeta;
  979. rvecdeg[X] = pq->q1 * ftmp;
  980. rvecdeg[Y] = pq->q2 * ftmp;
  981. rvecdeg[Z] = pq->q3 * ftmp;
  982. }
  983. }
  984. #if 0
  985. // function low pass filters an orientation quaternion and computes virtual gyro rotation rate
  986. void fLPFOrientationQuaternion(Quaternion_t *pq, Quaternion_t *pLPq, float flpf, float fdeltat,
  987. float fOmega[], int32_t loopcounter)
  988. {
  989. // local variables
  990. Quaternion_t fdeltaq; // delta rotation quaternion
  991. float rvecdeg[3]; // rotation vector (deg)
  992. float ftmp; // scratch variable
  993. // initialize delay line on first pass: LPq[n]=q[n]
  994. if (loopcounter == 0) {
  995. *pLPq = *pq;
  996. }
  997. // set fdeltaqn to the delta rotation quaternion conjg(fLPq[n-1) . fqn
  998. fdeltaq = qconjgAxB(pLPq, pq);
  999. if (fdeltaq.q0 < 0.0F) {
  1000. fdeltaq.q0 = -fdeltaq.q0;
  1001. fdeltaq.q1 = -fdeltaq.q1;
  1002. fdeltaq.q2 = -fdeltaq.q2;
  1003. fdeltaq.q3 = -fdeltaq.q3;
  1004. }
  1005. // set ftmp to a scaled lpf value which equals flpf in the limit of small rotations (q0=1)
  1006. // but which rises as the delta rotation angle increases (q0 tends to zero)
  1007. ftmp = flpf + 0.75F * (1.0F - fdeltaq.q0);
  1008. if (ftmp > 1.0F) {
  1009. ftmp = 1.0F;
  1010. }
  1011. // scale the delta rotation by the corrected lpf value
  1012. fdeltaq.q1 *= ftmp;
  1013. fdeltaq.q2 *= ftmp;
  1014. fdeltaq.q3 *= ftmp;
  1015. // compute the scalar component q0
  1016. ftmp = fdeltaq.q1 * fdeltaq.q1 + fdeltaq.q2 * fdeltaq.q2 + fdeltaq.q3 * fdeltaq.q3;
  1017. if (ftmp <= 1.0F) {
  1018. // normal case
  1019. fdeltaq.q0 = sqrtf(1.0F - ftmp);
  1020. } else {
  1021. // rounding errors present so simply set scalar component to 0
  1022. fdeltaq.q0 = 0.0F;
  1023. }
  1024. // calculate the delta rotation vector from fdeltaqn and the virtual gyro angular velocity (deg/s)
  1025. fRotationVectorDegFromQuaternion(&fdeltaq, rvecdeg);
  1026. ftmp = 1.0F / fdeltat;
  1027. fOmega[X] = rvecdeg[X] * ftmp;
  1028. fOmega[Y] = rvecdeg[Y] * ftmp;
  1029. fOmega[Z] = rvecdeg[Z] * ftmp;
  1030. // set LPq[n] = LPq[n-1] . deltaq[n]
  1031. qAeqAxB(pLPq, &fdeltaq);
  1032. // renormalize the low pass filtered quaternion to prevent error accumulation
  1033. // the renormalization function ensures that q0 is non-negative
  1034. fqAeqNormqA(pLPq);
  1035. }
  1036. // function low pass filters a scalar
  1037. void fLPFScalar(float *pfS, float *pfLPS, float flpf, int32_t loopcounter)
  1038. {
  1039. // set S[LP,n]=S[n] on first pass
  1040. if (loopcounter == 0) {
  1041. *pfLPS = *pfS;
  1042. }
  1043. // apply the exponential low pass filter
  1044. *pfLPS += flpf * (*pfS - *pfLPS);
  1045. }
  1046. #endif
  1047. // function compute the quaternion product qA * qB
  1048. static void qAeqBxC(Quaternion_t *pqA, const Quaternion_t *pqB, const Quaternion_t *pqC)
  1049. {
  1050. pqA->q0 = pqB->q0 * pqC->q0 - pqB->q1 * pqC->q1 - pqB->q2 * pqC->q2 - pqB->q3 * pqC->q3;
  1051. pqA->q1 = pqB->q0 * pqC->q1 + pqB->q1 * pqC->q0 + pqB->q2 * pqC->q3 - pqB->q3 * pqC->q2;
  1052. pqA->q2 = pqB->q0 * pqC->q2 - pqB->q1 * pqC->q3 + pqB->q2 * pqC->q0 + pqB->q3 * pqC->q1;
  1053. pqA->q3 = pqB->q0 * pqC->q3 + pqB->q1 * pqC->q2 - pqB->q2 * pqC->q1 + pqB->q3 * pqC->q0;
  1054. }
  1055. // function compute the quaternion product qA = qA * qB
  1056. static void qAeqAxB(Quaternion_t *pqA, const Quaternion_t *pqB)
  1057. {
  1058. Quaternion_t qProd;
  1059. // perform the quaternion product
  1060. qProd.q0 = pqA->q0 * pqB->q0 - pqA->q1 * pqB->q1 - pqA->q2 * pqB->q2 - pqA->q3 * pqB->q3;
  1061. qProd.q1 = pqA->q0 * pqB->q1 + pqA->q1 * pqB->q0 + pqA->q2 * pqB->q3 - pqA->q3 * pqB->q2;
  1062. qProd.q2 = pqA->q0 * pqB->q2 - pqA->q1 * pqB->q3 + pqA->q2 * pqB->q0 + pqA->q3 * pqB->q1;
  1063. qProd.q3 = pqA->q0 * pqB->q3 + pqA->q1 * pqB->q2 - pqA->q2 * pqB->q1 + pqA->q3 * pqB->q0;
  1064. // copy the result back into qA
  1065. *pqA = qProd;
  1066. }
  1067. #if 0
  1068. // function compute the quaternion product conjg(qA) * qB
  1069. static Quaternion_t qconjgAxB(const Quaternion_t *pqA, const Quaternion_t *pqB)
  1070. {
  1071. Quaternion_t qProd;
  1072. qProd.q0 = pqA->q0 * pqB->q0 + pqA->q1 * pqB->q1 + pqA->q2 * pqB->q2 + pqA->q3 * pqB->q3;
  1073. qProd.q1 = pqA->q0 * pqB->q1 - pqA->q1 * pqB->q0 - pqA->q2 * pqB->q3 + pqA->q3 * pqB->q2;
  1074. qProd.q2 = pqA->q0 * pqB->q2 + pqA->q1 * pqB->q3 - pqA->q2 * pqB->q0 - pqA->q3 * pqB->q1;
  1075. qProd.q3 = pqA->q0 * pqB->q3 - pqA->q1 * pqB->q2 + pqA->q2 * pqB->q1 - pqA->q3 * pqB->q0;
  1076. return qProd;
  1077. }
  1078. #endif
  1079. // function normalizes a rotation quaternion and ensures q0 is non-negative
  1080. static void fqAeqNormqA(Quaternion_t *pqA)
  1081. {
  1082. float fNorm; // quaternion Norm
  1083. // calculate the quaternion Norm
  1084. fNorm = sqrtf(pqA->q0 * pqA->q0 + pqA->q1 * pqA->q1 + pqA->q2 * pqA->q2 + pqA->q3 * pqA->q3);
  1085. if (fNorm > CORRUPTQUAT) {
  1086. // general case
  1087. fNorm = 1.0F / fNorm;
  1088. pqA->q0 *= fNorm;
  1089. pqA->q1 *= fNorm;
  1090. pqA->q2 *= fNorm;
  1091. pqA->q3 *= fNorm;
  1092. } else {
  1093. // return with identity quaternion since the quaternion is corrupted
  1094. pqA->q0 = 1.0F;
  1095. pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
  1096. }
  1097. // correct a negative scalar component if the function was called with negative q0
  1098. if (pqA->q0 < 0.0F) {
  1099. pqA->q0 = -pqA->q0;
  1100. pqA->q1 = -pqA->q1;
  1101. pqA->q2 = -pqA->q2;
  1102. pqA->q3 = -pqA->q3;
  1103. }
  1104. }
  1105. // set a quaternion to the unit quaternion
  1106. static void fqAeq1(Quaternion_t *pqA)
  1107. {
  1108. pqA->q0 = 1.0F;
  1109. pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
  1110. }
  1111. // function returns an approximation to angle(deg)=asin(x) for x in the range -1 <= x <= 1
  1112. // and returns -90 <= angle <= 90 deg
  1113. // maximum error is 10.29E-6 deg
  1114. static float fasin_deg(float x)
  1115. {
  1116. // for robustness, check for invalid argument
  1117. if (x >= 1.0F) return 90.0F;
  1118. if (x <= -1.0F) return -90.0F;
  1119. // call the atan which will return an angle in the correct range -90 to 90 deg
  1120. // this line cannot fail from division by zero or negative square root since |x| < 1
  1121. return (fatan_deg(x / sqrtf(1.0F - x * x)));
  1122. }
  1123. // function returns an approximation to angle(deg)=acos(x) for x in the range -1 <= x <= 1
  1124. // and returns 0 <= angle <= 180 deg
  1125. // maximum error is 14.67E-6 deg
  1126. static float facos_deg(float x)
  1127. {
  1128. // for robustness, check for invalid arguments
  1129. if (x >= 1.0F) return 0.0F;
  1130. if (x <= -1.0F) return 180.0F;
  1131. // call the atan which will return an angle in the incorrect range -90 to 90 deg
  1132. // these lines cannot fail from division by zero or negative square root
  1133. if (x == 0.0F) return 90.0F;
  1134. if (x > 0.0F) return fatan_deg((sqrtf(1.0F - x * x) / x));
  1135. return 180.0F + fatan_deg((sqrtf(1.0F - x * x) / x));
  1136. }
  1137. // function returns angle in range -90 to 90 deg
  1138. // maximum error is 9.84E-6 deg
  1139. static float fatan_deg(float x)
  1140. {
  1141. float fangledeg; // compute computed (deg)
  1142. int8_t ixisnegative; // argument x is negative
  1143. int8_t ixexceeds1; // argument x is greater than 1.0
  1144. int8_t ixmapped; // argument in range tan(15 deg) to tan(45 deg)=1.0
  1145. #define TAN15DEG 0.26794919243F // tan(15 deg) = 2 - sqrt(3)
  1146. #define TAN30DEG 0.57735026919F // tan(30 deg) = 1/sqrt(3)
  1147. // reset all flags
  1148. ixisnegative = ixexceeds1 = ixmapped = 0;
  1149. // test for negative argument to allow use of tan(-x)=-tan(x)
  1150. if (x < 0.0F) {
  1151. x = -x;
  1152. ixisnegative = 1;
  1153. }
  1154. // test for argument above 1 to allow use of atan(x)=pi/2-atan(1/x)
  1155. if (x > 1.0F) {
  1156. x = 1.0F / x;
  1157. ixexceeds1 = 1;
  1158. }
  1159. // at this point, x is in the range 0 to 1 inclusive
  1160. // map argument onto range -tan(15 deg) to tan(15 deg)
  1161. // using tan(angle-30deg) = (tan(angle)-tan(30deg)) / (1 + tan(angle)tan(30deg))
  1162. // tan(15deg) maps to tan(-15 deg) = -tan(15 deg)
  1163. // 1. maps to (sqrt(3) - 1) / (sqrt(3) + 1) = 2 - sqrt(3) = tan(15 deg)
  1164. if (x > TAN15DEG) {
  1165. x = (x - TAN30DEG)/(1.0F + TAN30DEG * x);
  1166. ixmapped = 1;
  1167. }
  1168. // call the atan estimator to obtain -15 deg <= angle <= 15 deg
  1169. fangledeg = fatan_15deg(x);
  1170. // undo the distortions applied earlier to obtain -90 deg <= angle <= 90 deg
  1171. if (ixmapped) fangledeg += 30.0F;
  1172. if (ixexceeds1) fangledeg = 90.0F - fangledeg;
  1173. if (ixisnegative) fangledeg = -fangledeg;
  1174. return (fangledeg);
  1175. }
  1176. // function returns approximate atan2 angle in range -180 to 180 deg
  1177. // maximum error is 14.58E-6 deg
  1178. static float fatan2_deg(float y, float x)
  1179. {
  1180. // check for zero x to avoid division by zero
  1181. if (x == 0.0F) {
  1182. // return 90 deg for positive y
  1183. if (y > 0.0F) return 90.0F;
  1184. // return -90 deg for negative y
  1185. if (y < 0.0F) return -90.0F;
  1186. // otherwise y= 0.0 and return 0 deg (invalid arguments)
  1187. return 0.0F;
  1188. }
  1189. // from here onwards, x is guaranteed to be non-zero
  1190. // compute atan2 for quadrant 1 (0 to 90 deg) and quadrant 4 (-90 to 0 deg)
  1191. if (x > 0.0F) return (fatan_deg(y / x));
  1192. // compute atan2 for quadrant 2 (90 to 180 deg)
  1193. if ((x < 0.0F) && (y > 0.0F)) return (180.0F + fatan_deg(y / x));
  1194. // compute atan2 for quadrant 3 (-180 to -90 deg)
  1195. return (-180.0F + fatan_deg(y / x));
  1196. }
  1197. // approximation to inverse tan function (deg) for x in range
  1198. // -tan(15 deg) to tan(15 deg) giving an output -15 deg <= angle <= 15 deg
  1199. // using modified Pade[3/2] approximation
  1200. static float fatan_15deg(float x)
  1201. {
  1202. float x2; // x^2
  1203. #define PADE_A 96.644395816F // theoretical Pade[3/2] value is 5/3*180/PI=95.49296
  1204. #define PADE_B 25.086941612F // theoretical Pade[3/2] value is 4/9*180/PI=25.46479
  1205. #define PADE_C 1.6867633134F // theoretical Pade[3/2] value is 5/3=1.66667
  1206. // compute the approximation to the inverse tangent
  1207. // the function is anti-symmetric as required for positive and negative arguments
  1208. x2 = x * x;
  1209. return (x * (PADE_A + x2 * PADE_B) / (PADE_C + x2));
  1210. }