ISF  2.2 rev 5
Intelligent Sensing Framework for Kinetis with Processor Expert
fusion.c
Go to the documentation of this file.
1 // Copyright (c) 2014, 2015, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 //
26 // This is the file that contains the fusion routines. It is STRONGLY RECOMMENDED
27 // that the casual developer NOT TOUCH THIS FILE. The mathematics behind this file
28 // is extremely complex, and it will be very easy (almost inevitable) that you screw
29 // it up.
30 //
31 
32 #include "math.h"
33 #include "basic_types.h"
34 #include "math_constants.h"
35 #include "approximations.h"
36 
37 #include "fusion_config.h"
38 #include "fusion.h"
39 #include "orientation.h"
40 #include "drivers.h" // ABHI_TEST
41 extern struct ProjectGlobals globals;// ABHI_TEST
42 
43 //////////////////////////////////////////////////////////////////////////////////////////////////
44 // intialization functions for the sensor fusion algorithms
45 //////////////////////////////////////////////////////////////////////////////////////////////////
46 #if 0
47 // function initializes the sensor fusion and magnetic calibration and sets loopcounter to zero
48 void fInitFusion(void)
49 {
50  // reset the default quaternion type to the simplest Q3 (it will be updated during the initializations)
51  globals.DefaultQuaternionPacketType = Q3;
52 
53  // force a reset of all the algorithms next time they execute
54  // the initialization will result in the default and current quaternion being set to the most sophisticated
55  // algorithm supported by the build
56 #if defined COMPUTE_1DOF_P_BASIC
58 #endif
59 #if defined COMPUTE_3DOF_G_BASIC
61 #endif
62 #if defined COMPUTE_3DOF_B_BASIC
64 #endif
65 #if defined COMPUTE_3DOF_Y_BASIC
67 #endif
68 #if defined COMPUTE_6DOF_GB_BASIC
70 #endif
71 #if defined COMPUTE_6DOF_GY_KALMAN
73 #endif
74 #if defined COMPUTE_9DOF_GBY_KALMAN
76 #endif
77 
78  // reset the loop counter to zero for first iteration
79  globals.loopcounter = 0;
80 
81  return;
82 }
83 #endif
84 void fInit_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, float flpftimesecs)
85 {
86  // set algorithm sampling interval (typically 25Hz) and low pass filter
87  // Note: the MPL3115 sensor only updates its output every 512ms and is therefore repeatedly oversampled at 25Hz
88  // but executing the exponenial filter at the 25Hz rate also performs an interpolation giving smoother output.
89  pthisSV->fdeltat = (float) OVERSAMPLE_RATIO / (float) SENSORFS;
90  pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
91  if (pthisSV->flpf > 1.0F)
92  {
93  pthisSV->flpf = 1.0F;
94  }
95 
96  // initialize the filters
97  pthisSV->fLPH = pthisPressure->fH;
98  pthisSV->fLPT = pthisPressure->fT;
99 
100  // clear the reset flag
101  pthisSV->resetflag = false;
102 
103  return;
104 } // end fInit_1DOF_P_BASIC
105 
106 void fInit_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, float flpftimesecs)
107 {
108  // set algorithm sampling interval (typically 25Hz)
109  pthisSV->fdeltat = (float) OVERSAMPLE_RATIO / (float) SENSORFS;
110 
111  // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
112  if (flpftimesecs > pthisSV->fdeltat)
113  pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
114  else
115  pthisSV->flpf = 1.0F;
116 
117  // apply the tilt estimation algorithm to set the low pass orientation matrix and quaternion
118  switch (THISCOORDSYSTEM)
119  {
120  case NED:
121  f3DOFTiltNED(pthisSV->fLPR, pthisAccel->fGsAvg);
122  break;
123  case ANDROID:
124  f3DOFTiltAndroid(pthisSV->fLPR, pthisAccel->fGsAvg);
125  break;
126  case WIN8:
127  default:
128  f3DOFTiltWin8(pthisSV->fLPR, pthisAccel->fGsAvg);
129  break;
130  }
131  fQuaternionFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPq));
132 
133  // update the default quaternion type supported to the most sophisticated
134  if (globals.DefaultQuaternionPacketType < Q3)
135  globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q3;
136 
137  // clear the reset flag
138  pthisSV->resetflag = false;
139 
140  return;
141 } // end fInit_3DOF_G_BASIC
142 
143 void fInit_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, float flpftimesecs)
144 {
145  // set algorithm sampling interval (typically 25Hz)
146  pthisSV->fdeltat = (float) OVERSAMPLE_RATIO / (float) SENSORFS;
147 
148  // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
149  if (flpftimesecs > pthisSV->fdeltat)
150  pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
151  else
152  pthisSV->flpf = 1.0F;
153 
154  // initialize the low pass filtered magnetometer orientation matrix and quaternion using fBcAvg
155  switch (THISCOORDSYSTEM)
156  {
157  case NED:
158  f3DOFMagnetometerMatrixNED(pthisSV->fLPR, pthisMag->fBcAvg);
159  break;
160  case ANDROID:
161  f3DOFMagnetometerMatrixAndroid(pthisSV->fLPR, pthisMag->fBcAvg);
162  break;
163  case WIN8:
164  default:
165  f3DOFMagnetometerMatrixWin8(pthisSV->fLPR, pthisMag->fBcAvg);
166  break;
167  }
168  fQuaternionFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPq));
169 
170  // update the default quaternion type supported to the most sophisticated
171  if (globals.DefaultQuaternionPacketType < Q3M)
172  globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q3M;
173 
174  // clear the reset flag
175  pthisSV->resetflag = false;
176 
177  return;
178 } // end fInit_3DOF_B_BASIC
179 
180 void fInit_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV)
181 {
182  // compute the sampling time intervals (secs)
183  pthisSV->fGyrodeltat = 1.0F / (float) SENSORFS;
184  pthisSV->fdeltat = (float) OVERSAMPLE_RATIO * pthisSV->fGyrodeltat;
185 
186  // initialize orientation estimate to flat
187  f3x3matrixAeqI(pthisSV->fR);
188  fqAeq1(&(pthisSV->fq));
189 
190  // update the default quaternion type supported to the most sophisticated
191  if (globals.DefaultQuaternionPacketType < Q3G)
192  globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q3G;
193 
194  // clear the reset flag
195  pthisSV->resetflag = false;
196 
197  return;
198 } // end fInit_3DOF_Y_BASIC
199 
200 void fInit_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, float flpftimesecs)
201 {
202  // set algorithm sampling interval (typically 25Hz)
203  pthisSV->fdeltat = (float) OVERSAMPLE_RATIO / (float) SENSORFS;
204 
205  // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
206  if (flpftimesecs > pthisSV->fdeltat)
207  pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
208  else
209  pthisSV->flpf = 1.0F;
210 
211  // initialize the instantaneous orientation matrix, inclination angle and quaternion
212  switch (THISCOORDSYSTEM)
213  {
214  case NED:
215  feCompassNED(pthisSV->fLPR, &(pthisSV->fLPDelta), pthisMag->fBcAvg, pthisAccel->fGsAvg);
216  break;
217  case ANDROID:
218  feCompassAndroid(pthisSV->fLPR, &(pthisSV->fLPDelta), pthisMag->fBcAvg, pthisAccel->fGsAvg);
219  break;
220  case WIN8:
221  default:
222  feCompassWin8(pthisSV->fLPR, &(pthisSV->fLPDelta), pthisMag->fBcAvg, pthisAccel->fGsAvg);
223  break;
224  }
225  fQuaternionFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPq));
226 
227  // update the default quaternion type supported to the most sophisticated
228  if (globals.DefaultQuaternionPacketType < Q6MA)
229  globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q6MA;
230 
231  // clear the reset flag
232  pthisSV->resetflag = false;
233 
234  return;
235 } // end fInit_6DOF_GB_BASIC
236 
237 // function initalizes the 6DOF accel + gyro Kalman filter algorithm
238 void fInit_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel)
239 {
240  int8 i; // counter
241 
242  // compute and store useful product terms to save floating point calculations later
243  pthisSV->fGyrodeltat = 1.0F / (float) SENSORFS;
244  pthisSV->fAlphaOver2 = 0.5F * FPIOVER180 * (float) OVERSAMPLE_RATIO / (float) SENSORFS;
245  pthisSV->fAlphaOver2Sq = pthisSV->fAlphaOver2 * pthisSV->fAlphaOver2;
246  pthisSV->fAlphaOver2Qwb = pthisSV->fAlphaOver2 * FQWB_6DOF_GY_KALMAN;
248 
249  // zero the a posteriori gyro offset and error vectors
250  for (i = CHX; i <= CHZ; i++)
251  {
252  pthisSV->fbPl[i] = 0.0F;
253  pthisSV->fqgErrPl[i] = 0.0F;
254  pthisSV->fbErrPl[i] = 0.0F;
255  }
256 
257  // initialize the a posteriori orientation state vector to the tilt orientation
258  switch (THISCOORDSYSTEM)
259  {
260  case NED:
261  f3DOFTiltNED(pthisSV->fRPl, pthisAccel->fGsAvg);
262  break;
263  case ANDROID:
264  f3DOFTiltAndroid(pthisSV->fRPl, pthisAccel->fGsAvg);
265  break;
266  case WIN8:
267  default:
268  f3DOFTiltWin8(pthisSV->fRPl, pthisAccel->fGsAvg);
269  break;
270  }
271  fQuaternionFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fqPl));
272 
273  // update the default quaternion type supported to the most sophisticated
274  if (globals.DefaultQuaternionPacketType < Q6AG)
275  globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q6AG;
276 
277  // clear the reset flag
278  pthisSV->resetflag = false;
279 
280  return;
281 } // end fInit_6DOF_GY_KALMAN
282 
283 // function initializes the 9DOF Kalman filter
284 void fInit_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag,
285  struct MagCalibration *pthisMagCal)
286 {
287  float fDelta6DOF; // eCompass estimate of inclination angle returned by least squares eCompass
288  float pfQvGQa; // accelerometer noise covariance to 1g sphere
289  float fQvBQd; // magnetometer noise covariances to geomagnetic sphere
290  int8 i; // counter
291 
292  // compute and store useful product terms to save floating point calculations later
293  pthisSV->fGyrodeltat = 1.0F / (float) SENSORFS;
294  pthisSV->fKalmandeltat = (float) OVERSAMPLE_RATIO / (float) SENSORFS;
295  pthisSV->fgKalmandeltat = GTOMSEC2 * pthisSV->fKalmandeltat;
296  pthisSV->fAlphaOver2 = 0.5F * FPIOVER180 * (float) OVERSAMPLE_RATIO / (float) SENSORFS;
297  pthisSV->fAlphaOver2Sq = pthisSV->fAlphaOver2 * pthisSV->fAlphaOver2;
298  pthisSV->fAlphaOver2Qwb = pthisSV->fAlphaOver2 * FQWB_9DOF_GBY_KALMAN;
300 
301  // zero the a posteriori gyro offset and error vectors
302  for (i = CHX; i <= CHZ; i++)
303  {
304  pthisSV->fbPl[i] = 0.0F;
305  pthisSV->fqgErrPl[i] = 0.0F;
306  pthisSV->fqmErrPl[i] = 0.0F;
307  pthisSV->fbErrPl[i] = 0.0F;
308  }
309  pthisSV->fDeltaPl = 0.0F;
310  pthisSV->fDeltaErrPl = 0.0F;
311 
312  // zero the inertial navigation velocity and displacement in the global frame
313  for (i = CHX; i <= CHZ; i++)
314  {
315  pthisSV->fVelGl[i] = 0.0F;
316  pthisSV->fDisGl[i] = 0.0F;
317  }
318 
319  // initialize the posteriori orientation state vector to the instantaneous eCompass orientation
320  pthisSV->iFirstAccelMagLock = false;
321  switch (THISCOORDSYSTEM)
322  {
323  case NED:
324  fLeastSquareseCompassNED(&(pthisSV->fqPl), pthisMagCal->fB, pthisSV->fDeltaPl, pthisSV->fsinDeltaPl,
325  pthisSV->fcosDeltaPl, &fDelta6DOF, pthisMag->fBcAvg, pthisAccel->fGsAvg, &fQvBQd, &pfQvGQa);
326  break;
327  case ANDROID:
328  fLeastSquareseCompassAndroid(&(pthisSV->fqPl), pthisMagCal->fB, pthisSV->fDeltaPl, pthisSV->fsinDeltaPl,
329  pthisSV->fcosDeltaPl, &fDelta6DOF, pthisMag->fBcAvg, pthisAccel->fGsAvg, &fQvBQd, &pfQvGQa);
330  break;
331  case WIN8:
332  default:
333  fLeastSquareseCompassWin8(&(pthisSV->fqPl), pthisMagCal->fB, pthisSV->fDeltaPl, pthisSV->fsinDeltaPl,
334  pthisSV->fcosDeltaPl, &fDelta6DOF, pthisMag->fBcAvg, pthisAccel->fGsAvg, &fQvBQd, &pfQvGQa);
335  break;
336  }
337  fRotationMatrixFromQuaternion(pthisSV->fRPl, &(pthisSV->fqPl));
338 
339  // initialize the geomagnetic inclination angle to the estimate from the eCompass call.
340  // this will be inaccurate because of the lack of a magnetic calibration but prevents a transient in
341  // the Kalman filter that will be tracking it even before the first magnetic calibration.
342  pthisSV->fDeltaPl = fDelta6DOF;
343  pthisSV->fsinDeltaPl = sinf(pthisSV->fDeltaPl * FPIOVER180);
344  pthisSV->fcosDeltaPl = sqrtf(1.0F - pthisSV->fsinDeltaPl * pthisSV->fsinDeltaPl);
345 
346  // update the default quaternion type supported to the most sophisticated
347  if (globals.DefaultQuaternionPacketType < Q9)
348  globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q9;
349 
350  // clear the reset flag
351  pthisSV->resetflag = false;
352 
353  return;
354 } // end fInit_9DOF_GBY_KALMAN
355 
356 //////////////////////////////////////////////////////////////////////////////////////////////////
357 // run time functions for the sensor fusion algorithms
358 //////////////////////////////////////////////////////////////////////////////////////////////////
359 
360 // 1DOF pressure function
361 void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure)
362 {
363  // if requested, do a reset (with low pass filter time constant 1.5s) and return
364  if (pthisSV->resetflag)
365  {
366  fInit_1DOF_P_BASIC(pthisSV, pthisPressure, 1.5F);
367  return;
368  }
369 
370  // exponentially low pass filter the pressure and temperature.
371  // when executed at (typically) 25Hz, this filter interpolates the raw signals which are
372  // updated every 512ms only.
373  pthisSV->fLPH += pthisSV->flpf * (pthisPressure->fH - pthisSV->fLPH);
374  pthisSV->fLPT += pthisSV->flpf * (pthisPressure->fT - pthisSV->fLPT);
375 
376  return;
377 } // end fRun_1DOF_P_BASIC
378 
379 // 3DOF orientation function which calls tilt functions and implements low pass filters
380 void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel)
381 {
382  // if requested, do a reset (with low pass filter time constant 0.5s) and return
383  if (pthisSV->resetflag)
384  {
385  fInit_3DOF_G_BASIC(pthisSV, pthisAccel, 0.5F);
386  return;
387  }
388 
389  // apply the tilt estimation algorithm to get the instantaneous orientation matrix and quaternion
390  switch (THISCOORDSYSTEM)
391  {
392  case NED:
393  f3DOFTiltNED(pthisSV->fR, pthisAccel->fGsAvg);
394  break;
395  case ANDROID:
396  f3DOFTiltAndroid(pthisSV->fR, pthisAccel->fGsAvg);
397  break;
398  case WIN8:
399  default:
400  f3DOFTiltWin8(pthisSV->fR, pthisAccel->fGsAvg);
401  break;
402  }
403  fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
404 
405  // low pass filter the orientation quaternion
406  fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf, pthisSV->fdeltat, pthisSV->fOmega);
407 
408  // compute the low pass rotation matrix and rotation vector
409  fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
410  fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);
411 
412  // calculate the Euler angles from the low pass orientation matrix
413  switch (THISCOORDSYSTEM)
414  {
415  case NED:
416  fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
417  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
418  break;
419  case ANDROID:
420  fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
421  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
422  break;
423  case WIN8:
424  default:
425  fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
426  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
427  break;
428  }
429 
430  // force the yaw and compass angles to zero
431  pthisSV->fLPPsi = pthisSV->fLPRho = 0.0F;
432 
433  return;
434 } // end fRun_3DOF_G_BASIC
435 
436 // 2D automobile eCompass
437 void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag)
438 {
439  // if requested, do a reset (with low pass filter time constant 1.0s) and return
440  if (pthisSV->resetflag)
441  {
442  fInit_3DOF_B_BASIC(pthisSV, pthisMag, 1.0F);
443  return;
444  }
445 
446  // calculate the 3DOF magnetometer orientation matrix and quaternion from fBcAvg
447  switch (THISCOORDSYSTEM)
448  {
449  case NED:
450  f3DOFMagnetometerMatrixNED(pthisSV->fR, pthisMag->fBcAvg);
451  break;
452  case ANDROID:
453  f3DOFMagnetometerMatrixAndroid(pthisSV->fR, pthisMag->fBcAvg);
454  break;
455  case WIN8:
456  default:
457  f3DOFMagnetometerMatrixWin8(pthisSV->fR, pthisMag->fBcAvg);
458  break;
459  }
460  fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
461 
462  // low pass filter the orientation quaternion and compute the low pass rotation matrix
463  fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf, pthisSV->fdeltat, pthisSV->fOmega);
464  fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
465  fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);
466 
467  // calculate the Euler angles from the low pass orientation matrix
468  switch (THISCOORDSYSTEM)
469  {
470  case NED:
471  fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
472  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
473  break;
474  case ANDROID:
475  fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
476  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
477  break;
478  case WIN8:
479  default:
480  fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
481  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
482  break;
483  }
484 
485  return;
486 }
487 
488 // basic gyro integration function
489 void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro)
490 {
491  struct fquaternion ftmpq; // scratch quaternion
492  float ftmpA3x1[3]; // rotation vector
493  int8 i, j; // loop counters
494 
495  // if requested, do a reset and return
496  if (pthisSV->resetflag)
497  {
498  fInit_3DOF_Y_BASIC(pthisSV);
499  return;
500  }
501 
502  // integrate the buffered high frequency (typically 200Hz) gyro readings
503  for (j = 0; j < OVERSAMPLE_RATIO; j++)
504  {
505  // compute the incremental fast (typically 200Hz) rotation vector rvec (deg)
506  for (i = CHX; i <= CHZ; i++)
507  {
508  pthisSV->fOmega[i] = (float)pthisGyro->iYsBuffer[j][i] * pthisGyro->fDegPerSecPerCount;
509  ftmpA3x1[i] = pthisSV->fOmega[i] * pthisSV->fGyrodeltat;
510  }
511 
512  // compute the incremental quaternion fDeltaq from the rotation vector
513  fQuaternionFromRotationVectorDeg(&ftmpq, ftmpA3x1, 1.0F);
514 
515  // incrementally rotate the orientation quaternion fq
516  qAeqAxB(&(pthisSV->fq), &ftmpq);
517  }
518 
519  // re-normalize the orientation quaternion to stop error propagation
520  // the renormalization function ensures that the scalar component q0 is non-negative
521  fqAeqNormqA(&(pthisSV->fq));
522 
523  // get the rotation matrix from the quaternion
524  fRotationMatrixFromQuaternion(pthisSV->fR, &(pthisSV->fq));
525 
526  // compute the rotation vector from the a posteriori quaternion
527  fRotationVectorDegFromQuaternion(&(pthisSV->fq), pthisSV->fRVec);
528 
529  // compute the Euler angles from the orientation matrix
530  switch (THISCOORDSYSTEM)
531  {
532  case NED:
533  fNEDAnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi), &(pthisSV->fThe), &(pthisSV->fPsi),
534  &(pthisSV->fRho), &(pthisSV->fChi));
535  break;
536  case ANDROID:
537  fAndroidAnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi), &(pthisSV->fThe), &(pthisSV->fPsi),
538  &(pthisSV->fRho), &(pthisSV->fChi));
539  break;
540  case WIN8:
541  default:
542  fWin8AnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi), &(pthisSV->fThe), &(pthisSV->fPsi),
543  &(pthisSV->fRho), &(pthisSV->fChi));
544  break;
545  }
546 
547  return;
548 } // end fRun_3DOF_Y_BASIC
549 
550 // 6DOF eCompass orientation function
551 void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
552 {
553  // if requested, do a reset (with low pass filter time constant 1.0s) and return
554  if (pthisSV->resetflag)
555  {
556  fInit_6DOF_GB_BASIC(pthisSV, pthisAccel, pthisMag, 1.0F);
557  return;
558  }
559 
560  // call the eCompass algorithm to get the instantaneous orientation matrix, inclination angle and quanternion
561  switch (THISCOORDSYSTEM)
562  {
563  case NED:
564  feCompassNED(pthisSV->fR, &(pthisSV->fDelta), pthisMag->fBcAvg, pthisAccel->fGsAvg);
565  break;
566  case ANDROID:
567  feCompassAndroid(pthisSV->fR, &(pthisSV->fDelta), pthisMag->fBcAvg, pthisAccel->fGsAvg);
568  break;
569  case WIN8:
570  default:
571  feCompassWin8(pthisSV->fR, &(pthisSV->fDelta), pthisMag->fBcAvg, pthisAccel->fGsAvg);
572  break;
573  }
574  fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
575 
576  // low pass filter the orientation quaternion and compute the low pass rotation matrix and rotation vector
577  fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf, pthisSV->fdeltat, pthisSV->fOmega);
578  fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
579  fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);
580 
581  // compute the low pass filtered Euler angles
582  switch (THISCOORDSYSTEM)
583  {
584  case NED:
585  fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
586  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
587  break;
588  case ANDROID:
589  fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
590  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
591  break;
592  case WIN8:
593  default:
594  fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
595  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
596  break;
597  }
598 
599  // low pass filter the geomagnetic inclination angle with a simple exponential filter
600  pthisSV->fLPDelta += pthisSV->flpf * (pthisSV->fDelta - pthisSV->fLPDelta);
601 
602  return;
603 } // end fRun_6DOF_GB_BASIC
604 
605 // 6DOF accelerometer+gyroscope orientation function implemented using indirect complementary Kalman filter
606 void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
607 {
608  // local scalars and arrays
609  float ftmpMi3x1[3]; // temporary vector used for a priori calculations
610  float ftmp3DOF3x1[3]; // temporary vector used for 3DOF calculations
611  float ftmpA3x3[3][3]; // scratch 3x3 matrix
612  float fC3x6ik; // element i, k of measurement matrix C
613  float fC3x6jk; // element j, k of measurement matrix C
614  float fmodSqGs; // modulus squared of accelerometer measurement
615  struct fquaternion fqMi; // a priori orientation quaternion
616  struct fquaternion ftmpq; // scratch quaternion
617  float ftmp; // scratch float
618  int8 ierror; // matrix inversion error flag
619  int8 i, j, k; // loop counters
620 
621  // working arrays for 3x3 matrix inversion
622  float *pfRows[3];
623  int8 iColInd[3];
624  int8 iRowInd[3];
625  int8 iPivot[3];
626 
627  // if requested, do a reset initialization with no further processing
628  if (pthisSV->resetflag)
629  {
630  fInit_6DOF_GY_KALMAN(pthisSV, pthisAccel);
631  return;
632  }
633 
634  // compute the a priori orientation quaternion fqMi by integrating the buffered gyro measurements
635  fqMi = pthisSV->fqPl;
636  // loop over all the buffered gyroscope measurements
637  for (j = 0; j < OVERSAMPLE_RATIO; j++)
638  {
639  // calculate the instantaneous angular velocity (after subtracting the gyro offset) and rotation vector ftmpMi3x1
640  for (i = CHX; i <= CHZ; i++)
641  {
642  pthisSV->fOmega[i] = (float)pthisGyro->iYsBuffer[j][i] * pthisGyro->fDegPerSecPerCount - pthisSV->fbPl[i];
643  ftmpMi3x1[i] = pthisSV->fOmega[i] * pthisSV->fGyrodeltat;
644  }
645  // convert the rotation vector ftmpMi3x1 to a rotation quaternion ftmpq
646  fQuaternionFromRotationVectorDeg(&ftmpq, ftmpMi3x1, 1.0F);
647  // integrate the gyro sensor incremental quaternions into the a priori orientation quaternion fqMi
648  qAeqAxB(&fqMi, &ftmpq);
649  }
650 
651  // set ftmp3DOF3x1 to the 3DOF gravity vector in the sensor frame
652  fmodSqGs = pthisAccel->fGsAvg[CHX] * pthisAccel->fGsAvg[CHX] + pthisAccel->fGsAvg[CHY] * pthisAccel->fGsAvg[CHY] +
653  pthisAccel->fGsAvg[CHZ] * pthisAccel->fGsAvg[CHZ];
654  if (fmodSqGs != 0.0F)
655  {
656  // normal non-freefall case
657  ftmp = 1.0F / sqrt(fabsf(fmodSqGs));
658  ftmp3DOF3x1[CHX] = pthisAccel->fGsAvg[CHX] * ftmp;
659  ftmp3DOF3x1[CHY] = pthisAccel->fGsAvg[CHY] * ftmp;
660  ftmp3DOF3x1[CHZ] = pthisAccel->fGsAvg[CHZ] * ftmp;
661  }
662  else
663  {
664  // use zero tilt in case of freefall
665  ftmp3DOF3x1[CHX] = 0.0F;
666  ftmp3DOF3x1[CHY] = 0.0F;
667  ftmp3DOF3x1[CHZ] = 1.0F;
668  }
669 
670  // correct accelerometer gravity vector for different coordinate systems
671  switch (THISCOORDSYSTEM)
672  {
673  case NED:
674  // +1g in accelerometer z axis (z down) when PCB is flat so no correction needed
675  break;
676  case ANDROID:
677  // +1g in accelerometer z axis (z up) when PCB is flat so negate the vector to obtain gravity
678  ftmp3DOF3x1[CHX] = -ftmp3DOF3x1[CHX];
679  ftmp3DOF3x1[CHY] = -ftmp3DOF3x1[CHY];
680  ftmp3DOF3x1[CHZ] = -ftmp3DOF3x1[CHZ];
681  break;
682  case WIN8:
683  default:
684  // -1g in accelerometer z axis (z up) when PCB is flat so no correction needed
685  break;
686  }
687 
688  // set ftmpMi3x1 to the a priori gravity vector in the sensor frame from the a priori quaternion
689  ftmpMi3x1[CHX] = 2.0F * (fqMi.q1 * fqMi.q3 - fqMi.q0 * fqMi.q2);
690  ftmpMi3x1[CHY] = 2.0F * (fqMi.q2 * fqMi.q3 + fqMi.q0 * fqMi.q1);
691  ftmpMi3x1[CHZ] = 2.0F * (fqMi.q0 * fqMi.q0 + fqMi.q3 * fqMi.q3) - 1.0F;
692 
693  // correct a priori gravity vector for different coordinate systems
694  switch (THISCOORDSYSTEM)
695  {
696  case NED:
697  // z axis is down (NED) so no correction needed
698  break;
699  case ANDROID:
700  case WIN8:
701  default:
702  // z axis is up (ENU) so no negate the vector to obtain gravity
703  ftmpMi3x1[CHX] = -ftmpMi3x1[CHX];
704  ftmpMi3x1[CHY] = -ftmpMi3x1[CHY];
705  ftmpMi3x1[CHZ] = -ftmpMi3x1[CHZ];
706  break;
707  }
708  // calculate the rotation quaternion between 3DOF and a priori gravity vectors (ignored minus signs cancel here)
709  // and copy the quaternion vector components to the measurement error vector fZErr
710  fveqconjgquq(&ftmpq, ftmp3DOF3x1, ftmpMi3x1);
711  pthisSV->fZErr[CHX] = ftmpq.q1;
712  pthisSV->fZErr[CHY] = ftmpq.q2;
713  pthisSV->fZErr[CHZ] = ftmpq.q3;
714 
715  // update Qw using the a posteriori error vectors from the previous iteration.
716  // as Qv increases or Qw decreases, K -> 0 and the Kalman filter is weighted towards the a priori prediction
717  // as Qv decreases or Qw increases, KC -> I and the Kalman filter is weighted towards the measurement.
718  // initialize Qw to all zeroes
719  for (i = 0; i < 6; i++)
720  for (j = 0; j < 6; j++)
721  pthisSV->fQw6x6[i][j] = 0.0F;
722  // partial diagonal gyro offset terms
723  pthisSV->fQw6x6[3][3] = pthisSV->fbErrPl[CHX] * pthisSV->fbErrPl[CHX];
724  pthisSV->fQw6x6[4][4] = pthisSV->fbErrPl[CHY] * pthisSV->fbErrPl[CHY];
725  pthisSV->fQw6x6[5][5] = pthisSV->fbErrPl[CHZ] * pthisSV->fbErrPl[CHZ];
726  // diagonal gravity vector components
727  pthisSV->fQw6x6[0][0] = pthisSV->fqgErrPl[CHX] * pthisSV->fqgErrPl[CHX] + pthisSV->fAlphaOver2SqQvYQwb +
728  pthisSV->fAlphaOver2Sq * pthisSV->fQw6x6[3][3];
729  pthisSV->fQw6x6[1][1] = pthisSV->fqgErrPl[CHY] * pthisSV->fqgErrPl[CHY] + pthisSV->fAlphaOver2SqQvYQwb +
730  pthisSV->fAlphaOver2Sq * pthisSV->fQw6x6[4][4];
731  pthisSV->fQw6x6[2][2] = pthisSV->fqgErrPl[CHZ] * pthisSV->fqgErrPl[CHZ] + pthisSV->fAlphaOver2SqQvYQwb +
732  pthisSV->fAlphaOver2Sq * pthisSV->fQw6x6[5][5];
733  // remaining diagonal gyro offset components
734  pthisSV->fQw6x6[3][3] += FQWB_6DOF_GY_KALMAN;
735  pthisSV->fQw6x6[4][4] += FQWB_6DOF_GY_KALMAN;
736  pthisSV->fQw6x6[5][5] += FQWB_6DOF_GY_KALMAN;
737  // off diagonal gravity and gyro offset components
738  pthisSV->fQw6x6[0][3] = pthisSV->fQw6x6[3][0] = pthisSV->fqgErrPl[CHX] * pthisSV->fbErrPl[CHX] - pthisSV->fAlphaOver2Qwb;
739  pthisSV->fQw6x6[1][4] = pthisSV->fQw6x6[4][1] = pthisSV->fqgErrPl[CHY] * pthisSV->fbErrPl[CHY] - pthisSV->fAlphaOver2Qwb;
740  pthisSV->fQw6x6[2][5] = pthisSV->fQw6x6[5][2] = pthisSV->fqgErrPl[CHZ] * pthisSV->fbErrPl[CHZ] - pthisSV->fAlphaOver2Qwb;
741 
742  // calculate the vector fQv containing the diagonal elements of the measurement covariance matrix Qv
743  pthisSV->fQv = 0.25F * fabsf(fmodSqGs - 1.0F) + pthisSV->fAlphaOver2SqQvYQwb;
744 
745  // calculate the 6x3 Kalman gain matrix K = Qw * C^T * inv(C * Qw * C^T + Qv)
746  // set fQwCT6x3 = Qw.C^T where Qw has size 6x6 and C^T has size 6x3
747  for (i = 0; i < 6; i++) // loop over rows
748  {
749  for (j = 0; j < 3; j++) // loop over columns
750  {
751  pthisSV->fQwCT6x3[i][j] = 0.0F;
752  // accumulate matrix sum
753  for (k = 0; k < 6; k++)
754  {
755  // determine fC3x6[j][k] since the matrix is highly sparse
756  fC3x6jk = 0.0F;
757  if (k == j) fC3x6jk = 1.0F;
758  if (k == (j + 3)) fC3x6jk = -pthisSV->fAlphaOver2;
759  // accumulate fQwCT6x3[i][j] += Qw6x6[i][k] * C[j][k]
760  if ((pthisSV->fQw6x6[i][k] != 0.0F) && (fC3x6jk != 0.0F))
761  {
762  if (fC3x6jk == 1.0F)
763  pthisSV->fQwCT6x3[i][j] += pthisSV->fQw6x6[i][k];
764  else
765  pthisSV->fQwCT6x3[i][j] += pthisSV->fQw6x6[i][k] * fC3x6jk;
766  }
767  }
768  }
769  }
770 
771  // set symmetric ftmpA3x3 = C.(Qw.C^T) + Qv = C.fQwCT6x3 + Qv
772  for (i = 0; i < 3; i++) // loop over rows
773  {
774  for (j = i; j < 3; j++) // loop over on and above diagonal columns
775  {
776  // zero off diagonal and set diagonal to Qv
777  if (i == j)
778  ftmpA3x3[i][j] = pthisSV->fQv;
779  else
780  ftmpA3x3[i][j] = 0.0F;
781  // accumulate matrix sum
782  for (k = 0; k < 6; k++)
783  {
784  // determine fC3x6[i][k]
785  fC3x6ik = 0.0F;
786  if (k == i) fC3x6ik = 1.0F;
787  if (k == (i + 3)) fC3x6ik = -pthisSV->fAlphaOver2;
788 
789  // accumulate ftmpA3x3[i][j] += C[i][k] & fQwCT6x3[k][j]
790  if ((fC3x6ik != 0.0F) && (pthisSV->fQwCT6x3[k][j] != 0.0F))
791  {
792  if (fC3x6ik == 1.0F)
793  ftmpA3x3[i][j] += pthisSV->fQwCT6x3[k][j];
794  else
795  ftmpA3x3[i][j] += fC3x6ik * pthisSV->fQwCT6x3[k][j];
796  }
797  }
798  }
799  }
800  // set ftmpA3x3 below diagonal elements to above diagonal elements
801  ftmpA3x3[1][0] = ftmpA3x3[0][1];
802  ftmpA3x3[2][0] = ftmpA3x3[0][2];
803  ftmpA3x3[2][1] = ftmpA3x3[1][2];
804 
805  // invert ftmpA3x3 in situ to give ftmpA3x3 = inv(C * Qw * C^T + Qv) = inv(ftmpA3x3)
806  for (i = 0; i < 3; i++)
807  pfRows[i] = ftmpA3x3[i];
808  fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 3, &ierror);
809 
810  // on successful inversion set Kalman gain matrix fK6x3 = Qw * C^T * inv(C * Qw * C^T + Qv) = fQwCT6x3 * ftmpA3x3
811  if (!ierror)
812  {
813  // normal case
814  for (i = 0; i < 6; i++) // loop over rows
815  {
816  for (j = 0; j < 3; j++) // loop over columns
817  {
818  pthisSV->fK6x3[i][j] = 0.0F;
819  for (k = 0; k < 3; k++)
820  {
821  if ((pthisSV->fQwCT6x3[i][k] != 0.0F) && (ftmpA3x3[k][j] != 0.0F))
822  {
823  pthisSV->fK6x3[i][j] += pthisSV->fQwCT6x3[i][k] * ftmpA3x3[k][j];
824  }
825  }
826  }
827  }
828  }
829  else
830  {
831  // ftmpA3x3 was singular so set Kalman gain matrix fK6x3 to zero
832  for (i = 0; i < 6; i++) // loop over rows
833  {
834  for (j = 0; j < 3; j++) // loop over columns
835  {
836  pthisSV->fK6x3[i][j] = 0.0F;
837  }
838  }
839  }
840 
841  // calculate the a posteriori gravity and geomagnetic tilt quaternion errors and gyro offset error vector
842  // from the Kalman matrix fK6x3 and from the measurement error vector fZErr.
843  for (i = CHX; i <= CHZ; i++)
844  {
845  // gravity tilt vector error component
846  if (pthisSV->fK6x3[i][CHX] != 0.0F)
847  pthisSV->fqgErrPl[i] = pthisSV->fK6x3[i][CHX] * pthisSV->fZErr[CHX];
848  else
849  pthisSV->fqgErrPl[i] = 0.0F;
850  if (pthisSV->fK6x3[i][CHY] != 0.0F) pthisSV->fqgErrPl[i] += pthisSV->fK6x3[i][CHY] * pthisSV->fZErr[CHY];
851  if (pthisSV->fK6x3[i][CHZ] != 0.0F) pthisSV->fqgErrPl[i] += pthisSV->fK6x3[i][CHZ] * pthisSV->fZErr[CHZ];
852 
853  // gyro offset vector error component
854  if (pthisSV->fK6x3[i + 3][CHX] != 0.0F)
855  pthisSV->fbErrPl[i] = pthisSV->fK6x3[i + 3][CHX] * pthisSV->fZErr[CHX];
856  else
857  pthisSV->fbErrPl[i] = 0.0F;
858  if (pthisSV->fK6x3[i + 3][CHY] != 0.0F) pthisSV->fbErrPl[i] += pthisSV->fK6x3[i + 3][CHY] * pthisSV->fZErr[CHY];
859  if (pthisSV->fK6x3[i + 3][CHZ] != 0.0F) pthisSV->fbErrPl[i] += pthisSV->fK6x3[i + 3][CHZ] * pthisSV->fZErr[CHZ];
860  }
861 
862  // set ftmpq to the gravity tilt correction (conjugate) quaternion
863  ftmpq.q1 = -pthisSV->fqgErrPl[CHX];
864  ftmpq.q2 = -pthisSV->fqgErrPl[CHY];
865  ftmpq.q3 = -pthisSV->fqgErrPl[CHZ];
866  ftmp = ftmpq.q1 * ftmpq.q1 + ftmpq.q2 * ftmpq.q2 + ftmpq.q3 * ftmpq.q3;
867  // determine the scalar component q0
868  if (ftmp <= 1.0F)
869  {
870  // normal case
871  ftmpq.q0 = sqrtf(fabsf(1.0F - ftmp));
872  }
873  else
874  {
875  // if vector component exceeds unity then rounding errors are present at 180 deg rotation
876  // so set scalar component to zero for 180 deg rotation about the rotation vector
877  ftmpq.q0 = 0.0F;
878  }
879  // apply the gravity tilt correction quaternion so fqPl = fqMi.(fqgErrPl)* = fqMi.ftmpq and normalize
880  qAeqBxC(&(pthisSV->fqPl), &fqMi, &ftmpq);
881 
882  // normalize the a posteriori quaternion and compute the a posteriori rotation matrix and rotation vector
883  fqAeqNormqA(&(pthisSV->fqPl));
884  fRotationMatrixFromQuaternion(pthisSV->fRPl, &(pthisSV->fqPl));
885  fRotationVectorDegFromQuaternion(&(pthisSV->fqPl), pthisSV->fRVecPl);
886 
887  // update the a posteriori gyro offset vector: b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s)
888  for (i = CHX; i <= CHZ; i++)
889  pthisSV->fbPl[i] -= pthisSV->fbErrPl[i];
890 
891  // compute the linear acceleration in the global frame
892  // de-rotate the accelerometer from the sensor to global frame using the transpose (inverse) of the orientation matrix
893  pthisSV->fAccGl[CHX] = pthisSV->fRPl[CHX][CHX] * pthisAccel->fGsAvg[CHX] + pthisSV->fRPl[CHY][CHX] * pthisAccel->fGsAvg[CHY] +
894  pthisSV->fRPl[CHZ][CHX] * pthisAccel->fGsAvg[CHZ];
895  pthisSV->fAccGl[CHY] = pthisSV->fRPl[CHX][CHY] * pthisAccel->fGsAvg[CHX] + pthisSV->fRPl[CHY][CHY] * pthisAccel->fGsAvg[CHY] +
896  pthisSV->fRPl[CHZ][CHY] * pthisAccel->fGsAvg[CHZ];
897  pthisSV->fAccGl[CHZ] = pthisSV->fRPl[CHX][CHZ] * pthisAccel->fGsAvg[CHX] + pthisSV->fRPl[CHY][CHZ] * pthisAccel->fGsAvg[CHY] +
898  pthisSV->fRPl[CHZ][CHZ] * pthisAccel->fGsAvg[CHZ];
899  // sutract the fixed gravity vector in the global frame leaving linear acceleration
900  switch (THISCOORDSYSTEM)
901  {
902  case NED:
903  // gravity positive NED
904  pthisSV->fAccGl[CHX] = -pthisSV->fAccGl[CHX];
905  pthisSV->fAccGl[CHY] = -pthisSV->fAccGl[CHY];
906  pthisSV->fAccGl[CHZ] = -(pthisSV->fAccGl[CHZ] - 1.0F);
907  break;
908  case ANDROID:
909  // acceleration positive ENU
910  pthisSV->fAccGl[CHZ] = pthisSV->fAccGl[CHZ] - 1.0F;
911  break;
912  case WIN8:
913  default:
914  // gravity positive ENU
915  pthisSV->fAccGl[CHX] = -pthisSV->fAccGl[CHX];
916  pthisSV->fAccGl[CHY] = -pthisSV->fAccGl[CHY];
917  pthisSV->fAccGl[CHZ] = -(pthisSV->fAccGl[CHZ] + 1.0F);
918  break;
919  }
920 
921  // compute the a posteriori Euler angles from the a posteriori orientation matrix fRPl
922  switch (THISCOORDSYSTEM)
923  {
924  case NED:
925  fNEDAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl), &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
926  break;
927  case ANDROID:
928  fAndroidAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl), &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
929  break;
930  case WIN8:
931  default:
932  fWin8AnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl), &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
933  break;
934  }
935 
936  return;
937 } // end fRun_6DOF_GY_KALMAN
938 
939 // 9DOF accelerometer+magnetometer+gyroscope orientation function implemented using indirect complementary Kalman filter
940 void fRun_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag,
941  struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal)
942 {
943  // local scalars and arrays
944  float ftmpA7x7[7][7]; // scratch 7x7 matrix
945  float fRMi[3][3]; // a priori orientation matrix
946  float fR6DOF[3][3]; // eCompass (6DOF accelerometer+magnetometer) orientation matrix
947  float ftmpMi3x1[3]; // temporary vector used for a priori calculations
948  float ftmp6DOF3x1[3]; // temporary vector used for 6DOF calculations
949  float ftmpA3x1[3]; // scratch vector
950  float fQvGQa; // accelerometer noise covariance to 1g sphere
951  float fQvBQd; // magnetometer noise covariance to geomagnetic sphere
952  float fC7x10ik; // element i, k of measurement matrix C
953  float fC7x10jk; // element j, k of measurement matrix C
954  struct fquaternion fqMi; // a priori orientation quaternion
955  struct fquaternion fq6DOF; // eCompass (6DOF accelerometer+magnetometer) orientation quaternion
956  struct fquaternion ftmpq; // scratch quaternion
957  float fDelta6DOF; // inclination angle from accelerometer and magnetometer
958  float ftmp; // scratch float
959  int8 ierror; // matrix inversion error flag
960  int8 i, j, k; // loop counters
961 
962  // working arrays for 7x7 matrix inversion
963  float *pfRows[7];
964  int8 iColInd[7];
965  int8 iRowInd[7];
966  int8 iPivot[7];
967 
968  // if requested, do a reset initialization with no further processing
969  if (pthisSV->resetflag)
970  {
971  fInit_9DOF_GBY_KALMAN(pthisSV, pthisAccel, pthisMag, pthisMagCal);
972  return;
973  }
974 
975  // compute the a priori orientation quaternion fqMi by integrating the buffered gyro measurements
976  fqMi = pthisSV->fqPl;
977  // loop over all the buffered gyroscope measurements
978  for (j = 0; j < OVERSAMPLE_RATIO; j++)
979  {
980  // calculate the instantaneous angular velocity (after subtracting the gyro offset) and rotation vector ftmpMi3x1
981  for (i = CHX; i <= CHZ; i++)
982  {
983  pthisSV->fOmega[i] = (float)pthisGyro->iYsBuffer[j][i] * pthisGyro->fDegPerSecPerCount - pthisSV->fbPl[i];
984  ftmpMi3x1[i] = pthisSV->fOmega[i] * pthisSV->fGyrodeltat;
985  }
986  // convert the rotation vector ftmpMi3x1 to a rotation quaternion ftmpq
987  fQuaternionFromRotationVectorDeg(&ftmpq, ftmpMi3x1, 1.0F);
988  // integrate the gyro sensor incremental quaternions into the a priori orientation quaternion fqMi
989  qAeqAxB(&fqMi, &ftmpq);
990  }
991  // compute the a priori orientation matrix from the a priori quaternion.
992  fRotationMatrixFromQuaternion(fRMi, &fqMi);
993 
994  // compute the 6DOF least squares orientation quaternion fq6DOF, matrix fR6DOF and inclination angle fDelta6DOF and
995  // the squared deviations of the accelerometer and magnetometer measurements from the 1g gravity and geomagnetic spheres.
996  switch (THISCOORDSYSTEM)
997  {
998  case NED:
999  fLeastSquareseCompassNED(&fq6DOF, pthisMagCal->fB, pthisSV->fDeltaPl, pthisSV->fsinDeltaPl, pthisSV->fcosDeltaPl,
1000  &fDelta6DOF, pthisMag->fBcAvg, pthisAccel->fGsAvg, &fQvBQd, &fQvGQa);
1001  break;
1002  case ANDROID:
1003  fLeastSquareseCompassAndroid(&fq6DOF, pthisMagCal->fB, pthisSV->fDeltaPl, pthisSV->fsinDeltaPl, pthisSV->fcosDeltaPl,
1004  &fDelta6DOF, pthisMag->fBcAvg, pthisAccel->fGsAvg, &fQvBQd, &fQvGQa);
1005  break;
1006  case WIN8:
1007  default:
1008  fLeastSquareseCompassWin8(&fq6DOF, pthisMagCal->fB, pthisSV->fDeltaPl, pthisSV->fsinDeltaPl, pthisSV->fcosDeltaPl,
1009  &fDelta6DOF, pthisMag->fBcAvg, pthisAccel->fGsAvg, &fQvBQd, &fQvGQa);
1010  break;
1011  }
1012  // compute the 6DOF orientation matrix from the 6DOF quaternion.
1013  fRotationMatrixFromQuaternion(fR6DOF, &fq6DOF);
1014 
1015  // do a once-only orientation lock immediately after the first valid magnetic calibration by setting the
1016  // a priori and a posteriori orientation to the 6DOF eCompass orientation.
1017  // also initialize the geomagnetic inclination angle Delta now that a magnetic calibration has been achieved and a
1018  // reasonable estimate is available for the first time.
1019  if (pthisMagCal->iValidMagCal && !pthisSV->iFirstAccelMagLock)
1020  {
1021  fqMi = pthisSV->fqPl = fq6DOF;
1022  f3x3matrixAeqB(fRMi, fR6DOF);
1023  pthisSV->fDeltaPl = fDelta6DOF;
1024  pthisSV->fsinDeltaPl = sinf(pthisSV->fDeltaPl * FPIOVER180);
1025  pthisSV->fcosDeltaPl = sqrtf(1.0F - pthisSV->fsinDeltaPl * pthisSV->fsinDeltaPl);
1026  pthisSV->iFirstAccelMagLock = true;
1027  }
1028 
1029  // set ftmp6DOF3x1 to the 6DOF gravity vector in the sensor frame (ignoring the minus sign for Android and Windows 8)
1030  ftmp6DOF3x1[CHX] = fR6DOF[CHX][CHZ];
1031  ftmp6DOF3x1[CHY] = fR6DOF[CHY][CHZ];
1032  ftmp6DOF3x1[CHZ] = fR6DOF[CHZ][CHZ];
1033  // set ftmpMi3x1 to the a priori gravity vector in the sensor frame (ignoring the minus sign for Android and Windows 8)
1034  ftmpMi3x1[CHX] = fRMi[CHX][CHZ];
1035  ftmpMi3x1[CHY] = fRMi[CHY][CHZ];
1036  ftmpMi3x1[CHZ] = fRMi[CHZ][CHZ];
1037  // set ftmpq to the quaternion that rotates the 6DOF gravity tilt vector to the a priori gravity tilt vector
1038  // and copy the vector components to the measurement error vector fZErr. the ignored minus signs cancel here.
1039  fveqconjgquq(&ftmpq, ftmp6DOF3x1, ftmpMi3x1);
1040  pthisSV->fZErr[0] = ftmpq.q1;
1041  pthisSV->fZErr[1] = ftmpq.q2;
1042  pthisSV->fZErr[2] = ftmpq.q3;
1043 
1044  // determine the normalized 6DOF and a priori geomagnetic vectors in the sensor frame.
1045  switch (THISCOORDSYSTEM)
1046  {
1047  case NED:
1048  // set ftmp6DOF3x1 to the normalized 6DOF geomagnetic vector in the sensor frame
1049  ftmp6DOF3x1[CHX] = fR6DOF[CHX][CHX] * pthisSV->fcosDeltaPl + fR6DOF[CHX][CHZ] * pthisSV->fsinDeltaPl;
1050  ftmp6DOF3x1[CHY] = fR6DOF[CHY][CHX] * pthisSV->fcosDeltaPl + fR6DOF[CHY][CHZ] * pthisSV->fsinDeltaPl;
1051  ftmp6DOF3x1[CHZ] = fR6DOF[CHZ][CHX] * pthisSV->fcosDeltaPl + fR6DOF[CHZ][CHZ] * pthisSV->fsinDeltaPl;
1052  // set ftmpMi3x1 to the normalized a priori geomagnetic vector in the sensor frame
1053  ftmpMi3x1[CHX] = fRMi[CHX][CHX] * pthisSV->fcosDeltaPl + fRMi[CHX][CHZ] * pthisSV->fsinDeltaPl;
1054  ftmpMi3x1[CHY] = fRMi[CHY][CHX] * pthisSV->fcosDeltaPl + fRMi[CHY][CHZ] * pthisSV->fsinDeltaPl;
1055  ftmpMi3x1[CHZ] = fRMi[CHZ][CHX] * pthisSV->fcosDeltaPl + fRMi[CHZ][CHZ] * pthisSV->fsinDeltaPl;
1056  break;
1057  case ANDROID:
1058  case WIN8:
1059  default:
1060  // set ftmp6DOF3x1 to the 6DOF geomagnetic vector in the sensor frame
1061  ftmp6DOF3x1[CHX] = fR6DOF[CHX][CHY] * pthisSV->fcosDeltaPl - fR6DOF[CHX][CHZ] * pthisSV->fsinDeltaPl;
1062  ftmp6DOF3x1[CHY] = fR6DOF[CHY][CHY] * pthisSV->fcosDeltaPl - fR6DOF[CHY][CHZ] * pthisSV->fsinDeltaPl;
1063  ftmp6DOF3x1[CHZ] = fR6DOF[CHZ][CHY] * pthisSV->fcosDeltaPl - fR6DOF[CHZ][CHZ] * pthisSV->fsinDeltaPl;
1064  // set ftmpMi3x1 to the a priori geomagnetic vector in the sensor frame
1065  ftmpMi3x1[CHX] = fRMi[CHX][CHY] * pthisSV->fcosDeltaPl - fRMi[CHX][CHZ] * pthisSV->fsinDeltaPl;
1066  ftmpMi3x1[CHY] = fRMi[CHY][CHY] * pthisSV->fcosDeltaPl - fRMi[CHY][CHZ] * pthisSV->fsinDeltaPl;
1067  ftmpMi3x1[CHZ] = fRMi[CHZ][CHY] * pthisSV->fcosDeltaPl - fRMi[CHZ][CHZ] * pthisSV->fsinDeltaPl;
1068  break;
1069  }
1070  // set ftmpq to the quaternion that rotates the 6DOF geomagnetic tilt vector to the a priori geomagnetic tilt vector
1071  // limit to 60 deg rotation and copy the vector components to the measurement error vector fZErr
1072  fveqconjgquq(&ftmpq, ftmp6DOF3x1, ftmpMi3x1);
1073  pthisSV->fZErr[3] = ftmpq.q1;
1074  pthisSV->fZErr[4] = ftmpq.q2;
1075  pthisSV->fZErr[5] = ftmpq.q3;
1076 
1077  // calculate the angle error between a priori (equal to a posteriori) and 6DOF estimates of the geomagnetic inclination
1078  // and copy into the last element of the measurement error vector fZErr
1079  pthisSV->fZErr[6] = pthisSV->fDeltaPl - fDelta6DOF;
1080 
1081  // update Qw using the a posteriori error vectors from the previous iteration.
1082  // as Qv increases or Qw decreases, K -> 0 and the Kalman filter is weighted towards the a priori prediction
1083  // as Qv decreases or Qw increases, KC -> I and the Kalman filter is weighted towards the measurement.
1084  // initialize on and above diagonal elements of Qw to zero
1085  for (i = 0; i < 10; i++)
1086  for (j = i; j < 10; j++)
1087  pthisSV->fQw10x10[i][j] = 0.0F;
1088  // partial diagonal gyro offset terms
1089  pthisSV->fQw10x10[6][6] = pthisSV->fbErrPl[CHX] * pthisSV->fbErrPl[CHX];
1090  pthisSV->fQw10x10[7][7] = pthisSV->fbErrPl[CHY] * pthisSV->fbErrPl[CHY];
1091  pthisSV->fQw10x10[8][8] = pthisSV->fbErrPl[CHZ] * pthisSV->fbErrPl[CHZ];
1092  // set ftmpA3x1 to (alpha/2)^2 * fbErrPl.fbErrPl
1093  ftmpA3x1[0] = pthisSV->fAlphaOver2Sq * pthisSV->fQw10x10[6][6];
1094  ftmpA3x1[1] = pthisSV->fAlphaOver2Sq * pthisSV->fQw10x10[7][7];
1095  ftmpA3x1[2] = pthisSV->fAlphaOver2Sq * pthisSV->fQw10x10[8][8];
1096  // diagonal gravity vector components
1097  pthisSV->fQw10x10[0][0] = pthisSV->fqgErrPl[CHX] * pthisSV->fqgErrPl[CHX] + pthisSV->fAlphaOver2SqQvYQwb + ftmpA3x1[0];
1098  pthisSV->fQw10x10[1][1] = pthisSV->fqgErrPl[CHY] * pthisSV->fqgErrPl[CHY] + pthisSV->fAlphaOver2SqQvYQwb + ftmpA3x1[1];
1099  pthisSV->fQw10x10[2][2] = pthisSV->fqgErrPl[CHZ] * pthisSV->fqgErrPl[CHZ] + pthisSV->fAlphaOver2SqQvYQwb + ftmpA3x1[2];
1100  // diagonal geomagnetic vector components
1101  pthisSV->fQw10x10[3][3] = pthisSV->fqmErrPl[CHX] * pthisSV->fqmErrPl[CHX] + pthisSV->fAlphaOver2SqQvYQwb + ftmpA3x1[0];
1102  pthisSV->fQw10x10[4][4] = pthisSV->fqmErrPl[CHY] * pthisSV->fqmErrPl[CHY] + pthisSV->fAlphaOver2SqQvYQwb + ftmpA3x1[1];
1103  pthisSV->fQw10x10[5][5] = pthisSV->fqmErrPl[CHZ] * pthisSV->fqmErrPl[CHZ] + pthisSV->fAlphaOver2SqQvYQwb + ftmpA3x1[2];
1104  // diagonal gyro offset components
1105  pthisSV->fQw10x10[6][6] += FQWB_9DOF_GBY_KALMAN;
1106  pthisSV->fQw10x10[7][7] += FQWB_9DOF_GBY_KALMAN;
1107  pthisSV->fQw10x10[8][8] += FQWB_9DOF_GBY_KALMAN;
1108  // diagonal geomagnetic inclination angle
1109  pthisSV->fQw10x10[9][9] = FQWDLT_9DOF_GBY_KALMAN + pthisSV->fDeltaErrPl * pthisSV->fDeltaErrPl;
1110  // off diagonal gravity and gyro offset components
1111  pthisSV->fQw10x10[0][6] = pthisSV->fqgErrPl[CHX] * pthisSV->fbErrPl[CHX] - pthisSV->fAlphaOver2Qwb;
1112  pthisSV->fQw10x10[1][7] = pthisSV->fqgErrPl[CHY] * pthisSV->fbErrPl[CHY] - pthisSV->fAlphaOver2Qwb;
1113  pthisSV->fQw10x10[2][8] = pthisSV->fqgErrPl[CHZ] * pthisSV->fbErrPl[CHZ] - pthisSV->fAlphaOver2Qwb;
1114  // off diagonal geomagnetic and gyro offset components
1115  pthisSV->fQw10x10[3][6] = pthisSV->fqmErrPl[CHX] * pthisSV->fbErrPl[CHX] - pthisSV->fAlphaOver2Qwb;
1116  pthisSV->fQw10x10[4][7] = pthisSV->fqmErrPl[CHY] * pthisSV->fbErrPl[CHY] - pthisSV->fAlphaOver2Qwb;
1117  pthisSV->fQw10x10[5][8] = pthisSV->fqmErrPl[CHZ] * pthisSV->fbErrPl[CHZ] - pthisSV->fAlphaOver2Qwb;
1118  // set below diagonal elements of Qw to above diagonal elements
1119  for (i = 1; i < 10; i++)
1120  for (j = 0; j < i; j++)
1121  pthisSV->fQw10x10[i][j] = pthisSV->fQw10x10[j][i];
1122 
1123  // calculate the vector fQv7x1 containing the diagonal elements of the measurement covariance matrix Qv
1125  ftmp = fQvBQd / (pthisMagCal->fB * pthisMagCal->fB);
1126  pthisSV->fQv7x1[0] = pthisSV->fQv7x1[1] = pthisSV->fQv7x1[2] = 0.25F * fQvGQa + pthisSV->fAlphaOver2SqQvYQwb;
1127  pthisSV->fQv7x1[3] = pthisSV->fQv7x1[4] = pthisSV->fQv7x1[5] = 0.25F * ftmp + pthisSV->fAlphaOver2SqQvYQwb;
1128  pthisSV->fQv7x1[6] = (fQvGQa + ftmp) * F180OVERPISQ;
1129 
1130  // calculate the Kalman gain matrix K = Qw * C^T * inv(C * Qw * C^T + Qv)
1131  // set fQwCT10x7 = Qw.C^T where Qw has size 10x10 and C^T has size 10x7
1132  for (i = 0; i < 10; i++) // loop over rows
1133  {
1134  for (j = 0; j < 7; j++) // loop over columns
1135  {
1136  pthisSV->fQwCT10x7[i][j] = 0.0F;
1137  // accumulate matrix sum
1138  for (k = 0; k < 10; k++)
1139  {
1140  // determine fC7x10[j][k] since the matrix is highly sparse
1141  fC7x10jk = 0.0F;
1142  // handle rows 0 to 2
1143  if (j < 3)
1144  {
1145  if (k == j) fC7x10jk = 1.0F;
1146  if (k == (j + 6)) fC7x10jk = -pthisSV->fAlphaOver2;
1147  }
1148  // handle rows 3 to 5
1149  else if (j < 6)
1150  {
1151  if (k == j) fC7x10jk = 1.0F;
1152  if (k == (j + 3)) fC7x10jk = -pthisSV->fAlphaOver2;
1153  }
1154  // handle last row 6
1155  else if (k == 9) fC7x10jk = 1.0F;
1156 
1157  // accumulate fQwCT10x7[i][j] += Qw10x10[i][k] * C[j][k]
1158  if ((pthisSV->fQw10x10[i][k] != 0.0F) && (fC7x10jk != 0.0F))
1159  {
1160  if (fC7x10jk == 1.0F)
1161  pthisSV->fQwCT10x7[i][j] += pthisSV->fQw10x10[i][k];
1162  else
1163  pthisSV->fQwCT10x7[i][j] += pthisSV->fQw10x10[i][k] * fC7x10jk;
1164  }
1165  }
1166  }
1167  }
1168 
1169  // set symmetric ftmpA7x7 = C.(Qw.C^T) + Qv = C.fQwCT10x7 + Qv
1170  for (i = 0; i < 7; i++) // loop over rows
1171  {
1172  for (j = i; j < 7; j++) // loop over on and above diagonal columns
1173  {
1174  // zero off diagonal and set diagonal to Qv
1175  if (i == j)
1176  ftmpA7x7[i][j] = pthisSV->fQv7x1[i];
1177  else
1178  ftmpA7x7[i][j] = 0.0F;
1179  // accumulate matrix sum
1180  for (k = 0; k < 10; k++)
1181  {
1182  // determine fC7x10[i][k]
1183  fC7x10ik = 0.0F;
1184  // handle rows 0 to 2
1185  if (i < 3)
1186  {
1187  if (k == i) fC7x10ik = 1.0F;
1188  if (k == (i + 6)) fC7x10ik = -pthisSV->fAlphaOver2;
1189  }
1190  // handle rows 3 to 5
1191  else if (i < 6)
1192  {
1193  if (k == i) fC7x10ik = 1.0F;
1194  if (k == (i + 3)) fC7x10ik = -pthisSV->fAlphaOver2;
1195  }
1196  // handle last row 6
1197  else if (k == 9) fC7x10ik = 1.0F;
1198 
1199  // accumulate ftmpA7x7[i][j] += C[i][k] & fQwCT10x7[k][j]
1200  if ((fC7x10ik != 0.0F) && (pthisSV->fQwCT10x7[k][j] != 0.0F))
1201  {
1202  if (fC7x10ik == 1.0F)
1203  ftmpA7x7[i][j] += pthisSV->fQwCT10x7[k][j];
1204  else
1205  ftmpA7x7[i][j] += fC7x10ik * pthisSV->fQwCT10x7[k][j];
1206  }
1207  }
1208  }
1209  }
1210  // set ftmpA7x7 below diagonal elements to above diagonal elements
1211  for (i = 1; i < 7; i++) // loop over rows
1212  {
1213  for (j = 0; j < i; j++) // loop over below diagonal columns
1214  {
1215  ftmpA7x7[i][j] = ftmpA7x7[j][i];
1216  }
1217  }
1218 
1219  // invert ftmpA7x7 in situ to give ftmpA7x7 = inv(C * Qw * C^T + Qv) = inv(ftmpA7x7)
1220  for (i = 0; i < 7; i++)
1221  pfRows[i] = ftmpA7x7[i];
1222  fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 7, &ierror);
1223 
1224  // on successful inversion set Kalman gain matrix K10x7 = Qw * C^T * inv(C * Qw * C^T + Qv) = fQwCT10x7 * ftmpA7x7
1225  if (!ierror)
1226  {
1227  // normal case
1228  for (i = 0; i < 10; i++) // loop over rows
1229  {
1230  for (j = 0; j < 7; j++) // loop over columns
1231  {
1232  pthisSV->fK10x7[i][j] = 0.0F;
1233  for (k = 0; k < 7; k++)
1234  {
1235  if ((pthisSV->fQwCT10x7[i][k] != 0.0F) && (ftmpA7x7[k][j] != 0.0F))
1236  {
1237  pthisSV->fK10x7[i][j] += pthisSV->fQwCT10x7[i][k] * ftmpA7x7[k][j];
1238  }
1239  }
1240  }
1241  }
1242  }
1243  else
1244  {
1245  // ftmpA7x7 was singular so set Kalman gain matrix to zero
1246  for (i = 0; i < 10; i++) // loop over rows
1247  {
1248  for (j = 0; j < 7; j++) // loop over columns
1249  {
1250  pthisSV->fK10x7[i][j] = 0.0F;
1251  }
1252  }
1253  }
1254 
1255  // calculate the a posteriori gravity and geomagnetic tilt quaternion errors and gyro offset error vector
1256  // from the Kalman matrix fK10x7 and from the measurement error vector fZErr.
1257  for (i = CHX; i <= CHZ; i++)
1258  {
1259  pthisSV->fqgErrPl[i] = pthisSV->fqmErrPl[i] = pthisSV->fbErrPl[i] = 0.0F;
1260  for (j = 0; j < 7; j++)
1261  {
1262  // gravity tilt quaternion vector error component fqgErrPl
1263  if (pthisSV->fK10x7[i][j] != 0.0F)
1264  pthisSV->fqgErrPl[i] += pthisSV->fK10x7[i][j] * pthisSV->fZErr[j];
1265 
1266  // geomagnetic tilt quaternion vector error component fqmErrPl
1267  if (pthisSV->fK10x7[i + 3][j] != 0.0F)
1268  pthisSV->fqmErrPl[i] += pthisSV->fK10x7[i + 3][j] * pthisSV->fZErr[j];
1269 
1270  // gyro offset vector error component fbErrPl
1271  if (pthisSV->fK10x7[i + 6][j] != 0.0F)
1272  pthisSV->fbErrPl[i] += pthisSV->fK10x7[i + 6][j] * pthisSV->fZErr[j];
1273  }
1274  }
1275 
1276  // calculate the a posteriori geomagnetic inclination angle error estimate fDeltaErrPl
1277  pthisSV->fDeltaErrPl = 0.0F;
1278  for (j = 0; j < 7; j++)
1279  {
1280  if (pthisSV->fK10x7[9][j] != 0.0F)
1281  pthisSV->fDeltaErrPl += pthisSV->fK10x7[9][j] * pthisSV->fZErr[j];
1282  }
1283 
1284  // limit a posteriori gravity tilt correction (conjugate) quaternion to 90 deg correction and apply
1285  ftmp = pthisSV->fqgErrPl[CHX] * pthisSV->fqgErrPl[CHX] + pthisSV->fqgErrPl[CHY] * pthisSV->fqgErrPl[CHY] +
1286  pthisSV->fqgErrPl[CHZ] * pthisSV->fqgErrPl[CHZ];
1287  if (ftmp <= 0.5F)
1288  {
1289  // normal case with less than 90 deg correction
1290  ftmpq.q0 = sqrtf(fabsf(1.0F - ftmp));
1291  ftmpq.q1 = -pthisSV->fqgErrPl[CHX];
1292  ftmpq.q2 = -pthisSV->fqgErrPl[CHY];
1293  ftmpq.q3 = -pthisSV->fqgErrPl[CHZ];
1294  }
1295  else
1296  {
1297  // limit to 90 deg rotation with q0=1/sqrt(2) and quaternion vector magnitude=1/sqrt(2)
1298  ftmpq.q0 = ONEOVERSQRT2;
1299  ftmp = ONEOVERSQRT2 / sqrtf(fabsf(ftmp));
1300  pthisSV->fqgErrPl[CHX] *= ftmp;
1301  pthisSV->fqgErrPl[CHY] *= ftmp;
1302  pthisSV->fqgErrPl[CHZ] *= ftmp;
1303  ftmpq.q1 = -pthisSV->fqgErrPl[CHX];
1304  ftmpq.q2 = -pthisSV->fqgErrPl[CHY];
1305  ftmpq.q3 = -pthisSV->fqgErrPl[CHZ];
1306  }
1307  // apply the gravity tilt correction quaternion so fqPl = fqMi.(fqgErrPl)*
1308  qAeqBxC(&(pthisSV->fqPl), &fqMi, &ftmpq);
1309 
1310  // limit a posteriori geomagnetic tilt correction (conjugate) quaternion to 90 deg correction and apply
1311  ftmp = pthisSV->fqmErrPl[CHX] * pthisSV->fqmErrPl[CHX] + pthisSV->fqmErrPl[CHY] * pthisSV->fqmErrPl[CHY] +
1312  pthisSV->fqmErrPl[CHZ] * pthisSV->fqmErrPl[CHZ];
1313  if (ftmp <= 0.5F)
1314  {
1315  // normal case with less than 90 deg correction
1316  ftmpq.q0 = sqrtf(fabsf(1.0F - ftmp));
1317  ftmpq.q1 = -pthisSV->fqmErrPl[CHX];
1318  ftmpq.q2 = -pthisSV->fqmErrPl[CHY];
1319  ftmpq.q3 = -pthisSV->fqmErrPl[CHZ];
1320  }
1321  else
1322  {
1323  // limit to 90 deg rotation with q0=1/sqrt(2) and quaternion vector magnitude=1/sqrt(2)
1324  ftmpq.q0 = ONEOVERSQRT2;
1325  ftmp = ONEOVERSQRT2 / sqrtf(fabsf(ftmp));
1326  pthisSV->fqmErrPl[CHX] *= ftmp;
1327  pthisSV->fqmErrPl[CHY] *= ftmp;
1328  pthisSV->fqmErrPl[CHZ] *= ftmp;
1329  ftmpq.q1 = -pthisSV->fqmErrPl[CHX];
1330  ftmpq.q2 = -pthisSV->fqmErrPl[CHY];
1331  ftmpq.q3 = -pthisSV->fqmErrPl[CHZ];
1332  }
1333  // apply the geomagnetic tilt correction quaternion so fqPl = fqMi.(fqgErrPl)*.(fqmErrPl)*
1334  qAeqAxB(&(pthisSV->fqPl), &ftmpq);
1335 
1336  // normalize the a posteriori quaternion and compute the a posteriori rotation matrix and rotation vector
1337  fqAeqNormqA(&(pthisSV->fqPl));
1338  fRotationMatrixFromQuaternion(pthisSV->fRPl, &(pthisSV->fqPl));
1339  fRotationVectorDegFromQuaternion(&(pthisSV->fqPl), pthisSV->fRVecPl);
1340 
1341  // update the a posteriori gyro offset vector: b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s)
1342  // and limit gyro offset vector range as defensive programming.
1343  for (i = CHX; i <= CHZ; i++)
1344  {
1345  pthisSV->fbPl[i] -= pthisSV->fbErrPl[i];
1348  }
1349 
1350  // update a posteriori geomagnetic inclination angle: delta+[k] = delta-[k] - deltae+[k] = delta+[k] - deltae+[k]
1351  pthisSV->fDeltaPl -= pthisSV->fDeltaErrPl;
1352  if (pthisSV->fDeltaPl > 90.0F) pthisSV->fDeltaPl = 90.0F;
1353  else if (pthisSV->fDeltaPl < -90.0F) pthisSV->fDeltaPl = -90.0F;
1354  pthisSV->fsinDeltaPl = sinf(pthisSV->fDeltaPl * FPIOVER180);
1355  pthisSV->fcosDeltaPl = sqrtf(1.0F - pthisSV->fsinDeltaPl * pthisSV->fsinDeltaPl);
1356 
1357  // compute the linear acceleration in the global frame
1358  // de-rotate the accelerometer from the sensor to global frame using the transpose (inverse) of the orientation matrix
1359  pthisSV->fAccGl[CHX] = pthisSV->fRPl[CHX][CHX] * pthisAccel->fGsAvg[CHX] + pthisSV->fRPl[CHY][CHX] * pthisAccel->fGsAvg[CHY] +
1360  pthisSV->fRPl[CHZ][CHX] * pthisAccel->fGsAvg[CHZ];
1361  pthisSV->fAccGl[CHY] = pthisSV->fRPl[CHX][CHY] * pthisAccel->fGsAvg[CHX] + pthisSV->fRPl[CHY][CHY] * pthisAccel->fGsAvg[CHY] +
1362  pthisSV->fRPl[CHZ][CHY] * pthisAccel->fGsAvg[CHZ];
1363  pthisSV->fAccGl[CHZ] = pthisSV->fRPl[CHX][CHZ] * pthisAccel->fGsAvg[CHX] + pthisSV->fRPl[CHY][CHZ] * pthisAccel->fGsAvg[CHY] +
1364  pthisSV->fRPl[CHZ][CHZ] * pthisAccel->fGsAvg[CHZ];
1365  // sutract the fixed gravity vector in the global frame leaving linear acceleration
1366  switch (THISCOORDSYSTEM)
1367  {
1368  case NED:
1369  // gravity positive NED
1370  pthisSV->fAccGl[CHX] = -pthisSV->fAccGl[CHX];
1371  pthisSV->fAccGl[CHY] = -pthisSV->fAccGl[CHY];
1372  pthisSV->fAccGl[CHZ] = -(pthisSV->fAccGl[CHZ] - 1.0F);
1373  break;
1374  case ANDROID:
1375  // acceleration positive ENU
1376  pthisSV->fAccGl[CHZ] = pthisSV->fAccGl[CHZ] - 1.0F;
1377  break;
1378  case WIN8:
1379  default:
1380  // gravity positive ENU
1381  pthisSV->fAccGl[CHX] = -pthisSV->fAccGl[CHX];
1382  pthisSV->fAccGl[CHY] = -pthisSV->fAccGl[CHY];
1383  pthisSV->fAccGl[CHZ] = -(pthisSV->fAccGl[CHZ] + 1.0F);
1384  break;
1385  }
1386 
1387  // integrate the acceleration to velocity and displacement in the global frame.
1388  // Note: integration errors accumulate without limit over time and this code should only be
1389  // used for inertial integration of the order of seconds.
1390  for (i = CHX; i <= CHZ; i++)
1391  {
1392  // integrate acceleration (in g) to velocity in m/s
1393  pthisSV->fVelGl[i] += pthisSV->fAccGl[i] * pthisSV->fgKalmandeltat;
1394  // integrate velocity (in m/s) to displacement (m)
1395  pthisSV->fDisGl[i] += pthisSV->fVelGl[i] * pthisSV->fKalmandeltat;
1396  }
1397 
1398  // compute the a posteriori Euler angles from the a posteriori orientation matrix fRPl
1399  switch (THISCOORDSYSTEM)
1400  {
1401  case NED:
1402  fNEDAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl), &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
1403  break;
1404  case ANDROID:
1405  fAndroidAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl), &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
1406  break;
1407  case WIN8:
1408  default:
1409  fWin8AnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl), &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
1410  break;
1411  }
1412 
1413  return;
1414 } // end fRun_9DOF_GBY_KALMAN
float fLPRVec[3]
Definition: fusion_types.h:117
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure)
Definition: fusion.c:361
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
Definition: matrix.c:496
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
Definition: orientation.c:193
void fLeastSquareseCompassWin8(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
Definition: orientation.c:688
struct fquaternion fLPq
Definition: fusion_types.h:90
#define WIN8
Definition: fusion_types.h:33
float fQw6x6[6][6]
Definition: fusion_types.h:199
float fR[3][3]
Definition: fusion_types.h:173
void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
Definition: fusion.c:551
void f3x3matrixAeqB(float A[][3], float B[][3])
Definition: matrix.c:55
float fK6x3[6][3]
Definition: fusion_types.h:200
#define FQWB_9DOF_GBY_KALMAN
Definition: fusion.h:49
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:853
float fDegPerSecPerCount
#define SENSORFS
Definition: fusion_config.h:22
void fveqconjgquq(struct fquaternion *pfq, float fu[], float fv[])
Definition: orientation.c:1341
struct SV_3DOF_B_BASIC thisSV_3DOF_B_BASIC
#define FGYRO_OFFSET_MIN_9DOF_GBY_KALMAN
Definition: fusion.h:55
#define FQVY_6DOF_GY_KALMAN
Definition: fusion.h:38
float fRPl[3][3]
Definition: fusion_types.h:193
float fRPl[3][3]
Definition: fusion_types.h:228
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag)
Definition: fusion.c:437
struct fquaternion fqPl
Definition: fusion_types.h:229
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:799
Definition: matrix.h:35
float fLPRVec[3]
Definition: fusion_types.h:91
struct fquaternion fq
Definition: fusion_types.h:124
void fqAeq1(struct fquaternion *pqA)
Definition: orientation.c:1331
#define FQWDLT_9DOF_GBY_KALMAN
Definition: fusion.h:51
struct SV_6DOF_GY_KALMAN thisSV_6DOF_GY_KALMAN
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGs[])
Definition: orientation.c:258
struct fquaternion fq
Definition: fusion_types.h:98
struct fquaternion fLPq
Definition: fusion_types.h:116
void f3DOFTiltNED(float fR[][3], float fGs[])
Definition: orientation.c:44
float fQwCT6x3[6][3]
Definition: fusion_types.h:201
#define FGYRO_OFFSET_MAX_9DOF_GBY_KALMAN
Definition: fusion.h:56
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
Definition: orientation.c:249
#define ANDROID
Definition: fusion_types.h:32
int8 iValidMagCal
Definition: magnetic.h:77
Definition: matrix.h:34
float fQwCT10x7[10][7]
Definition: fusion_types.h:238
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
Definition: orientation.c:1148
int16 iYsBuffer[GYRO_OVERSAMPLE_RATIO][3]
float fK10x7[10][7]
Definition: fusion_types.h:237
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[])
Definition: orientation.c:1198
void fInit_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, float flpftimesecs)
Definition: fusion.c:143
float fLPR[3][3]
Definition: fusion_types.h:115
void fInit_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel)
Definition: fusion.c:238
struct fquaternion fqPl
Definition: fusion_types.h:194
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro)
Definition: fusion.c:489
void fLeastSquareseCompassAndroid(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
Definition: orientation.c:576
float fR[3][3]
Definition: fusion_types.h:141
float fR[3][3]
Definition: fusion_types.h:97
float fLPR[3][3]
Definition: fusion_types.h:165
void fInit_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, float flpftimesecs)
Definition: fusion.c:200
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
Definition: orientation.c:1255
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGs[])
Definition: orientation.c:394
void f3DOFTiltAndroid(float fR[][3], float fGs[])
Definition: orientation.c:110
void fInit_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, float flpftimesecs)
Definition: fusion.c:84
float fR[3][3]
Definition: fusion_types.h:123
#define GTOMSEC2
Definition: fusion_types.h:46
struct ProjectGlobals globals
struct SV_3DOF_Y_BASIC thisSV_3DOF_Y_BASIC
#define FQVBQD_MIN_9DOF_GBY_KALMAN
Definition: fusion.h:53
void fqAeqNormqA(struct fquaternion *pqA)
Definition: orientation.c:1296
#define FQVY_9DOF_GBY_KALMAN
Definition: fusion.h:47
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
#define FQWB_6DOF_GY_KALMAN
Definition: fusion.h:40
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
Definition: orientation.c:999
void fInit_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV)
Definition: fusion.c:180
#define THISCOORDSYSTEM
Definition: fusion_config.h:32
struct fquaternion fq
Definition: fusion_types.h:174
float fLPR[3][3]
Definition: fusion_types.h:89
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
Definition: orientation.c:1266
struct fquaternion fq
Definition: fusion_types.h:142
struct SV_1DOF_P_BASIC thisSV_1DOF_P_BASIC
void fRun_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal)
Definition: fusion.c:940
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel)
Definition: fusion.c:380
void fInitFusion(void)
void f3DOFTiltWin8(float fR[][3], float fGs[])
Definition: orientation.c:119
float fOmega[3]
Definition: fusion_types.h:93
struct SV_3DOF_G_BASIC thisSV_3DOF_G_BASIC
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
void fInit_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
Definition: fusion.c:284
void fInit_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, float flpftimesecs)
Definition: fusion.c:106
#define NED
Definition: fusion_types.h:31
struct SV_6DOF_GB_BASIC thisSV_6DOF_GB_BASIC
struct SV_9DOF_GBY_KALMAN thisSV_9DOF_GBY_KALMAN
#define F180OVERPISQ
Definition: fusion_types.h:39
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGs[])
Definition: orientation.c:326
void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
Definition: fusion.c:606
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
Definition: orientation.c:1108
#define FPIOVER180
Definition: fusion_types.h:37
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
Definition: orientation.c:1067
void fLeastSquareseCompassNED(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
Definition: orientation.c:463
#define OVERSAMPLE_RATIO
Definition: fusion_config.h:21
#define ONEOVERSQRT2
Definition: fusion_types.h:45
float fBcAvg[3]
struct fquaternion fLPq
Definition: fusion_types.h:166
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
Definition: orientation.c:221
float fQw10x10[10][10]
Definition: fusion_types.h:236
signed char int8
Definition: isf_types.h:72
Definition: matrix.h:33
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:908