ISF  2.1
Intelligent Sensing Framework for Kinetis with Processor Expert
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
fusion.c
Go to the documentation of this file.
1 // Copyright (c) 2014, 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 
41 // *********************************************************************************
42 // COMPUTE_6DOF_GY_KALMAN constants
43 // *********************************************************************************
44 // kalman filter noise variances
45 #define FQVA_6DOF_GY_KALMAN 2E-6F // accelerometer noise g^2 so 1.4mg RMS
46 #define FQVG_6DOF_GY_KALMAN 0.3F // gyro noise (deg/s)^2
47 #define FQWB_6DOF_GY_KALMAN 1E-9F // gyro offset drift (deg/s)^2: 1E-9 implies 0.09deg/s max at 50Hz
48 #define FQWA_6DOF_GY_KALMAN 1E-4F // linear acceleration drift g^2 (increase slows convergence to g but reduces sensitivity to shake)
49 // initialization of Qw covariance matrix
50 #define FQWINITTHTH_6DOF_GY_KALMAN 2000E-5F // th_e * th_e terms
51 #define FQWINITBB_6DOF_GY_KALMAN 250E-3F // for FXAS21000: b_e * b_e terms
52 #define FQWINITTHB_6DOF_GY_KALMAN 0.0F // th_e * b_e terms
53 #define FQWINITAA_6DOF_GY_KALMAN 10E-5F // a_e * a_e terms (increase slows convergence to g but reduces sensitivity to shake)
54 // linear acceleration time constant
55 #define FCA_6DOF_GY_KALMAN 0.5F // linear acceleration decay factor
56 
57 // *********************************************************************************
58 // COMPUTE_9DOF_GBY_KALMAN constants
59 // *********************************************************************************
60 // kalman filter noise variances
61 #define FQVA_9DOF_GBY_KALMAN 2E-6F // accelerometer noise g^2 so 1.4mg RMS
62 #define FQVM_9DOF_GBY_KALMAN 0.1F // magnetometer noise uT^2
63 #define FQVG_9DOF_GBY_KALMAN 0.3F // gyro noise (deg/s)^2
64 #define FQWB_9DOF_GBY_KALMAN 1E-9F // gyro offset drift (deg/s)^2: 1E-9 implies 0.09deg/s max at 50Hz
65 #define FQWA_9DOF_GBY_KALMAN 1E-4F // linear acceleration drift g^2 (increase slows convergence to g but reduces sensitivity to shake)
66 #define FQWD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance drift uT^2 (increase slows convergence to B but reduces sensitivity to magnet)
67 // initialization of Qw covariance matrix
68 #define FQWINITTHTH_9DOF_GBY_KALMAN 2000E-5F // th_e * th_e terms
69 #define FQWINITBB_9DOF_GBY_KALMAN 250E-3F // b_e * b_e terms
70 #define FQWINITTHB_9DOF_GBY_KALMAN 0.0F // th_e * b_e terms
71 #define FQWINITAA_9DOF_GBY_KALMAN 10E-5F // a_e * a_e terms (increase slows convergence to g but reduces sensitivity to shake)
72 #define FQWINITDD_9DOF_GBY_KALMAN 600E-3F // d_e * d_e terms (increase slows convergence to B but reduces sensitivity to magnet)
73 // linear acceleration and magnetic disturbance time constants
74 #define FCA_9DOF_GBY_KALMAN 0.5F // linear acceleration decay factor
75 #define FCD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance decay factor
76 // maximum geomagnetic inclination angle tracked by Kalman filter
77 #define SINDELTAMAX 0.9063078F // sin of max +ve geomagnetic inclination angle: here 65.0 deg
78 #define COSDELTAMAX 0.4226183F // cos of max +ve geomagnetic inclination angle: here 65.0 deg
79 
80 
81 void fInit_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
82 {
83  // set algorithm sampling interval (typically 25Hz) and low pass filter
84  pthisSV->fdeltat = (float) iOverSampleRatio / (float) iSensorFS;
85  pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
86  if (pthisSV->flpf > 1.0F)
87  {
88  pthisSV->flpf = 1.0F;
89  }
90 
91  // clear the reset flag
92  pthisSV->resetflag = false;
93 
94  return;
95 } // end fInit_1DOF_P_BASIC
96 
97 void fInit_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
98 {
99  // set algorithm sampling interval (typically 25Hz)
100  pthisSV->fdeltat = (float) iOverSampleRatio / (float) iSensorFS;
101 
102  // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
103  if (flpftimesecs > pthisSV->fdeltat)
104  pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
105  else
106  pthisSV->flpf = 1.0F;
107 
108  // clear the reset flag
109  pthisSV->resetflag = false;
110 
111  return;
112 } // end fInit_3DOF_G_BASIC
113 
114 void fInit_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
115 {
116  // set algorithm sampling interval (typically 25Hz)
117  pthisSV->fdeltat = (float) iOverSampleRatio / (float) iSensorFS;
118 
119  // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
120  if (flpftimesecs > pthisSV->fdeltat)
121  pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
122  else
123  pthisSV->flpf = 1.0F;
124 
125  // clear the reset flag
126  pthisSV->resetflag = false;
127 
128  return;
129 } // end fInit_3DOF_B_BASIC
130 
131 void fInit_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
132 {
133  // compute the sampling time intervals (secs)
134  pthisSV->fFastdeltat = 1.0F / (float) iSensorFS;
135  pthisSV->fdeltat = (float) iOverSampleRatio * pthisSV->fFastdeltat;
136 
137  // initialize orientation estimates
138  f3x3matrixAeqI(pthisSV->fR);
139  fqAeq1(&(pthisSV->fq));
140 
141 
142  // clear the reset flag
143  pthisSV->resetflag = false;
144 
145  return;
146 } // end fInit_3DOF_Y_BASIC
147 
148 void fInit_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
149 {
150  // set algorithm sampling interval (typically 25Hz)
151  pthisSV->fdeltat = (float) iOverSampleRatio / (float) iSensorFS;
152 
153  // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
154  if (flpftimesecs > pthisSV->fdeltat)
155  pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
156  else
157  pthisSV->flpf = 1.0F;
158 
159  // clear the reset flag
160  pthisSV->resetflag = false;
161 
162  return;
163 } // end fInit_6DOF_GB_BASIC
164 
165 // function initalizes the 6DOF accel + gyro Kalman filter algorithm
166 void fInit_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
167 {
168  int8 i, j; // loop counters
169 
170  // reset the flag denoting that a first 6DOF orientation lock has been achieved
171  pthisSV->iFirstOrientationLock = 0;
172 
173  // compute and store useful product terms to save floating point calculations later
174  pthisSV->fFastdeltat = 1.0F / (float) iSensorFS;
175  pthisSV->fdeltat = (float) iOverSampleRatio * pthisSV->fFastdeltat;
176  pthisSV->fdeltatsq = pthisSV->fdeltat * pthisSV->fdeltat;
179 
180  // initialize the fixed entries in the measurement matrix C
181  for (i = 0; i < 3; i++)
182  {
183  for (j = 0; j < 9; j++)
184  {
185  pthisSV->fC3x9[i][j]= 0.0F;
186  }
187  }
188  pthisSV->fC3x9[0][6] = pthisSV->fC3x9[1][7] = pthisSV->fC3x9[2][8] = 1.0F;
189 
190  // zero a posteriori orientation, error vector xe+ (thetae+, be+, ae+) and b+
191  f3x3matrixAeqI(pthisSV->fRPl);
192  fqAeq1(&(pthisSV->fqPl));
193  for (i = X; i <= Z; i++)
194  {
195  pthisSV->fThErrPl[i] = pthisSV->fbErrPl[i] = pthisSV->faErrSePl[i] = pthisSV->fbPl[i] = 0.0F;
196  pthisSV->faSePl[i] = 0.0;
197  }
198 
199  // initialize noise variance for Qv and Qw matrix updates
202 
203  // initialize the 6x6 noise covariance matrix Qw of the a priori error vector xe-
204  // Qw is then recursively updated as P+ = (1 - K * C) * P- = (1 - K * C) * Qw and Qw updated from P+
205  // zero the matrix Qw
206  for (i = 0; i < 9; i++)
207  {
208  for (j = 0; j < 9; j++)
209  {
210  pthisSV->fQw9x9[i][j] = 0.0F;
211  }
212  }
213  // loop over non-zero values
214  for (i = 0; i < 3; i++)
215  {
216  // theta_e * theta_e terms
217  pthisSV->fQw9x9[i][i] = FQWINITTHTH_6DOF_GY_KALMAN;
218  // b_e * b_e terms
219  pthisSV->fQw9x9[i + 3][i + 3] = FQWINITBB_6DOF_GY_KALMAN;
220  // th_e * b_e terms
221  pthisSV->fQw9x9[i][i + 3] = pthisSV->fQw9x9[i + 3][i] = FQWINITTHB_6DOF_GY_KALMAN;
222  // a_e * a_e terms
223  pthisSV->fQw9x9[i + 6][i + 6] = FQWINITAA_6DOF_GY_KALMAN;
224  }
225 
226 
227  // clear the reset flag
228  pthisSV->resetflag = false;
229 
230  return;
231 } // end fInit_6DOF_GY_KALMAN
232 
233 // function initializes the 9DOF Kalman filter
234 void fInit_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, int16 ithisCoordSystem, int16 iSensorFS, int16 iOverSampleRatio)
235 {
236  int8 i, j; // loop counters
237 
238  // reset the flag denoting that a first 9DOF orientation lock has been achieved
239  pthisSV->iFirstOrientationLock = 0;
240 
241  // compute and store useful product terms to save floating point calculations later
242  pthisSV->fFastdeltat = 1.0F / (float) iSensorFS;
243  pthisSV->fdeltat = (float) iOverSampleRatio * pthisSV->fFastdeltat;
244  pthisSV->fdeltatsq = pthisSV->fdeltat * pthisSV->fdeltat;
248 
249  // initialize the fixed entries in the measurement matrix C
250  for (i = 0; i < 6; i++)
251  {
252  for (j = 0; j < 12; j++)
253  {
254  pthisSV->fC6x12[i][j]= 0.0F;
255  }
256  }
257  pthisSV->fC6x12[0][6] = pthisSV->fC6x12[1][7] = pthisSV->fC6x12[2][8] = 1.0F;
258  pthisSV->fC6x12[3][9] = pthisSV->fC6x12[4][10] = pthisSV->fC6x12[5][11] = -1.0F;
259 
260  // zero a posteriori orientation, error vector xe+ (thetae+, be+, de+, ae+) and b+ and inertial
261  f3x3matrixAeqI(pthisSV->fRPl);
262  fqAeq1(&(pthisSV->fqPl));
263  for (i = X; i <= Z; i++)
264  {
265  pthisSV->fThErrPl[i] = pthisSV->fbErrPl[i] = pthisSV->faErrSePl[i] = pthisSV->fdErrSePl[i] = pthisSV->fbPl[i] = 0.0F;
266  pthisSV->faSePl[i] = 0.0;
267  }
268 
269  // initialize the reference geomagnetic vector (uT, global frame)
270  pthisSV->fDeltaPl = 0.0F;
271  if (ithisCoordSystem == NED)
272  {
273  // initialize NED geomagnetic vector to zero degrees inclination
274  pthisSV->fmGl[X] = DEFAULTB;
275  pthisSV->fmGl[Y] = 0.0F;
276  pthisSV->fmGl[Z] = 0.0F;
277  }
278  else
279  {
280  // initialize Android and Win8 geomagnetic vector to zero degrees inclination
281  pthisSV->fmGl[X] = 0.0F;
282  pthisSV->fmGl[Y] = DEFAULTB;
283  pthisSV->fmGl[Z] = 0.0F;
284  }
285 
286  // initialize noise variances for Qv and Qw matrix updates
289 
290  // initialize the 12x12 noise covariance matrix Qw of the a priori error vector xe-
291  // Qw is then recursively updated as P+ = (1 - K * C) * P- = (1 - K * C) * Qw and Qw updated from P+
292  // zero the matrix Qw
293  for (i = 0; i < 12; i++)
294  {
295  for (j = 0; j < 12; j++)
296  {
297  pthisSV->fQw12x12[i][j] = 0.0F;
298  }
299  }
300  // loop over non-zero values
301  for (i = 0; i < 3; i++)
302  {
303  // theta_e * theta_e terms
304  pthisSV->fQw12x12[i][i] = FQWINITTHTH_9DOF_GBY_KALMAN;
305  // b_e * b_e terms
306  pthisSV->fQw12x12[i + 3][i + 3] = FQWINITBB_9DOF_GBY_KALMAN;
307  // th_e * b_e terms
308  pthisSV->fQw12x12[i][i + 3] = pthisSV->fQw12x12[i + 3][i] = FQWINITTHB_9DOF_GBY_KALMAN;
309  // a_e * a_e terms
310  pthisSV->fQw12x12[i + 6][i + 6] = FQWINITAA_9DOF_GBY_KALMAN;
311  // d_e * d_e terms
312  pthisSV->fQw12x12[i + 9][i + 9] = FQWINITDD_9DOF_GBY_KALMAN;
313  }
314 
315 
316  // clear the reset flag
317  pthisSV->resetflag = false;
318 
319  return;
320 } // end fInit_9DOF_GBY_KALMAN
321 
322 // 1DOF pressure function
323 void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, int32 loopcounter)
324 {
325  // do a reset and return if requested
326  if (pthisSV->resetflag)
327  {
329  return;
330  }
331 
332  // low pass filter the block averaged (typically 25Hz) height and temperature readings
333  fLPFScalar(&(pthisPressure->fHp), &(pthisSV->fLPHp), pthisSV->flpf, loopcounter);
334  fLPFScalar(&(pthisPressure->fTp), &(pthisSV->fLPTp), pthisSV->flpf, loopcounter);
335 
336  return;
337 } // end fRun_1DOF_P_BASIC
338 
339 // 3DOF orientation function which calls tilt functions and implements low pass filters
340 void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
341 {
342  // do a reset and return if requested
343  if (pthisSV->resetflag)
344  {
345  fInit_3DOF_G_BASIC(pthisSV, 0.2F, SENSORFS*ACCEL_OVERSAMPLE_RATIO, ACCEL_OVERSAMPLE_RATIO);
346  return;
347  }
348 
349  // apply the tilt estimation algorithm to get the instantaneous orientation matrix
350  if (ithisCoordSystem == NED)
351  {
352  // call NED tilt function
353  f3DOFTiltNED(pthisSV->fR, pthisAccel->fGp);
354  }
355  else if (ithisCoordSystem == ANDROID)
356  {
357  // call Android tilt function
358  f3DOFTiltAndroid(pthisSV->fR, pthisAccel->fGp);
359  }
360  else
361  {
362  // call Windows 8 tilt function
363  f3DOFTiltWin8(pthisSV->fR, pthisAccel->fGp);
364  }
365 
366  // compute the instanteneous quaternion from the instantaneous rotation matrix
367  fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
368 
369  // low pass filter the orientation quaternion
370  fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf, pthisSV->fdeltat, pthisSV->fOmega, loopcounter);
371 
372  // compute the low pass rotation matrix and rotation vector
373  fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
374  fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);
375 
376  // calculate the Euler angles from the low pass orientation matrix
377  if (ithisCoordSystem == NED)
378  {
379  // calculate NED Euler angles
380  fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
381  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
382  }
383  else if (ithisCoordSystem == ANDROID)
384  {
385  // calculate Android Euler angles
386  fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
387  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
388  }
389  else
390  {
391  // calculate Windows 8 Euler angles
392  fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
393  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
394  }
395 
396  // force the yaw and compass angles to zero
397  pthisSV->fLPPsi = pthisSV->fLPRho = 0.0F;
398 
399  return;
400 } // end fRun_3DOF_G_BASIC
401 
402 // 2D automobile eCompass
403 void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, int32 loopcounter, int16 ithisCoordSystem)
404 {
405  // do a reset and return if requested
406  if (pthisSV->resetflag)
407  {
408  fInit_3DOF_B_BASIC(pthisSV, 0.6F, SENSORFS*MAG_OVERSAMPLE_RATIO, MAG_OVERSAMPLE_RATIO);
409  return;
410  }
411 
412  // calculate the 3DOF magnetometer orientation matrix from fBc
413  if (ithisCoordSystem == NED)
414  {
415  // call NED magnetic rotation matrix function
416  f3DOFMagnetometerMatrixNED(pthisSV->fR, pthisMag->fBc);
417  }
418  else if (ithisCoordSystem == ANDROID)
419  {
420  // call Android magnetic rotation matrix function
421  f3DOFMagnetometerMatrixAndroid(pthisSV->fR, pthisMag->fBc);
422  }
423  else
424  {
425  // call Windows 8 magnetic rotation matrix function
426  f3DOFMagnetometerMatrixWin8(pthisSV->fR, pthisMag->fBc);
427  }
428 
429  // compute the instanteneous quaternion from the instantaneous rotation matrix
430  fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
431 
432  // low pass filter the orientation quaternion and compute the low pass rotation matrix
433  fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf, pthisSV->fdeltat, pthisSV->fOmega, loopcounter);
434  fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
435  fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);
436 
437  // calculate the Euler angles from the low pass orientation matrix
438  if (ithisCoordSystem == NED)
439  {
440  // calculate NED Euler angles
441  fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
442  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
443  }
444  else if (ithisCoordSystem == ANDROID)
445  {
446  // calculate Android Euler angles
447  fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
448  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
449  }
450  else
451  {
452  // calculate Windows 8 Euler angles
453  fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
454  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
455  }
456 
457  return;
458 }
459 
460 // basic gyro integration function
461 void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro, int32 loopcounter, int16 ithisCoordSystem, int16 iOverSampleRatio)
462 {
463  float rvec[3]; // rotation vector
464  int8 i, j; // loop counters
465 
466  // do a reset and return if requested
467  if (pthisSV->resetflag)
468  {
469  fInit_3DOF_Y_BASIC(pthisSV, SENSORFS*GYRO_OVERSAMPLE_RATIO, GYRO_OVERSAMPLE_RATIO);
470  return;
471  }
472 
473  // set the angular velocity to the last raw gyro reading: omega[k] = yG[k]
474  for (i = X; i <= Z; i++)
475  {
476  pthisSV->fOmega[i] = pthisGyro->fYpBuffer[iOverSampleRatio - 1][i];
477  }
478 
479  // integrate the buffered high frequency (typically 200Hz) gyro readings
480  for (j = 0; j < iOverSampleRatio; j++)
481  {
482  // compute the incremental fast (typically 200Hz) rotation vector rvec (deg)
483  for (i = X; i <= Z; i++)
484  {
485  rvec[i] = pthisGyro->fYpBuffer[j][i] * pthisSV->fFastdeltat;
486  }
487 
488  // compute the incremental quaternion fDeltaq from the rotation vector
489  fQuaternionFromRotationVectorDeg(&(pthisSV->fDeltaq), rvec, 1.0F);
490 
491  // incrementally rotate the orientation quaternion fq
492  qAeqAxB(&(pthisSV->fq), &(pthisSV->fDeltaq));
493  }
494 
495  // re-normalize the orientation quaternion to stop error propagation
496  // the renormalization function ensures that the scalar component q0 is non-negative
497  fqAeqNormqA(&(pthisSV->fq));
498 
499  // get the rotation matrix from the quaternion
500  fRotationMatrixFromQuaternion(pthisSV->fR, &(pthisSV->fq));
501 
502  // compute the rotation vector from the a posteriori quaternion
503  fRotationVectorDegFromQuaternion(&(pthisSV->fq), pthisSV->fRVec);
504 
505  // *********************************************************************************
506  // compute the Euler angles from the orientation matrix
507  // *********************************************************************************
508 
509  if (ithisCoordSystem == NED)
510  {
511  // calculate the NED Euler angles
512  fNEDAnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi), &(pthisSV->fThe), &(pthisSV->fPsi),
513  &(pthisSV->fRho), &(pthisSV->fChi));
514  }
515  else if (ithisCoordSystem == ANDROID)
516  {
517  // calculate the Android Euler angles
518  fAndroidAnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi), &(pthisSV->fThe), &(pthisSV->fPsi),
519  &(pthisSV->fRho), &(pthisSV->fChi));
520  }
521  else
522  {
523  // calculate Win8 Euler angles
524  fWin8AnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi), &(pthisSV->fThe), &(pthisSV->fPsi),
525  &(pthisSV->fRho), &(pthisSV->fChi));
526  }
527 
528  return;
529 } // end fRun_3DOF_Y_BASIC
530 
531 // 6DOF orientation function which calls ecompass and implements low pass filters
532 void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
533 {
534  // do a reset and return if requested
535  if (pthisSV->resetflag)
536  {
537  fInit_6DOF_GB_BASIC(pthisSV, 0.6F, SENSORFS*GYRO_OVERSAMPLE_RATIO, GYRO_OVERSAMPLE_RATIO);
538  return;
539  }
540 
541  // call the eCompass algorithm to get the instantaneous orientation matrix and inclination angle
542  if (ithisCoordSystem == NED)
543  {
544  // call the NED eCompass
545  feCompassNED(pthisSV->fR, &(pthisSV->fDelta), pthisMag->fBc, pthisAccel->fGp);
546  }
547  else if (ithisCoordSystem == ANDROID)
548  {
549  // call the Android eCompass
550  feCompassAndroid(pthisSV->fR, &(pthisSV->fDelta), pthisMag->fBc, pthisAccel->fGp);
551  }
552  else
553  {
554  // call the Win8 eCompass
555  feCompassWin8(pthisSV->fR, &(pthisSV->fDelta), pthisMag->fBc, pthisAccel->fGp);
556  }
557 
558  // compute the instanteneous quaternion from the instantaneous rotation matrix
559  fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
560 
561  // low pass filter the orientation quaternion and compute the low pass rotation matrix
562  fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf, pthisSV->fdeltat, pthisSV->fOmega, loopcounter);
563  fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
564  fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);
565 
566  // compute the low pass filtered Euler angles
567  if (ithisCoordSystem == NED)
568  {
569  // calculate the NED Euler angles
570  fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
571  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
572  }
573  else if (ithisCoordSystem == ANDROID)
574  {
575  // calculate the Android Euler angles
576  fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
577  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
578  }
579  else
580  {
581  // calculate the Windows 8 Euler angles
582  fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi), &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
583  &(pthisSV->fLPRho), &(pthisSV->fLPChi));
584  }
585 
586  // low pass filter the geomagnetic inclination angle with a simple exponential filter
587  fLPFScalar(&(pthisSV->fDelta), &(pthisSV->fLPDelta), pthisSV->flpf, loopcounter);
588 
589  return;
590 } // end fRun_6DOF_GB_BASIC
591 
592 // 6DOF accel + gyro Kalman filter algorithm
593 void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro,
594  int16 ithisCoordSystem, int16 iOverSampleRatio)
595 {
596  // local arrays and scalars
597  float rvec[3]; // rotation vector
598  float ftmpA9x3[9][3]; // scratch array
599 
600  // assorted array pointers
601  float *pfPPlus9x9kj;
602  float *pfPPlus9x9ij;
603  float *pfK9x3ij;
604  float *pfK9x3ik;
605  float *pftmpA9x3ik;
606  float *pftmpA9x3ij;
607  float *pftmpA9x3kj;
608  float *pfQw9x9ij;
609  float *pfQw9x9ik;
610  float *pfQw9x9kj;
611  float *pfC3x9ik;
612  float *pfC3x9jk;
613 
614  int8 i, j, k; // loop counters
615 
616  // working arrays for 3x3 matrix inversion
617  float *pfRows[3];
618  int8 iColInd[3];
619  int8 iRowInd[3];
620  int8 iPivot[3];
621 
622  // do a reset and return if requested
623  if (pthisSV->resetflag)
624  {
625  fInit_6DOF_GY_KALMAN(pthisSV, SENSORFS*GYRO_OVERSAMPLE_RATIO, GYRO_OVERSAMPLE_RATIO);
626  return;
627  }
628 
629  // do a once-only orientation lock to accelerometer tilt
630  if (!pthisSV->iFirstOrientationLock)
631  {
632  // get the 3DOF orientation matrix and initial inclination angle
633  if (ithisCoordSystem == NED)
634  {
635  // call NED tilt function
636  f3DOFTiltNED(pthisSV->fRPl, pthisAccel->fGp);
637  }
638  else if (ithisCoordSystem == ANDROID)
639  {
640  // call Android tilt function
641  f3DOFTiltAndroid(pthisSV->fRPl, pthisAccel->fGp);
642  }
643  else
644  {
645  // call Windows 8 tilt function
646  f3DOFTiltWin8(pthisSV->fRPl, pthisAccel->fGp);
647  }
648 
649  // get the orientation quaternion from the orientation matrix
650  fQuaternionFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fqPl));
651 
652  // set the orientation lock flag so this initial alignment is only performed once
653  pthisSV->iFirstOrientationLock = 1;
654  }
655 
656  // *********************************************************************************
657  // calculate a priori rotation matrix
658  // *********************************************************************************
659 
660  // initialize the a priori orientation quaternion to the a posteriori orientation estimate
661  pthisSV->fqMi = pthisSV->fqPl;
662 
663  // integrate the buffered high frequency (typically 200Hz) gyro readings
664  for (j = 0; j < iOverSampleRatio; j++)
665  {
666  // compute the incremental fast (typically 200Hz) rotation vector rvec (deg)
667  for (i = X; i <= Z; i++)
668  {
669  rvec[i] = (pthisGyro->fYpBuffer[j][i] - pthisSV->fbPl[i]) * pthisSV->fFastdeltat;
670  }
671 
672  // compute the incremental quaternion fDeltaq from the rotation vector
673  fQuaternionFromRotationVectorDeg(&(pthisSV->fDeltaq), rvec, 1.0F);
674 
675  // incrementally rotate the a priori orientation quaternion fqMi
676  // the a posteriori orientation is re-normalized later so this update is stable
677  qAeqAxB(&(pthisSV->fqMi), &(pthisSV->fDeltaq));
678  }
679 
680  // get the a priori rotation matrix from the a priori quaternion
681  fRotationMatrixFromQuaternion(pthisSV->fRMi, &(pthisSV->fqMi));
682 
683  // *********************************************************************************
684  // calculate a priori gyro and accelerometer estimates of the gravity vector
685  // and the error between the two
686  // *********************************************************************************
687 
688  // compute the a priori **gyro** estimate of the gravitational vector (g, sensor frame)
689  // using an absolute rotation of the global frame gravity vector (with magnitude 1g)
690  for (i = X; i <= Z; i++)
691  {
692  if (ithisCoordSystem == NED)
693  {
694  // NED gravity is along positive z axis
695  pthisSV->fgSeGyMi[i] = pthisSV->fRMi[i][Z];
696  }
697  else
698  {
699  // Android and Win8 gravity are along negative z axis
700  pthisSV->fgSeGyMi[i] = -pthisSV->fRMi[i][Z];
701  }
702 
703  // compute a priori acceleration (a-) (g, sensor frame) from a posteriori estimate (g, sensor frame)
704  pthisSV->faSeMi[i] = FCA_6DOF_GY_KALMAN * pthisSV->faSePl[i];
705 
706  // compute the a priori gravity error vector (accelerometer minus gyro estimates) (g, sensor frame)
707  if ((ithisCoordSystem == NED) || (ithisCoordSystem == WIN8))
708  {
709  // NED and Windows 8 have positive sign for gravity: y = g - a and g = y + a
710  pthisSV->fgErrSeMi[i] = pthisAccel->fGp[i] + pthisSV->faSeMi[i] - pthisSV->fgSeGyMi[i];
711  }
712  else
713  {
714  // Android has negative sign for gravity: y = a - g, g = -y + a
715  pthisSV->fgErrSeMi[i] = -pthisAccel->fGp[i] + pthisSV->faSeMi[i] - pthisSV->fgSeGyMi[i];
716  }
717  }
718 
719  // *********************************************************************************
720  // update variable elements of measurement matrix C
721  // *********************************************************************************
722 
723  // update measurement matrix C (3x9) with -alpha(g-)x from gyro (g, sensor frame)
724  pthisSV->fC3x9[0][1] = FDEGTORAD * pthisSV->fgSeGyMi[Z];
725  pthisSV->fC3x9[0][2] = -FDEGTORAD * pthisSV->fgSeGyMi[Y];
726  pthisSV->fC3x9[1][2] = FDEGTORAD * pthisSV->fgSeGyMi[X];
727  pthisSV->fC3x9[1][0] = -pthisSV->fC3x9[0][1];
728  pthisSV->fC3x9[2][0] = -pthisSV->fC3x9[0][2];
729  pthisSV->fC3x9[2][1] = -pthisSV->fC3x9[1][2];
730  pthisSV->fC3x9[0][4] = -pthisSV->fdeltat * pthisSV->fC3x9[0][1];
731  pthisSV->fC3x9[0][5] = -pthisSV->fdeltat * pthisSV->fC3x9[0][2];
732  pthisSV->fC3x9[1][5] = -pthisSV->fdeltat * pthisSV->fC3x9[1][2];
733  pthisSV->fC3x9[1][3]= -pthisSV->fC3x9[0][4];
734  pthisSV->fC3x9[2][3]= -pthisSV->fC3x9[0][5];
735  pthisSV->fC3x9[2][4]= -pthisSV->fC3x9[1][5];
736 
737  // *********************************************************************************
738  // calculate the Kalman gain matrix K (9x3)
739  // K = P- * C^T * inv(C * P- * C^T + Qv) = Qw * C^T * inv(C * Qw * C^T + Qv)
740  // Qw is used as a proxy for P- throughout the code
741  // P+ is used here as a working array to reduce RAM usage and is re-computed later
742  // *********************************************************************************
743 
744  // set ftmpA9x3 = P- * C^T = Qw * C^T where Qw and C are both sparse
745  // C also has a significant number of +1 and -1 entries
746  // ftmpA9x3 is also sparse but not symmetric
747  for (i = 0; i < 9; i++) // loop over rows of ftmpA9x3
748  {
749  // initialize pftmpA9x3ij for current i, j=0
750  pftmpA9x3ij = ftmpA9x3[i];
751 
752  for (j = 0; j < 3; j++) // loop over columns of ftmpA9x3
753  {
754  // zero ftmpA9x3[i][j]
755  *pftmpA9x3ij = 0.0F;
756 
757  // initialize pfC3x9jk for current j, k=0
758  pfC3x9jk = pthisSV->fC3x9[j];
759 
760  // initialize pfQw9x9ik for current i, k=0
761  pfQw9x9ik = pthisSV->fQw9x9[i];
762 
763  // sum matrix products over inner loop over k
764  for (k = 0; k < 9; k++)
765  {
766  if ((*pfQw9x9ik != 0.0F) && (*pfC3x9jk != 0.0F))
767  {
768  if (*pfC3x9jk == 1.0F)
769  *pftmpA9x3ij += *pfQw9x9ik;
770  else if (*pfC3x9jk == -1.0F)
771  *pftmpA9x3ij -= *pfQw9x9ik;
772  else
773  *pftmpA9x3ij += *pfQw9x9ik * *pfC3x9jk;
774  }
775 
776  // increment pfC3x9jk and pfQw9x9ik for next iteration of k
777  pfC3x9jk++;
778  pfQw9x9ik++;
779 
780  } // end of loop over k
781 
782  // increment pftmpA9x3ij for next iteration of j
783  pftmpA9x3ij++;
784 
785  } // end of loop over j
786  } // end of loop over i
787 
788  // set symmetric P+ (3x3 scratch sub-matrix) to C * P- * C^T + Qv
789  // = C * (Qw * C^T) + Qv = C * ftmpA9x3 + Qv
790  // both C and ftmpA9x3 are sparse but not symmetric
791  for (i = 0; i < 3; i++) // loop over rows of P+
792  {
793  // initialize pfPPlus9x9ij for current i, j=i
794  pfPPlus9x9ij = pthisSV->fPPlus9x9[i] + i;
795 
796  for (j = i; j < 3; j++) // loop over above diagonal columns of P+
797  {
798  // zero P+[i][j]
799  *pfPPlus9x9ij = 0.0F;
800 
801  // initialize pfC3x9ik for current i, k=0
802  pfC3x9ik = pthisSV->fC3x9[i];
803 
804  // initialize pftmpA9x3kj for current j, k=0
805  pftmpA9x3kj = *ftmpA9x3 + j;
806 
807  // sum matrix products over inner loop over k
808  for (k = 0; k < 9; k++)
809  {
810  if ((*pfC3x9ik != 0.0F) && (*pftmpA9x3kj != 0.0F))
811  {
812  if (*pfC3x9ik == 1.0F)
813  *pfPPlus9x9ij += *pftmpA9x3kj;
814  else if (*pfC3x9ik == -1.0F)
815  *pfPPlus9x9ij -= *pftmpA9x3kj;
816  else
817  *pfPPlus9x9ij += *pfC3x9ik * *pftmpA9x3kj;
818  }
819 
820  // update pfC3x9ik and pftmpA9x3kj for next iteration of k
821  pfC3x9ik++;
822  pftmpA9x3kj += 3;
823 
824  } // end of loop over k
825 
826  // increment pfPPlus9x9ij for next iteration of j
827  pfPPlus9x9ij++;
828 
829  } // end of loop over j
830  } // end of loop over i
831 
832  // add in noise covariance terms to the diagonal
833  pthisSV->fPPlus9x9[0][0] += pthisSV->fQvAA;
834  pthisSV->fPPlus9x9[1][1] += pthisSV->fQvAA;
835  pthisSV->fPPlus9x9[2][2] += pthisSV->fQvAA;
836 
837  // copy above diagonal terms of P+ (3x3 scratch sub-matrix) to below diagonal terms
838  pthisSV->fPPlus9x9[1][0] = pthisSV->fPPlus9x9[0][1];
839  pthisSV->fPPlus9x9[2][0] = pthisSV->fPPlus9x9[0][2];
840  pthisSV->fPPlus9x9[2][1] = pthisSV->fPPlus9x9[1][2];
841 
842  // calculate inverse of P+ (3x3 scratch sub-matrix) = inv(C * P- * C^T + Qv) = inv(C * Qw * C^T + Qv)
843  for (i = 0; i < 3; i++)
844  {
845  pfRows[i] = pthisSV->fPPlus9x9[i];
846  }
847  fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 3);
848 
849  // set K = P- * C^T * inv(C * P- * C^T + Qv) = Qw * C^T * inv(C * Qw * C^T + Qv)
850  // = ftmpA9x3 * P+ (3x3 sub-matrix)
851  // ftmpA9x3 = Qw * C^T is sparse but P+ (3x3 sub-matrix) is not
852  // K is not symmetric because C is not symmetric
853  for (i = 0; i < 9; i++) // loop over rows of K9x3
854  {
855  // initialize pfK9x3ij for i, j=0
856  pfK9x3ij = pthisSV->fK9x3[i];
857 
858  for (j = 0; j < 3; j++) // loop over columns of K9x3
859  {
860  // zero the matrix element fK9x3[i][j]
861  *pfK9x3ij = 0.0F;
862 
863  // initialize pftmpA9x3ik for i, k=0
864  pftmpA9x3ik = ftmpA9x3[i];
865 
866  // initialize pfPPlus9x9kj for j, k=0
867  pfPPlus9x9kj = *pthisSV->fPPlus9x9 + j;
868 
869  // sum matrix products over inner loop over k
870  for (k = 0; k < 3; k++)
871  {
872  if (*pftmpA9x3ik != 0.0F)
873  {
874  *pfK9x3ij += *pftmpA9x3ik * *pfPPlus9x9kj;
875  }
876 
877  // increment pftmpA9x3ik and pfPPlus9x9kj for next iteration of k
878  pftmpA9x3ik++;
879  pfPPlus9x9kj += 9;
880 
881  } // end of loop over k
882 
883  // increment pfK9x3ij for the next iteration of j
884  pfK9x3ij++;
885 
886  } // end of loop over j
887  } // end of loop over i
888 
889  // *********************************************************************************
890  // calculate a posteriori error estimate: xe+ = K * ze-
891  // *********************************************************************************
892 
893  // update the a posteriori state vector
894  for (i = X; i <= Z; i++)
895  {
896  // zero a posteriori error terms
897  pthisSV->fThErrPl[i] = pthisSV->fbErrPl[i] = pthisSV->faErrSePl[i] = 0.0F;
898 
899  // accumulate the error vector terms K * ze-
900  for (k = 0; k < 3; k++)
901  {
902  pthisSV->fThErrPl[i] += pthisSV->fK9x3[i][k] * pthisSV->fgErrSeMi[k];
903  pthisSV->fbErrPl[i] += pthisSV->fK9x3[i + 3][k] * pthisSV->fgErrSeMi[k];
904  pthisSV->faErrSePl[i] += pthisSV->fK9x3[i + 6][k] * pthisSV->fgErrSeMi[k];
905  }
906  }
907 
908  // *********************************************************************************
909  // apply the a posteriori error corrections to the a posteriori state vector
910  // *********************************************************************************
911 
912  // get the a posteriori delta quaternion
913  fQuaternionFromRotationVectorDeg(&(pthisSV->fDeltaq), pthisSV->fThErrPl, -1.0F);
914 
915  // compute the a posteriori orientation quaternion fqPl = fqMi * Deltaq(-thetae+)
916  // the resulting quaternion may have negative scalar component q0
917  qAeqBxC(&(pthisSV->fqPl), &(pthisSV->fqMi), &(pthisSV->fDeltaq));
918 
919  // normalize the a posteriori orientation quaternion to stop error propagation
920  // the renormalization function ensures that the scalar component q0 is non-negative
921  fqAeqNormqA(&(pthisSV->fqPl));
922 
923  // compute the a posteriori rotation matrix from the a posteriori quaternion
924  fRotationMatrixFromQuaternion(pthisSV->fRPl, &(pthisSV->fqPl));
925 
926  // compute the rotation vector from the a posteriori quaternion
927  fRotationVectorDegFromQuaternion(&(pthisSV->fqPl), pthisSV->fRVecPl);
928 
929  // update the a posteriori gyro offset vector b+ and linear acceleration vector a+ (sensor frame)
930  for (i = X; i <= Z; i++)
931  {
932  // b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s)
933  pthisSV->fbPl[i] -= pthisSV->fbErrPl[i];
934  // a+ = a- - ae+ (g, sensor frame)
935  pthisSV->faSePl[i] = pthisSV->faSeMi[i] - pthisSV->faErrSePl[i];
936  }
937 
938  // compute the angular velocity for display purposes only: omega[k] = yG[k] - b+[k] (deg/s)
939  for (i = X; i <= Z; i++)
940  {
941  pthisSV->fOmega[i] = pthisGyro->fYpBuffer[iOverSampleRatio - 1][i] - pthisSV->fbPl[i];
942  }
943 
944  // *********************************************************************************
945  // compute the a posteriori Euler angles from the orientation matrix
946  // *********************************************************************************
947 
948  if (ithisCoordSystem == NED)
949  {
950  // calculate the NED Euler angles
951  fNEDAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl),
952  &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
953  }
954  else if (ithisCoordSystem == ANDROID)
955  {
956  // calculate the Android Euler angles
957  fAndroidAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl),
958  &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
959  }
960  else
961  {
962  // calculate Win8 Euler angles
963  fWin8AnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl),
964  &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
965  }
966 
967  // ***********************************************************************************
968  // calculate (symmetric) a posteriori error covariance matrix P+
969  // P+ = (I12 - K * C) * P- = (I12 - K * C) * Qw = Qw - K * (C * Qw)
970  // both Qw and P+ are used as working arrays in this section
971  // at the end of this section, P+ is valid but Qw is over-written
972  // ***********************************************************************************
973 
974  // set P+ (3x9 scratch sub-matrix) to the product C (3x9) * Qw (9x9)
975  // where both C and Qw are sparse and C has a significant number of +1 entries
976  // the resulting matrix is sparse but not symmetric
977  for (i = 0; i < 3; i++) // loop over the rows of P+
978  {
979  // initialize pfPPlus9x9ij for current i, j=0
980  pfPPlus9x9ij = pthisSV->fPPlus9x9[i];
981 
982  for (j = 0; j < 9; j++) // loop over the columns of P+
983  {
984  // zero P+[i][j]
985  *pfPPlus9x9ij = 0.0F;
986 
987  // initialize pfC3x9ik for current i, k=0
988  pfC3x9ik = pthisSV->fC3x9[i];
989 
990  // initialize pfQw9x9kj for current j, k=0
991  pfQw9x9kj = &pthisSV->fQw9x9[0][j];
992 
993  // sum matrix products over inner loop over k
994  for (k = 0; k < 9; k++)
995  {
996  if ((*pfC3x9ik != 0.0F) && (*pfQw9x9kj != 0.0F))
997  {
998  if (*pfC3x9ik == 1.0F)
999  *pfPPlus9x9ij += *pfQw9x9kj;
1000  else if (*pfC3x9ik == -1.0F)
1001  *pfPPlus9x9ij -= *pfQw9x9kj;
1002  else
1003  *pfPPlus9x9ij += *pfC3x9ik * *pfQw9x9kj;
1004  }
1005 
1006  // update pfC3x9ik and pfQw9x9kj for next iteration of k
1007  pfC3x9ik++;
1008  pfQw9x9kj += 9;
1009 
1010  } // end of loop over k
1011 
1012  // increment pfPPlus9x9ij for next iteration of j
1013  pfPPlus9x9ij++;
1014 
1015  } // end of loop over j
1016  } // end of loop over i
1017 
1018  // compute P+ = (I9 - K * C) * Qw = Qw - K * (C * Qw) = Qw - K * P+ (3x9 sub-matrix)
1019  // storing result P+ in Qw and over-writing Qw which is OK since Qw is later computed from P+
1020  // where working array P+ (6x12 sub-matrix) is sparse but K is not sparse
1021  // only on and above diagonal terms of P+ are computed since P+ is symmetric
1022  for (i = 0; i < 9; i++)
1023  {
1024  // initialize pfQw9x9ij for i, j=i
1025  pfQw9x9ij = pthisSV->fQw9x9[i] + i;
1026 
1027  for (j = i; j < 9; j++)
1028  {
1029  // initialize pfK9x3ik for i, k=0
1030  pfK9x3ik = pthisSV->fK9x3[i];
1031 
1032  // initialize pfPPlus9x9kj for j, k=0
1033  pfPPlus9x9kj = *pthisSV->fPPlus9x9 + j;
1034 
1035  // compute on and above diagonal matrix entry
1036  for (k = 0; k < 3; k++)
1037  {
1038  // check for non-zero values since P+ (3x9 scratch sub-matrix) is sparse
1039  if (*pfPPlus9x9kj != 0.0F)
1040  {
1041  *pfQw9x9ij -= *pfK9x3ik * *pfPPlus9x9kj;
1042  }
1043 
1044  // increment pfK9x3ik and pfPPlus9x9kj for next iteration of k
1045  pfK9x3ik++;
1046  pfPPlus9x9kj += 9;
1047 
1048  } // end of loop over k
1049 
1050  // increment pfQw9x9ij for next iteration of j
1051  pfQw9x9ij++;
1052 
1053  } // end of loop over j
1054  } // end of loop over i
1055 
1056  // Qw now holds the on and above diagonal elements of P+ (9x9)
1057  // so perform a simple copy to the all elements of P+
1058  // after execution of this code P+ is valid but Qw remains invalid
1059  for (i = 0; i < 9; i++)
1060  {
1061  // initialize pfPPlus9x9ij and pfQw9x9ij for i, j=i
1062  pfPPlus9x9ij = pthisSV->fPPlus9x9[i] + i;
1063  pfQw9x9ij = pthisSV->fQw9x9[i] + i;
1064 
1065  // copy the on-diagonal elements and increment pointers to enter loop at j=i+1
1066  *(pfPPlus9x9ij++) = *(pfQw9x9ij++);
1067 
1068  // loop over above diagonal columns j copying to below-diagonal elements
1069  for (j = i + 1; j < 9; j++)
1070  {
1071  *(pfPPlus9x9ij++)= pthisSV->fPPlus9x9[j][i] = *(pfQw9x9ij++);
1072  }
1073  }
1074 
1075  // *********************************************************************************
1076  // re-create the noise covariance matrix Qw=fn(P+) for the next iteration
1077  // using the elements of P+ which are now valid
1078  // Qw was over-written earlier but is here recomputed (all elements)
1079  // *********************************************************************************
1080 
1081  // zero the matrix Qw (9x9)
1082  for (i = 0; i < 9; i++)
1083  {
1084  for (j = 0; j < 9; j++)
1085  {
1086  pthisSV->fQw9x9[i][j] = 0.0F;
1087  }
1088  }
1089 
1090  // update the covariance matrix components
1091  for (i = 0; i < 3; i++)
1092  {
1093  // Qw[th-th-] = Qw[0-2][0-2] = E[th-(th-)^T] = Q[th+th+] + deltat^2 * (Q[b+b+] + (Qwb + QvG) * I)
1094  pthisSV->fQw9x9[i][i] = pthisSV->fPPlus9x9[i][i] + pthisSV->fdeltatsq * (pthisSV->fPPlus9x9[i + 3][i + 3] + pthisSV->fQwbplusQvG);
1095 
1096  // Qw[b-b-] = Qw[3-5][3-5] = E[b-(b-)^T] = Q[b+b+] + Qwb * I
1097  pthisSV->fQw9x9[i + 3][i + 3] = pthisSV->fPPlus9x9[i + 3][i + 3] + FQWB_6DOF_GY_KALMAN;
1098 
1099  // Qw[th-b-] = Qw[0-2][3-5] = E[th-(b-)^T] = -deltat * (Q[b+b+] + Qwb * I) = -deltat * Qw[b-b-]
1100  pthisSV->fQw9x9[i][i + 3] = pthisSV->fQw9x9[i + 3][i] = -pthisSV->fdeltat * pthisSV->fQw9x9[i + 3][i + 3];
1101 
1102  // Qw[a-a-] = Qw[6-8][6-8] = E[a-(a-)^T] = ca^2 * Q[a+a+] + Qwa * I
1103  pthisSV->fQw9x9[i + 6][i + 6] = pthisSV->fcasq * pthisSV->fPPlus9x9[i + 6][i + 6] + FQWA_6DOF_GY_KALMAN;
1104  }
1105 
1106  return;
1107 } // end fRun_6DOF_GY_KALMAN
1108 
1109 // 9DOF orientation function implemented using a 12 element Kalman filter
1110 void fRun_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro,
1111  struct MagCalibration *pthisMagCal, int16 ithisCoordSystem, int16 iOverSampleRatio)
1112 {
1113  // local scalars and arrays
1114  float fopp, fadj, fhyp; // opposite, adjacent and hypoteneuse
1115  float fsindelta, fcosdelta; // sin and cos of inclination angle delta
1116  float rvec[3]; // rotation vector
1117  float ftmp; // scratch variable
1118  float ftmpA12x6[12][6]; // scratch array
1119  int8 i, j, k; // loop counters
1120  int8 iMagJamming; // magnetic jamming flag
1121 
1122  // assorted array pointers
1123  float *pfPPlus12x12ij;
1124  float *pfPPlus12x12kj;
1125  float *pfQw12x12ij;
1126  float *pfQw12x12ik;
1127  float *pfQw12x12kj;
1128  float *pfK12x6ij;
1129  float *pfK12x6ik;
1130  float *pftmpA12x6ik;
1131  float *pftmpA12x6kj;
1132  float *pftmpA12x6ij;
1133  float *pfC6x12ik;
1134  float *pfC6x12jk;
1135 
1136  // working arrays for 6x6 matrix inversion
1137  float *pfRows[6];
1138  int8 iColInd[6];
1139  int8 iRowInd[6];
1140  int8 iPivot[6];
1141 
1142  // do a reset and return if requested
1143  if (pthisSV->resetflag)
1144  {
1145  fInit_9DOF_GBY_KALMAN(pthisSV, THISCOORDSYSTEM, SENSORFS*GYRO_OVERSAMPLE_RATIO, GYRO_OVERSAMPLE_RATIO);
1146  return;
1147  }
1148 
1149  // *********************************************************************************
1150  // initial orientation lock to accelerometer and magnetometer eCompass orientation
1151  // *********************************************************************************
1152 
1153  // do a once-only orientation lock after the first valid magnetic calibration
1154  if (pthisMagCal->iValidMagCal && !pthisSV->iFirstOrientationLock)
1155  {
1156  // get the 6DOF orientation matrix and initial inclination angle
1157  if (ithisCoordSystem == NED)
1158  {
1159  feCompassNED(pthisSV->fRPl, &(pthisSV->fDeltaPl), pthisMag->fBc, pthisAccel->fGp);
1160  }
1161  else if (ithisCoordSystem == ANDROID)
1162  {
1163  feCompassAndroid(pthisSV->fRPl, &(pthisSV->fDeltaPl), pthisMag->fBc, pthisAccel->fGp);
1164  }
1165  else if (ithisCoordSystem == WIN8)
1166  {
1167  feCompassWin8(pthisSV->fRPl, &(pthisSV->fDeltaPl), pthisMag->fBc, pthisAccel->fGp);
1168  }
1169 
1170  // get the orientation quaternion from the orientation matrix
1171  fQuaternionFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fqPl));
1172 
1173  // set the orientation lock flag so this initial alignment is only performed once
1174  pthisSV->iFirstOrientationLock = 1;
1175  }
1176 
1177  // *********************************************************************************
1178  // calculate a priori rotation matrix
1179  // *********************************************************************************
1180 
1181  // initialize the a priori orientation quaternion to the previous a posteriori estimate
1182  pthisSV->fqMi = pthisSV->fqPl;
1183 
1184  // integrate the buffered high frequency (typically 200Hz) gyro readings
1185  for (j = 0; j < iOverSampleRatio; j++)
1186  {
1187  // compute the incremental fast (typically 200Hz) rotation vector rvec (deg)
1188  for (i = X; i <= Z; i++)
1189  {
1190  rvec[i] = (pthisGyro->fYpBuffer[j][i] - pthisSV->fbPl[i]) * pthisSV->fFastdeltat;
1191  }
1192 
1193  // compute the incremental quaternion fDeltaq from the rotation vector
1194  fQuaternionFromRotationVectorDeg(&(pthisSV->fDeltaq), rvec, 1.0F);
1195 
1196  // incrementally rotate the a priori orientation quaternion fqMi
1197  // the a posteriori quaternion fqPl is re-normalized later so this update is stable
1198  qAeqAxB(&(pthisSV->fqMi), &(pthisSV->fDeltaq));
1199  }
1200 
1201  // get the a priori rotation matrix from the a priori quaternion
1202  fRotationMatrixFromQuaternion(pthisSV->fRMi, &(pthisSV->fqMi));
1203 
1204  // *********************************************************************************
1205  // calculate a priori gyro, accelerometer and magnetometer estimates
1206  // of the gravity and geomagnetic vectors and errors
1207  // the most recent 'Fast' measurements are used to reduce phase errors
1208  // *********************************************************************************
1209 
1210  for (i = X; i <= Z; i++)
1211  {
1212  // compute the a priori gyro estimate of the gravitational vector (g, sensor frame)
1213  // using an absolute rotation of the global frame gravity vector (with magnitude 1g)
1214  if (ithisCoordSystem == NED)
1215  {
1216  // NED gravity is along positive z axis
1217  pthisSV->fgSeGyMi[i] = pthisSV->fRMi[i][Z];
1218  }
1219  else
1220  {
1221  // Android and Win8 gravity are along negative z axis
1222  pthisSV->fgSeGyMi[i] = -pthisSV->fRMi[i][Z];
1223  }
1224 
1225  // compute a priori acceleration (a-) (g, sensor frame) from decayed a posteriori estimate (g, sensor frame)
1226  pthisSV->faSeMi[i] = FCA_9DOF_GBY_KALMAN * pthisSV->faSePl[i];
1227 
1228  // compute the a priori gravity error vector (accelerometer minus gyro estimates) (g, sensor frame)
1229  if ((ithisCoordSystem == NED) || (ithisCoordSystem == WIN8))
1230  {
1231  // NED and Windows 8 have positive sign for gravity: y = g - a and g = y + a
1232  pthisSV->fgErrSeMi[i] = pthisAccel->fGp[i] + pthisSV->faSeMi[i] - pthisSV->fgSeGyMi[i];
1233  }
1234  else
1235  {
1236  // Android has negative sign for gravity: y = a - g, g = -y + a
1237  pthisSV->fgErrSeMi[i] = -pthisAccel->fGp[i] + pthisSV->faSeMi[i] - pthisSV->fgSeGyMi[i];
1238  }
1239 
1240  // compute the a priori gyro estimate of the geomagnetic vector (uT, sensor frame)
1241  // using an absolute rotation of the global frame geomagnetic vector (with magnitude B uT)
1242  if (ithisCoordSystem == NED)
1243  {
1244  // NED y component of geomagnetic vector in global frame is zero
1245  pthisSV->fmSeGyMi[i] = pthisSV->fRMi[i][X] * pthisSV->fmGl[X] + pthisSV->fRMi[i][Z] * pthisSV->fmGl[Z];
1246  }
1247  else
1248  {
1249  // Android and Windows 8 x component of geomagnetic vector in global frame is zero
1250  pthisSV->fmSeGyMi[i] = pthisSV->fRMi[i][Y] * pthisSV->fmGl[Y] + pthisSV->fRMi[i][Z] * pthisSV->fmGl[Z];
1251  }
1252 
1253  // compute the a priori geomagnetic error vector (magnetometer minus gyro estimates) (g, sensor frame)
1254  pthisSV->fmErrSeMi[i] = pthisMag->fBc[i] - pthisSV->fmSeGyMi[i];
1255  }
1256 
1257  // *********************************************************************************
1258  // update variable elements of measurement matrix C
1259  // *********************************************************************************
1260 
1261  // update measurement matrix C with -alpha(g-)x and -alpha(m-)x from gyro (g, uT, sensor frame)
1262  pthisSV->fC6x12[0][1] = FDEGTORAD * pthisSV->fgSeGyMi[Z];
1263  pthisSV->fC6x12[0][2] = -FDEGTORAD * pthisSV->fgSeGyMi[Y];
1264  pthisSV->fC6x12[1][2] = FDEGTORAD * pthisSV->fgSeGyMi[X];
1265  pthisSV->fC6x12[1][0] = -pthisSV->fC6x12[0][1];
1266  pthisSV->fC6x12[2][0] = -pthisSV->fC6x12[0][2];
1267  pthisSV->fC6x12[2][1] = -pthisSV->fC6x12[1][2];
1268  pthisSV->fC6x12[3][1] = FDEGTORAD * pthisSV->fmSeGyMi[Z];
1269  pthisSV->fC6x12[3][2] = -FDEGTORAD * pthisSV->fmSeGyMi[Y];
1270  pthisSV->fC6x12[4][2] = FDEGTORAD * pthisSV->fmSeGyMi[X];
1271  pthisSV->fC6x12[4][0] = -pthisSV->fC6x12[3][1];
1272  pthisSV->fC6x12[5][0] = -pthisSV->fC6x12[3][2];
1273  pthisSV->fC6x12[5][1] = -pthisSV->fC6x12[4][2];
1274  pthisSV->fC6x12[0][4] = -pthisSV->fdeltat * pthisSV->fC6x12[0][1];
1275  pthisSV->fC6x12[0][5] = -pthisSV->fdeltat * pthisSV->fC6x12[0][2];
1276  pthisSV->fC6x12[1][5] = -pthisSV->fdeltat * pthisSV->fC6x12[1][2];
1277  pthisSV->fC6x12[1][3]= -pthisSV->fC6x12[0][4];
1278  pthisSV->fC6x12[2][3]= -pthisSV->fC6x12[0][5];
1279  pthisSV->fC6x12[2][4]= -pthisSV->fC6x12[1][5];
1280  pthisSV->fC6x12[3][4] = -pthisSV->fdeltat * pthisSV->fC6x12[3][1];
1281  pthisSV->fC6x12[3][5] = -pthisSV->fdeltat * pthisSV->fC6x12[3][2];
1282  pthisSV->fC6x12[4][5] = -pthisSV->fdeltat * pthisSV->fC6x12[4][2];
1283  pthisSV->fC6x12[4][3] = -pthisSV->fC6x12[3][4];
1284  pthisSV->fC6x12[5][3] = -pthisSV->fC6x12[3][5];
1285  pthisSV->fC6x12[5][4] = -pthisSV->fC6x12[4][5];
1286 
1287  // *********************************************************************************
1288  // calculate the Kalman gain matrix K
1289  // K = P- * C^T * inv(C * P- * C^T + Qv) = Qw * C^T * inv(C * Qw * C^T + Qv)
1290  // Qw is used as a proxy for P- throughout the code
1291  // P+ is used here as a working array to reduce RAM usage and is re-computed later
1292  // *********************************************************************************
1293 
1294  // set ftmpA12x6 = P- * C^T = Qw * C^T where Qw and C are both sparse
1295  // C also has a significant number of +1 and -1 entries
1296  // ftmpA12x6 is also sparse but not symmetric
1297  for (i = 0; i < 12; i++) // loop over rows of ftmpA12x6
1298  {
1299  // initialize pftmpA12x6ij for current i, j=0
1300  pftmpA12x6ij = ftmpA12x6[i];
1301 
1302  for (j = 0; j < 6; j++) // loop over columns of ftmpA12x6
1303  {
1304  // zero ftmpA12x6[i][j]
1305  *pftmpA12x6ij = 0.0F;
1306 
1307  // initialize pfC6x12jk for current j, k=0
1308  pfC6x12jk = pthisSV->fC6x12[j];
1309 
1310  // initialize pfQw12x12ik for current i, k=0
1311  pfQw12x12ik = pthisSV->fQw12x12[i];
1312 
1313  // sum matrix products over inner loop over k
1314  for (k = 0; k < 12; k++)
1315  {
1316  if ((*pfQw12x12ik != 0.0F) && (*pfC6x12jk != 0.0F))
1317  {
1318  if (*pfC6x12jk == 1.0F)
1319  *pftmpA12x6ij += *pfQw12x12ik;
1320  else if (*pfC6x12jk == -1.0F)
1321  *pftmpA12x6ij -= *pfQw12x12ik;
1322  else
1323  *pftmpA12x6ij += *pfQw12x12ik * *pfC6x12jk;
1324  }
1325 
1326  // increment pfC6x12jk and pfQw12x12ik for next iteration of k
1327  pfC6x12jk++;
1328  pfQw12x12ik++;
1329 
1330  } // end of loop over k
1331 
1332  // increment pftmpA12x6ij for next iteration of j
1333  pftmpA12x6ij++;
1334 
1335  } // end of loop over j
1336  } // end of loop over i
1337 
1338  // set symmetric P+ (6x6 scratch sub-matrix) to C * P- * C^T + Qv
1339  // = C * (Qw * C^T) + Qv = C * ftmpA12x6 + Qv
1340  // both C and ftmpA12x6 are sparse but not symmetric
1341  for (i = 0; i < 6; i++) // loop over rows of P+
1342  {
1343  // initialize pfPPlus12x12ij for current i, j=i
1344  pfPPlus12x12ij = pthisSV->fPPlus12x12[i] + i;
1345 
1346  for (j = i; j < 6; j++) // loop over above diagonal columns of P+
1347  {
1348  // zero P+[i][j]
1349  *pfPPlus12x12ij = 0.0F;
1350 
1351  // initialize pfC6x12ik for current i, k=0
1352  pfC6x12ik = pthisSV->fC6x12[i];
1353 
1354  // initialize pftmpA12x6kj for current j, k=0
1355  pftmpA12x6kj = *ftmpA12x6 + j;
1356 
1357  // sum matrix products over inner loop over k
1358  for (k = 0; k < 12; k++)
1359  {
1360  if ((*pfC6x12ik != 0.0F) && (*pftmpA12x6kj != 0.0F))
1361  {
1362  if (*pfC6x12ik == 1.0F)
1363  *pfPPlus12x12ij += *pftmpA12x6kj;
1364  else if (*pfC6x12ik == -1.0F)
1365  *pfPPlus12x12ij -= *pftmpA12x6kj;
1366  else
1367  *pfPPlus12x12ij += *pfC6x12ik * *pftmpA12x6kj;
1368  }
1369 
1370  // update pfC6x12ik and pftmpA12x6kj for next iteration of k
1371  pfC6x12ik++;
1372  pftmpA12x6kj += 6;
1373 
1374  } // end of loop over k
1375 
1376  // increment pfPPlus12x12ij for next iteration of j
1377  pfPPlus12x12ij++;
1378 
1379  } // end of loop over j
1380  } // end of loop over i
1381 
1382  // add in noise covariance terms to the diagonal
1383  pthisSV->fPPlus12x12[0][0] += pthisSV->fQvAA;
1384  pthisSV->fPPlus12x12[1][1] += pthisSV->fQvAA;
1385  pthisSV->fPPlus12x12[2][2] += pthisSV->fQvAA;
1386  pthisSV->fPPlus12x12[3][3] += pthisSV->fQvMM;
1387  pthisSV->fPPlus12x12[4][4] += pthisSV->fQvMM;
1388  pthisSV->fPPlus12x12[5][5] += pthisSV->fQvMM;
1389 
1390  // copy above diagonal elements of P+ (6x6 scratch sub-matrix) to below diagonal
1391  for (i = 1; i < 6; i++)
1392  for (j = 0; j < i; j++)
1393  pthisSV->fPPlus12x12[i][j] = pthisSV->fPPlus12x12[j][i];
1394 
1395  // calculate inverse of P+ (6x6 scratch sub-matrix) = inv(C * P- * C^T + Qv) = inv(C * Qw * C^T + Qv)
1396  for (i = 0; i < 6; i++)
1397  {
1398  pfRows[i] = pthisSV->fPPlus12x12[i];
1399  }
1400  fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 3);
1401 
1402  // set K = P- * C^T * inv(C * P- * C^T + Qv) = Qw * C^T * inv(C * Qw * C^T + Qv)
1403  // = ftmpA12x6 * P+ (6x6 sub-matrix)
1404  // ftmpA12x6 = Qw * C^T is sparse but P+ (6x6 sub-matrix) is not
1405  // K is not symmetric because C is not symmetric
1406  for (i = 0; i < 12; i++) // loop over rows of K12x6
1407  {
1408  // initialize pfK12x6ij for current i, j=0
1409  pfK12x6ij = pthisSV->fK12x6[i];
1410 
1411  for (j = 0; j < 6; j++) // loop over columns of K12x6
1412  {
1413  // zero the matrix element fK12x6[i][j]
1414  *pfK12x6ij = 0.0F;
1415 
1416  // initialize pftmpA12x6ik for current i, k=0
1417  pftmpA12x6ik = ftmpA12x6[i];
1418 
1419  // initialize pfPPlus12x12kj for current j, k=0
1420  pfPPlus12x12kj = *pthisSV->fPPlus12x12 + j;
1421 
1422  // sum matrix products over inner loop over k
1423  for (k = 0; k < 6; k++)
1424  {
1425  if (*pftmpA12x6ik != 0.0F)
1426  {
1427  *pfK12x6ij += *pftmpA12x6ik * *pfPPlus12x12kj;
1428  }
1429 
1430  // increment pftmpA12x6ik and pfPPlus12x12kj for next iteration of k
1431  pftmpA12x6ik++;
1432  pfPPlus12x12kj += 12;
1433 
1434  } // end of loop over k
1435 
1436  // increment pfK12x6ij for the next iteration of j
1437  pfK12x6ij++;
1438 
1439  } // end of loop over j
1440  } // end of loop over i
1441 
1442  // *********************************************************************************
1443  // calculate a posteriori error estimate: xe+ = K * ze-
1444  // *********************************************************************************
1445 
1446  // first calculate all four error vector components using accelerometer error component only
1447  // for fThErrPl, fbErrPl, faErrSePl but also magnetometer for fdErrSePl
1448  for (i = X; i <= Z; i++)
1449  {
1450  pthisSV->fThErrPl[i] = pthisSV->fK12x6[i][0] * pthisSV->fgErrSeMi[X] +
1451  pthisSV->fK12x6[i][1] * pthisSV->fgErrSeMi[Y] +
1452  pthisSV->fK12x6[i][2] * pthisSV->fgErrSeMi[Z];
1453  pthisSV->fbErrPl[i] = pthisSV->fK12x6[i + 3][0] * pthisSV->fgErrSeMi[X] +
1454  pthisSV->fK12x6[i + 3][1] * pthisSV->fgErrSeMi[Y] +
1455  pthisSV->fK12x6[i + 3][2] * pthisSV->fgErrSeMi[Z];
1456  pthisSV->faErrSePl[i] = pthisSV->fK12x6[i + 6][0] * pthisSV->fgErrSeMi[X] +
1457  pthisSV->fK12x6[i + 6][1] * pthisSV->fgErrSeMi[Y] +
1458  pthisSV->fK12x6[i + 6][2] * pthisSV->fgErrSeMi[Z];
1459  pthisSV->fdErrSePl[i] = pthisSV->fK12x6[i + 9][0] * pthisSV->fgErrSeMi[X] +
1460  pthisSV->fK12x6[i + 9][1] * pthisSV->fgErrSeMi[Y] +
1461  pthisSV->fK12x6[i + 9][2] * pthisSV->fgErrSeMi[Z] +
1462  pthisSV->fK12x6[i + 9][3] * pthisSV->fmErrSeMi[X] +
1463  pthisSV->fK12x6[i + 9][4] * pthisSV->fmErrSeMi[Y] +
1464  pthisSV->fK12x6[i + 9][5] * pthisSV->fmErrSeMi[Z];
1465  }
1466 
1467  // set the magnetic jamming flag if there is a significant magnetic error power after calibration
1468  ftmp = pthisSV->fdErrSePl[X] * pthisSV->fdErrSePl[X] + pthisSV->fdErrSePl[Y] * pthisSV->fdErrSePl[Y] +
1469  pthisSV->fdErrSePl[Z] * pthisSV->fdErrSePl[Z];
1470  iMagJamming = (pthisMagCal->iValidMagCal) && (ftmp > pthisMagCal->fFourBsq);
1471 
1472  // add the remaining magnetic error terms if there is calibration and no magnetic jamming
1473  if (pthisMagCal->iValidMagCal && !iMagJamming)
1474  {
1475  for (i = X; i <= Z; i++)
1476  {
1477  pthisSV->fThErrPl[i] += pthisSV->fK12x6[i][3] * pthisSV->fmErrSeMi[X] +
1478  pthisSV->fK12x6[i][4] * pthisSV->fmErrSeMi[Y] +
1479  pthisSV->fK12x6[i][5] * pthisSV->fmErrSeMi[Z];
1480  pthisSV->fbErrPl[i] += pthisSV->fK12x6[i + 3][3] * pthisSV->fmErrSeMi[X] +
1481  pthisSV->fK12x6[i + 3][4] * pthisSV->fmErrSeMi[Y] +
1482  pthisSV->fK12x6[i + 3][5] * pthisSV->fmErrSeMi[Z];
1483  pthisSV->faErrSePl[i] += pthisSV->fK12x6[i + 6][3] * pthisSV->fmErrSeMi[X] +
1484  pthisSV->fK12x6[i + 6][4] * pthisSV->fmErrSeMi[Y] +
1485  pthisSV->fK12x6[i + 6][5] * pthisSV->fmErrSeMi[Z];
1486  }
1487  }
1488 
1489  // *********************************************************************************
1490  // apply the a posteriori error corrections to the a posteriori state vector
1491  // *********************************************************************************
1492 
1493  // get the a posteriori delta quaternion
1494  fQuaternionFromRotationVectorDeg(&(pthisSV->fDeltaq), pthisSV->fThErrPl, -1.0F);
1495 
1496  // compute the a posteriori orientation quaternion fqPl = fqMi * Deltaq(-thetae+)
1497  // the resulting quaternion may have negative scalar component q0
1498  qAeqBxC(&(pthisSV->fqPl), &(pthisSV->fqMi), &(pthisSV->fDeltaq));
1499 
1500  // normalize the a posteriori orientation quaternion to stop error propagation
1501  // the renormalization function ensures that the scalar component q0 is non-negative
1502  fqAeqNormqA(&(pthisSV->fqPl));
1503 
1504  // compute the a posteriori rotation matrix from the a posteriori quaternion
1505  fRotationMatrixFromQuaternion(pthisSV->fRPl, &(pthisSV->fqPl));
1506 
1507  // compute the rotation vector from the a posteriori quaternion
1508  fRotationVectorDegFromQuaternion(&(pthisSV->fqPl), pthisSV->fRVecPl);
1509 
1510  // update the a posteriori gyro offset vector b+ and
1511  // assign the entire linear acceleration error vector to update the linear acceleration
1512  for (i = X; i <= Z; i++)
1513  {
1514  // b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s)
1515  pthisSV->fbPl[i] -= pthisSV->fbErrPl[i];
1516  // a+ = a- - ae+ (g, sensor frame)
1517  pthisSV->faSePl[i] = pthisSV->faSeMi[i] - pthisSV->faErrSePl[i];
1518  }
1519 
1520  // compute the angular velocity for display purposes only: omega[k] = yG[k] - b+[k] (deg/s)
1521  for (i = X; i <= Z; i++)
1522  {
1523  pthisSV->fOmega[i] = pthisGyro->fYpBuffer[iOverSampleRatio - 1][i] - pthisSV->fbPl[i];
1524  }
1525 
1526  // compute the linear acceleration in the global frame from the accelerometer measurement (sensor frame).
1527  // de-rotate the accelerometer measurement from the sensor to global frame using the inverse (transpose)
1528  // of the a posteriori rotation matrix
1529  pthisSV->faGlPl[X] = pthisSV->fRPl[X][X] * pthisAccel->fGp[X] + pthisSV->fRPl[Y][X] * pthisAccel->fGp[Y] +
1530  pthisSV->fRPl[Z][X] * pthisAccel->fGp[Z];
1531  pthisSV->faGlPl[Y] = pthisSV->fRPl[X][Y] * pthisAccel->fGp[X] + pthisSV->fRPl[Y][Y] * pthisAccel->fGp[Y] +
1532  pthisSV->fRPl[Z][Y] * pthisAccel->fGp[Z];
1533  pthisSV->faGlPl[Z] = pthisSV->fRPl[X][Z] * pthisAccel->fGp[X] + pthisSV->fRPl[Y][Z] * pthisAccel->fGp[Y] +
1534  pthisSV->fRPl[Z][Z] * pthisAccel->fGp[Z];
1535  // remove gravity and correct the sign if the coordinate system is gravity positive / acceleration negative
1536  switch (ithisCoordSystem)
1537  {
1538  case NED:
1539  // gravity positive NED
1540  pthisSV->faGlPl[X] = -pthisSV->faGlPl[X];
1541  pthisSV->faGlPl[Y] = -pthisSV->faGlPl[Y];
1542  pthisSV->faGlPl[Z] = -(pthisSV->faGlPl[Z] - 1.0F);
1543  break;
1544  case ANDROID:
1545  // acceleration positive ENU
1546  pthisSV->faGlPl[X] = pthisSV->faGlPl[X];
1547  pthisSV->faGlPl[Y] = pthisSV->faGlPl[Y];
1548  pthisSV->faGlPl[Z] = pthisSV->faGlPl[Z] - 1.0F;
1549  break;
1550  case WIN8:
1551  default:
1552  // gravity positive ENU
1553  pthisSV->faGlPl[X] = -pthisSV->faGlPl[X];
1554  pthisSV->faGlPl[Y] = -pthisSV->faGlPl[Y];
1555  pthisSV->faGlPl[Z] = -(pthisSV->faGlPl[Z] + 1.0F);
1556  break;
1557  }
1558 
1559  // update the reference geomagnetic vector using magnetic disturbance error if valid calibration and no jamming
1560  if (pthisMagCal->iValidMagCal && !iMagJamming)
1561  {
1562  if (ithisCoordSystem == NED)
1563  {
1564  // de-rotate the NED magnetic disturbance error de+ from the sensor to the global reference frame
1565  // using the inverse (transpose) of the a posteriori rotation matrix
1566  pthisSV->fdErrGlPl[X] = pthisSV->fRPl[X][X] * pthisSV->fdErrSePl[X] + pthisSV->fRPl[Y][X] * pthisSV->fdErrSePl[Y] +
1567  pthisSV->fRPl[Z][X] * pthisSV->fdErrSePl[Z];
1568  pthisSV->fdErrGlPl[Z] = pthisSV->fRPl[X][Z] * pthisSV->fdErrSePl[X] + pthisSV->fRPl[Y][Z] * pthisSV->fdErrSePl[Y] +
1569  pthisSV->fRPl[Z][Z] * pthisSV->fdErrSePl[Z];
1570 
1571  // compute components of the new geomagnetic vector
1572  // the north pointing component fadj must always be non-negative
1573  fopp = pthisSV->fmGl[Z] - pthisSV->fdErrGlPl[Z];
1574  fadj = pthisSV->fmGl[X] - pthisSV->fdErrGlPl[X];
1575  if (fadj < 0.0F)
1576  {
1577  fadj = 0.0F;
1578  }
1579  fhyp = sqrtf(fopp * fopp + fadj * fadj);
1580 
1581  // check for the pathological condition of zero geomagnetic field
1582  if (fhyp != 0.0F)
1583  {
1584  // compute the sine and cosine of the new geomagnetic vector
1585  ftmp = 1.0F / fhyp;
1586  fsindelta = fopp * ftmp;
1587  fcosdelta = fadj * ftmp;
1588 
1589  // limit the inclination angle between limits to prevent runaway
1590  if (fsindelta > SINDELTAMAX)
1591  {
1592  fsindelta = SINDELTAMAX;
1593  fcosdelta = COSDELTAMAX;
1594  }
1595  else if (fsindelta < -SINDELTAMAX)
1596  {
1597  fsindelta = -SINDELTAMAX;
1598  fcosdelta = COSDELTAMAX;
1599  }
1600 
1601  // compute the new geomagnetic vector (always north pointing)
1602  pthisSV->fDeltaPl = fasin_deg(fsindelta);
1603  pthisSV->fmGl[X] = pthisMagCal->fB * fcosdelta;
1604  pthisSV->fmGl[Z] = pthisMagCal->fB * fsindelta;
1605  } // end hyp == 0.0F
1606  } // end NED
1607  else
1608  {
1609  // de-rotate the Android and Windows 8 magnetic disturbance error de+ from the sensor to the global reference frame
1610  // using the inverse (transpose) of the a posteriori rotation matrix
1611  pthisSV->fdErrGlPl[Y] = pthisSV->fRPl[X][Y] * pthisSV->fdErrSePl[X] + pthisSV->fRPl[Y][Y] * pthisSV->fdErrSePl[Y] +
1612  pthisSV->fRPl[Z][Y] * pthisSV->fdErrSePl[Z];
1613  pthisSV->fdErrGlPl[Z] = pthisSV->fRPl[X][Z] * pthisSV->fdErrSePl[X] + pthisSV->fRPl[Y][Z] * pthisSV->fdErrSePl[Y] +
1614  pthisSV->fRPl[Z][Z] * pthisSV->fdErrSePl[Z];
1615 
1616  // compute components of the new geomagnetic vector
1617  // the north pointing component fadj must always be non-negative
1618  fopp = -(pthisSV->fmGl[Z] - pthisSV->fdErrGlPl[Z]);
1619  fadj = pthisSV->fmGl[Y] - pthisSV->fdErrGlPl[Y];
1620  if (fadj < 0.0F)
1621  {
1622  fadj = 0.0F;
1623  }
1624  fhyp = sqrtf(fopp * fopp + fadj * fadj);
1625 
1626  // check for the pathological condition of zero geomagnetic field
1627  if (fhyp != 0.0F)
1628  {
1629  // compute the sine and cosine of the new geomagnetic vector
1630  ftmp = 1.0F / fhyp;
1631  fsindelta = fopp * ftmp;
1632  fcosdelta = fadj * ftmp;
1633 
1634  // limit the inclination angle between limits to prevent runaway
1635  if (fsindelta > SINDELTAMAX)
1636  {
1637  fsindelta = SINDELTAMAX;
1638  fcosdelta = COSDELTAMAX;
1639  }
1640  else if (fsindelta < -SINDELTAMAX)
1641  {
1642  fsindelta = -SINDELTAMAX;
1643  fcosdelta = COSDELTAMAX;
1644  }
1645 
1646  // compute the new geomagnetic vector (always north pointing)
1647  pthisSV->fDeltaPl = fasin_deg(fsindelta);
1648  pthisSV->fmGl[Y] = pthisMagCal->fB * fcosdelta;
1649  pthisSV->fmGl[Z] = -pthisMagCal->fB * fsindelta;
1650  } // end hyp == 0.0F
1651  } // end Android and Win8
1652  } // end iValidMagCal
1653 
1654  // *********************************************************************************
1655  // compute the a posteriori Euler angles from the orientation matrix
1656  // *********************************************************************************
1657 
1658  if (ithisCoordSystem == NED)
1659  {
1660  // calculate the NED Euler angles
1661  fNEDAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl),
1662  &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
1663  }
1664  else if (ithisCoordSystem == ANDROID)
1665  {
1666  // calculate the Android Euler angles
1667  fAndroidAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl),
1668  &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
1669  }
1670  else
1671  {
1672  // calculate Win8 Euler angles
1673  fWin8AnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl), &(pthisSV->fThePl), &(pthisSV->fPsiPl),
1674  &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
1675  }
1676 
1677  // ***********************************************************************************
1678  // calculate (symmetric) a posteriori error covariance matrix P+
1679  // P+ = (I12 - K * C) * P- = (I12 - K * C) * Qw = Qw - K * (C * Qw)
1680  // both Qw and P+ are used as working arrays in this section
1681  // at the end of this section, P+ is valid but Qw is over-written
1682  // ***********************************************************************************
1683 
1684  // set P+ (6x12 scratch sub-matrix) to the product C (6x12) * Qw (12x12)
1685  // where both C and Qw are sparse and C has a significant number of +1 and -1 entries
1686  // the resulting matrix is sparse but not symmetric
1687  for (i = 0; i < 6; i++)
1688  {
1689  // initialize pfPPlus12x12ij for current i, j=0
1690  pfPPlus12x12ij = pthisSV->fPPlus12x12[i];
1691 
1692  for (j = 0; j < 12; j++)
1693  {
1694  // zero P+[i][j]
1695  *pfPPlus12x12ij = 0.0F;
1696 
1697  // initialize pfC6x12ik for current i, k=0
1698  pfC6x12ik = pthisSV->fC6x12[i];
1699 
1700  // initialize pfQw12x12kj for current j, k=0
1701  pfQw12x12kj = &pthisSV->fQw12x12[0][j];
1702 
1703  // sum matrix products over inner loop over k
1704  for (k = 0; k < 12; k++)
1705  {
1706  if ((*pfC6x12ik != 0.0F) && (*pfQw12x12kj != 0.0F))
1707  {
1708  if (*pfC6x12ik == 1.0F)
1709  *pfPPlus12x12ij += *pfQw12x12kj;
1710  else if (*pfC6x12ik == -1.0F)
1711  *pfPPlus12x12ij -= *pfQw12x12kj;
1712  else
1713  *pfPPlus12x12ij += *pfC6x12ik * *pfQw12x12kj;
1714  }
1715 
1716  // update pfC6x12ik and pfQw12x12kj for next iteration of k
1717  pfC6x12ik++;
1718  pfQw12x12kj += 12;
1719 
1720  } // end of loop over k
1721 
1722  // increment pfPPlus12x12ij for next iteration of j
1723  pfPPlus12x12ij++;
1724 
1725  } // end of loop over j
1726  } // end of loop over i
1727 
1728  // compute P+ = (I12 - K * C) * Qw = Qw - K * (C * Qw) = Qw - K * P+ (6x12 sub-matrix)
1729  // storing result P+ in Qw and over-writing Qw which is OK since Qw is later computed from P+
1730  // where working array P+ (6x12 sub-matrix) is sparse but K is not sparse
1731  // only on and above diagonal terms of P+ are computed since P+ is symmetric
1732  for (i = 0; i < 12; i++)
1733  {
1734  // initialize pfQw12x12ij for current i, j=i
1735  pfQw12x12ij = pthisSV->fQw12x12[i] + i;
1736 
1737  for (j = i; j < 12; j++)
1738  {
1739  // initialize pfK12x6ik for current i, k=0
1740  pfK12x6ik = pthisSV->fK12x6[i];
1741 
1742  // initialize pfPPlus12x12kj for current j, k=0
1743  pfPPlus12x12kj = *pthisSV->fPPlus12x12 + j;
1744 
1745  // compute on and above diagonal matrix entry
1746  for (k = 0; k < 6; k++)
1747  {
1748  // check for non-zero values since P+ (6x12 scratch sub-matrix) is sparse
1749  if (*pfPPlus12x12kj != 0.0F)
1750  {
1751  *pfQw12x12ij -= *pfK12x6ik * *pfPPlus12x12kj;
1752  }
1753 
1754  // increment pfK12x6ik and pfPPlus12x12kj for next iteration of k
1755  pfK12x6ik++;
1756  pfPPlus12x12kj += 12;
1757 
1758  } // end of loop over k
1759 
1760  // increment pfQw12x12ij for next iteration of j
1761  pfQw12x12ij++;
1762 
1763  } // end of loop over j
1764  } // end of loop over i
1765 
1766  // Qw now holds the on and above diagonal elements of P+
1767  // so perform a simple copy to the all elements of P+
1768  // after execution of this code P+ is valid but Qw remains invalid
1769  for (i = 0; i < 12; i++)
1770  {
1771  // initialize pfPPlus12x12ij and pfQw12x12ij for current i, j=i
1772  pfPPlus12x12ij = pthisSV->fPPlus12x12[i] + i;
1773  pfQw12x12ij = pthisSV->fQw12x12[i] + i;
1774 
1775  // copy the on-diagonal elements and increment pointers to enter loop at j=i+1
1776  *(pfPPlus12x12ij++) = *(pfQw12x12ij++);
1777 
1778  // loop over above diagonal columns j copying to below-diagonal elements
1779  for (j = i + 1; j < 12; j++)
1780  {
1781  *(pfPPlus12x12ij++)= pthisSV->fPPlus12x12[j][i] = *(pfQw12x12ij++);
1782  }
1783  }
1784 
1785  // *********************************************************************************
1786  // re-create the noise covariance matrix Qw=fn(P+) for the next iteration
1787  // using the elements of P+ which are now valid
1788  // Qw was over-written earlier but is here recomputed (all elements)
1789  // *********************************************************************************
1790 
1791  // zero the matrix Qw
1792  for (i = 0; i < 12; i++)
1793  {
1794  for (j = 0; j < 12; j++)
1795  {
1796  pthisSV->fQw12x12[i][j] = 0.0F;
1797  }
1798  }
1799 
1800  // update the covariance matrix components
1801  for (i = 0; i < 3; i++)
1802  {
1803  // Qw[th-th-] = Qw[0-2][0-2] = E[th-(th-)^T] = Q[th+th+] + deltat^2 * (Q[b+b+] + (Qwb + QvG) * I)
1804  pthisSV->fQw12x12[i][i] = pthisSV->fPPlus12x12[i][i] + pthisSV->fdeltatsq * (pthisSV->fPPlus12x12[i + 3][i + 3] + pthisSV->fQwbplusQvG);
1805 
1806  // Qw[b-b-] = Qw[3-5][3-5] = E[b-(b-)^T] = Q[b+b+] + Qwb * I
1807  pthisSV->fQw12x12[i + 3][i + 3] = pthisSV->fPPlus12x12[i + 3][i + 3] + FQWB_9DOF_GBY_KALMAN;
1808 
1809  // Qw[th-b-] = Qw[0-2][3-5] = E[th-(b-)^T] = -deltat * (Q[b+b+] + Qwb * I) = -deltat * Qw[b-b-]
1810  pthisSV->fQw12x12[i][i + 3] = pthisSV->fQw12x12[i + 3][i] = -pthisSV->fdeltat * pthisSV->fQw12x12[i + 3][i + 3];
1811 
1812  // Qw[a-a-] = Qw[6-8][6-8] = E[a-(a-)^T] = ca^2 * Q[a+a+] + Qwa * I
1813  pthisSV->fQw12x12[i + 6][i + 6] = pthisSV->fcasq * pthisSV->fPPlus12x12[i + 6][i + 6] + FQWA_9DOF_GBY_KALMAN;
1814 
1815  // Qw[d-d-] = Qw[9-11][9-11] = E[d-(d-)^T] = cd^2 * Q[d+d+] + Qwd * I
1816  pthisSV->fQw12x12[i + 9][i + 9] = pthisSV->fcdsq * pthisSV->fPPlus12x12[i + 9][i + 9] + FQWD_9DOF_GBY_KALMAN;
1817  }
1818 
1819  return;
1820 } // end fRun_9DOF_GBY_KALMAN
1821 
1822 
#define FDEGTORAD
int16 iFirstOrientationLock
Definition: fusion_types.h:197
float fLPRVec[3]
Definition: fusion_types.h:86
#define FQWINITBB_9DOF_GBY_KALMAN
Definition: fusion.c:69
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
Definition: orientation.c:197
void f3DOFTiltNED(float fR[][3], float fGp[])
Definition: orientation.c:48
#define FCD_9DOF_GBY_KALMAN
Definition: fusion.c:75
struct fquaternion fLPq
Definition: fusion_types.h:59
float fRMi[3][3]
Definition: fusion_types.h:179
float fasin_deg(float x)
#define WIN8
Definition: fusion_types.h:16
#define FQWA_9DOF_GBY_KALMAN
Definition: fusion.c:65
float fR[3][3]
Definition: fusion_types.h:143
#define SINDELTAMAX
Definition: fusion.c:77
struct fquaternion fqMi
Definition: fusion_types.h:247
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:520
#define SENSORFS
Definition: fusion_config.h:21
float fPPlus9x9[9][9]
Definition: fusion_types.h:188
float fC6x12[6][12]
Definition: fusion_types.h:244
#define FQWB_9DOF_GBY_KALMAN
Definition: fusion.c:64
void fInit_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:97
float fRPl[3][3]
Definition: fusion_types.h:163
float fRPl[3][3]
Definition: fusion_types.h:212
struct fquaternion fDeltaq
Definition: fusion_types.h:246
void fRun_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal, int16 ithisCoordSystem, int16 iOverSampleRatio)
Definition: fusion.c:1110
void fInit_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:148
float fQw12x12[12][12]
Definition: fusion_types.h:243
struct fquaternion fqPl
Definition: fusion_types.h:213
void fInit_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:131
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:466
#define FQWINITDD_9DOF_GBY_KALMAN
Definition: fusion.c:72
struct fquaternion fDeltaq
Definition: fusion_types.h:181
float fLPRVec[3]
Definition: fusion_types.h:60
struct fquaternion fq
Definition: fusion_types.h:93
void fqAeq1(struct fquaternion *pqA)
Definition: orientation.c:1091
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:403
#define FQVM_9DOF_GBY_KALMAN
Definition: fusion.c:62
#define FQWD_9DOF_GBY_KALMAN
Definition: fusion.c:66
#define FQWINITTHTH_6DOF_GY_KALMAN
Definition: fusion.c:50
#define GYRO_OVERSAMPLE_RATIO
Definition: fusion_config.h:24
void fInit_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:114
Definition: matrix.h:33
signed char int8
Definition: basic_types.h:12
struct fquaternion fq
Definition: fusion_types.h:67
struct fquaternion fLPq
Definition: fusion_types.h:85
#define FQWINITTHTH_9DOF_GBY_KALMAN
Definition: fusion.c:68
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
Definition: orientation.c:253
#define ANDROID
Definition: fusion_types.h:15
int8 iValidMagCal
Definition: magnetic.h:61
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
Definition: orientation.c:882
float fLPR[3][3]
Definition: fusion_types.h:84
struct fquaternion fqPl
Definition: fusion_types.h:164
#define MAG_OVERSAMPLE_RATIO
Definition: fusion_config.h:23
void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro, int16 ithisCoordSystem, int16 iOverSampleRatio)
Definition: fusion.c:593
#define FQVA_9DOF_GBY_KALMAN
Definition: fusion.c:61
float fR[3][3]
Definition: fusion_types.h:110
float fR[3][3]
Definition: fusion_types.h:66
float fLPR[3][3]
Definition: fusion_types.h:135
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[])
Definition: orientation.c:262
void fInit_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, int16 ithisCoordSystem, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:234
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
Definition: orientation.c:1015
#define FQVG_9DOF_GBY_KALMAN
Definition: fusion.c:63
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro, int32 loopcounter, int16 ithisCoordSystem, int16 iOverSampleRatio)
Definition: fusion.c:461
#define FQWINITAA_9DOF_GBY_KALMAN
Definition: fusion.c:71
float fR[3][3]
Definition: fusion_types.h:92
#define FQWA_6DOF_GY_KALMAN
Definition: fusion.c:48
void fInit_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:81
float fYpBuffer[GYRO_OVERSAMPLE_RATIO][3]
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGp[])
Definition: orientation.c:330
#define FCA_6DOF_GY_KALMAN
Definition: fusion.c:55
void fqAeqNormqA(struct fquaternion *pqA)
Definition: orientation.c:1056
void f3DOFTiltAndroid(float fR[][3], float fGp[])
Definition: orientation.c:114
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
Definition: orientation.c:666
float fFourBsq
Definition: magnetic.h:47
#define THISCOORDSYSTEM
Definition: fusion_config.h:37
float fK12x6[12][6]
Definition: fusion_types.h:242
float fC3x9[3][9]
Definition: fusion_types.h:191
struct fquaternion fq
Definition: fusion_types.h:144
float fOmega[3]
Definition: fusion_types.h:88
float fLPR[3][3]
Definition: fusion_types.h:58
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
Definition: orientation.c:1026
struct fquaternion fq
Definition: fusion_types.h:111
void fInit_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:166
#define FQWINITTHB_6DOF_GY_KALMAN
Definition: fusion.c:52
#define FQWB_6DOF_GY_KALMAN
Definition: fusion.c:47
#define FQVG_6DOF_GY_KALMAN
Definition: fusion.c:46
long int32
This defines int32 as long.
Definition: isf_types.h:32
#define COSDELTAMAX
Definition: fusion.c:78
float fRMi[3][3]
Definition: fusion_types.h:245
float fOmega[3]
Definition: fusion_types.h:62
Definition: matrix.h:34
float fQw9x9[9][9]
Definition: fusion_types.h:190
short int16
This defines int16 as short.
Definition: isf_types.h:23
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
float fBc[3]
#define NED
Definition: fusion_types.h:14
struct fquaternion fqMi
Definition: fusion_types.h:180
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:340
Definition: matrix.h:35
float fPPlus12x12[12][12]
Definition: fusion_types.h:241
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[], int32 loopcounter)
Definition: orientation.c:932
#define FQWINITBB_6DOF_GY_KALMAN
Definition: fusion.c:51
#define ACCEL_OVERSAMPLE_RATIO
Definition: fusion_config.h:22
struct fquaternion fDeltaq
Definition: fusion_types.h:118
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
Definition: orientation.c:775
#define FQWINITTHB_9DOF_GBY_KALMAN
Definition: fusion.c:70
#define DEFAULTB
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, int32 loopcounter)
Definition: fusion.c:323
#define FCA_9DOF_GBY_KALMAN
Definition: fusion.c:74
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)
Definition: matrix.c:325
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
Definition: orientation.c:734
#define FQWINITAA_6DOF_GY_KALMAN
Definition: fusion.c:53
#define FQVA_6DOF_GY_KALMAN
Definition: fusion.c:45
void fLPFScalar(float *pfS, float *pfLPS, float flpf, int32 loopcounter)
Definition: orientation.c:1000
void f3DOFTiltWin8(float fR[][3], float fGp[])
Definition: orientation.c:123
void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:532
float fK9x3[9][3]
Definition: fusion_types.h:189
struct fquaternion fLPq
Definition: fusion_types.h:136
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
Definition: orientation.c:225
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGp[])
Definition: orientation.c:398
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:575