| ISF 2.1 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

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;
177 pthisSV->fcasq = FCA_6DOF_GY_KALMAN * FCA_6DOF_GY_KALMAN;
178 pthisSV->fQwbplusQvG = FQWB_6DOF_GY_KALMAN + FQVG_6DOF_GY_KALMAN;
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
200 pthisSV->fQvAA = FQVA_6DOF_GY_KALMAN + FQWA_6DOF_GY_KALMAN +
201 FDEGTORAD * FDEGTORAD * pthisSV->fdeltatsq * (FQWB_6DOF_GY_KALMAN + FQVG_6DOF_GY_KALMAN);
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;
245 pthisSV->fcasq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN;
246 pthisSV->fcdsq = FCD_9DOF_GBY_KALMAN * FCD_9DOF_GBY_KALMAN;
247 pthisSV->fQwbplusQvG = FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN;
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
287 pthisSV->fQvAA = FQVA_9DOF_GBY_KALMAN + FQWA_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * pthisSV->fdeltatsq * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
288 pthisSV->fQvMM = FQVM_9DOF_GBY_KALMAN + FQWD_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * pthisSV->fdeltatsq * DEFAULTB * DEFAULTB * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
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 {
328 fInit_1DOF_P_BASIC(pthisSV, 1.5F, SENSORFS, GYRO_OVERSAMPLE_RATIO);
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
FDEGTORAD
#define FDEGTORAD
Definition: math_constants.h:13
SV_6DOF_GY_KALMAN::iFirstOrientationLock
int16 iFirstOrientationLock
Definition: fusion_types.h:197
SV_3DOF_B_BASIC::fLPRVec
float fLPRVec[3]
Definition: fusion_types.h:86
SV_9DOF_GBY_KALMAN::fDeltaPl
float fDeltaPl
Definition: fusion_types.h:231
FQWINITBB_9DOF_GBY_KALMAN
#define FQWINITBB_9DOF_GBY_KALMAN
Definition: fusion.c:69
SV_3DOF_Y_BASIC::fChi
float fChi
Definition: fusion_types.h:108
f3DOFMagnetometerMatrixNED
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
Definition: orientation.c:197
SV_9DOF_GBY_KALMAN::fThePl
float fThePl
Definition: fusion_types.h:207
SV_3DOF_Y_BASIC::fOmega
float fOmega[3]
Definition: fusion_types.h:114
f3DOFTiltNED
void f3DOFTiltNED(float fR[][3], float fGp[])
Definition: orientation.c:48
FCD_9DOF_GBY_KALMAN
#define FCD_9DOF_GBY_KALMAN
Definition: fusion.c:75
SV_3DOF_G_BASIC::fLPq
struct fquaternion fLPq
Definition: fusion_types.h:59
SV_6DOF_GY_KALMAN::fRMi
float fRMi[3][3]
Definition: fusion_types.h:179
fasin_deg
float fasin_deg(float x)
Definition: approximations.c:37
WIN8
#define WIN8
Definition: fusion_types.h:16
FQWA_9DOF_GBY_KALMAN
#define FQWA_9DOF_GBY_KALMAN
Definition: fusion.c:65
SV_6DOF_GB_BASIC::fR
float fR[3][3]
Definition: fusion_types.h:143
SINDELTAMAX
#define SINDELTAMAX
Definition: fusion.c:77
SV_3DOF_B_BASIC::fdeltat
float fdeltat
Definition: fusion_types.h:94
SV_9DOF_GBY_KALMAN::fqMi
struct fquaternion fqMi
Definition: fusion_types.h:247
SV_9DOF_GBY_KALMAN::fRVecPl
float fRVecPl[3]
Definition: fusion_types.h:214
SV_9DOF_GBY_KALMAN::fcasq
float fcasq
Definition: fusion_types.h:248
fAndroidAnglesDegFromRotationMatrix
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:520
SENSORFS
#define SENSORFS
Definition: fusion_config.h:21
SV_6DOF_GY_KALMAN::fFastdeltat
float fFastdeltat
Definition: fusion_types.h:193
SV_6DOF_GY_KALMAN::fPPlus9x9
float fPPlus9x9[9][9]
Definition: fusion_types.h:188
SV_6DOF_GY_KALMAN::fdeltatsq
float fdeltatsq
Definition: fusion_types.h:195
PressureSensor::fTp
float fTp
Definition: sensor_data_types.h:20
SV_6DOF_GY_KALMAN::fPsiPl
float fPsiPl
Definition: fusion_types.h:159
fusion.h
SV_3DOF_Y_BASIC::fFastdeltat
float fFastdeltat
Definition: fusion_types.h:119
SV_9DOF_GBY_KALMAN::fC6x12
float fC6x12[6][12]
Definition: fusion_types.h:244
FQWB_9DOF_GBY_KALMAN
#define FQWB_9DOF_GBY_KALMAN
Definition: fusion.c:64
SV_6DOF_GY_KALMAN::fbPl
float fbPl[3]
Definition: fusion_types.h:173
SV_6DOF_GB_BASIC::fLPPsi
float fLPPsi
Definition: fusion_types.h:131
orientation.h
SV_9DOF_GBY_KALMAN::fgSeGyMi
float fgSeGyMi[3]
Definition: fusion_types.h:236
SV_9DOF_GBY_KALMAN::fbErrPl
float fbErrPl[3]
Definition: fusion_types.h:224
math_constants.h
fInit_3DOF_G_BASIC
void fInit_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:97
SV_6DOF_GY_KALMAN::fRPl
float fRPl[3][3]
Definition: fusion_types.h:163
SV_9DOF_GBY_KALMAN::fRPl
float fRPl[3][3]
Definition: fusion_types.h:212
SV_9DOF_GBY_KALMAN::fDeltaq
struct fquaternion fDeltaq
Definition: fusion_types.h:246
SV_6DOF_GY_KALMAN::fOmega
float fOmega[3]
Definition: fusion_types.h:167
SV_9DOF_GBY_KALMAN::fbPl
float fbPl[3]
Definition: fusion_types.h:222
fRun_9DOF_GBY_KALMAN
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
fInit_6DOF_GB_BASIC
void fInit_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:148
SV_6DOF_GY_KALMAN::faSeMi
float faSeMi[3]
Definition: fusion_types.h:186
SV_9DOF_GBY_KALMAN::fQw12x12
float fQw12x12[12][12]
Definition: fusion_types.h:243
SV_9DOF_GBY_KALMAN::fqPl
struct fquaternion fqPl
Definition: fusion_types.h:213
SV_9DOF_GBY_KALMAN::iFirstOrientationLock
int16 iFirstOrientationLock
Definition: fusion_types.h:254
fInit_3DOF_Y_BASIC
void fInit_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:131
fNEDAnglesDegFromRotationMatrix
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:466
SV_1DOF_P_BASIC::resetflag
int8 resetflag
Definition: fusion_types.h:44
FQWINITDD_9DOF_GBY_KALMAN
#define FQWINITDD_9DOF_GBY_KALMAN
Definition: fusion.c:72
SV_6DOF_GY_KALMAN::fDeltaq
struct fquaternion fDeltaq
Definition: fusion_types.h:181
SV_9DOF_GBY_KALMAN::fdErrGlPl
float fdErrGlPl[3]
Definition: fusion_types.h:227
SV_3DOF_G_BASIC::fLPPhi
float fLPPhi
Definition: fusion_types.h:52
SV_9DOF_GBY_KALMAN::fdErrSePl
float fdErrSePl[3]
Definition: fusion_types.h:228
SV_3DOF_G_BASIC::fLPRVec
float fLPRVec[3]
Definition: fusion_types.h:60
SV_6DOF_GY_KALMAN::fQvAA
float fQvAA
Definition: fusion_types.h:187
SV_9DOF_GBY_KALMAN::fmErrSeMi
float fmErrSeMi[3]
Definition: fusion_types.h:235
SV_3DOF_B_BASIC::fq
struct fquaternion fq
Definition: fusion_types.h:93
fqAeq1
void fqAeq1(struct fquaternion *pqA)
Definition: orientation.c:1091
SV_9DOF_GBY_KALMAN::fmSeGyMi
float fmSeGyMi[3]
Definition: fusion_types.h:237
fRun_3DOF_B_BASIC
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:403
SV_6DOF_GB_BASIC::fdeltat
float fdeltat
Definition: fusion_types.h:147
SV_3DOF_B_BASIC
Definition: fusion_types.h:74
FQVM_9DOF_GBY_KALMAN
#define FQVM_9DOF_GBY_KALMAN
Definition: fusion.c:62
FQWD_9DOF_GBY_KALMAN
#define FQWD_9DOF_GBY_KALMAN
Definition: fusion.c:66
FQWINITTHTH_6DOF_GY_KALMAN
#define FQWINITTHTH_6DOF_GY_KALMAN
Definition: fusion.c:50
GYRO_OVERSAMPLE_RATIO
#define GYRO_OVERSAMPLE_RATIO
Definition: fusion_config.h:24
SV_6DOF_GY_KALMAN::faErrSePl
float faErrSePl[3]
Definition: fusion_types.h:183
fInit_3DOF_B_BASIC
void fInit_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:114
SV_9DOF_GBY_KALMAN::fcdsq
float fcdsq
Definition: fusion_types.h:249
X
Definition: matrix.h:33
SV_9DOF_GBY_KALMAN::faSePl
float faSePl[3]
Definition: fusion_types.h:232
int8
signed char int8
Definition: basic_types.h:12
SV_3DOF_G_BASIC::fq
struct fquaternion fq
Definition: fusion_types.h:67
basic_types.h
SV_3DOF_B_BASIC::fLPq
struct fquaternion fLPq
Definition: fusion_types.h:85
AccelSensor
Definition: sensor_data_types.h:28
SV_6DOF_GY_KALMAN::resetflag
int8 resetflag
Definition: fusion_types.h:198
SV_6DOF_GY_KALMAN::fRVecPl
float fRVecPl[3]
Definition: fusion_types.h:165
SV_9DOF_GBY_KALMAN
Definition: fusion_types.h:202
FQWINITTHTH_9DOF_GBY_KALMAN
#define FQWINITTHTH_9DOF_GBY_KALMAN
Definition: fusion.c:68
f3DOFMagnetometerMatrixWin8
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
Definition: orientation.c:253
SV_6DOF_GB_BASIC::fDelta
float fDelta
Definition: fusion_types.h:145
ANDROID
#define ANDROID
Definition: fusion_types.h:15
SV_3DOF_Y_BASIC::fRVec
float fRVec[3]
Definition: fusion_types.h:112
MagCalibration::iValidMagCal
int8 iValidMagCal
Definition: magnetic.h:61
MagSensor
Definition: sensor_data_types.h:39
SV_9DOF_GBY_KALMAN::fQwbplusQvG
float fQwbplusQvG
Definition: fusion_types.h:253
fRotationVectorDegFromQuaternion
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
Definition: orientation.c:882
SV_6DOF_GY_KALMAN::fcasq
float fcasq
Definition: fusion_types.h:192
SV_9DOF_GBY_KALMAN::faSeMi
float faSeMi[3]
Definition: fusion_types.h:230
SV_6DOF_GY_KALMAN::fgErrSeMi
float fgErrSeMi[3]
Definition: fusion_types.h:184
SV_1DOF_P_BASIC
Definition: fusion_types.h:37
SV_3DOF_B_BASIC::fLPR
float fLPR[3][3]
Definition: fusion_types.h:84
SV_6DOF_GB_BASIC::flpf
float flpf
Definition: fusion_types.h:148
SV_6DOF_GY_KALMAN::fqPl
struct fquaternion fqPl
Definition: fusion_types.h:164
SV_9DOF_GBY_KALMAN::fgErrSeMi
float fgErrSeMi[3]
Definition: fusion_types.h:234
SV_3DOF_Y_BASIC::fPsi
float fPsi
Definition: fusion_types.h:106
SV_6DOF_GY_KALMAN
Definition: fusion_types.h:153
MAG_OVERSAMPLE_RATIO
#define MAG_OVERSAMPLE_RATIO
Definition: fusion_config.h:23
fRun_6DOF_GY_KALMAN
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
FQVA_9DOF_GBY_KALMAN
#define FQVA_9DOF_GBY_KALMAN
Definition: fusion.c:61
SV_3DOF_B_BASIC::fLPThe
float fLPThe
Definition: fusion_types.h:79
SV_3DOF_Y_BASIC::fR
float fR[3][3]
Definition: fusion_types.h:110
SV_6DOF_GB_BASIC::fLPChi
float fLPChi
Definition: fusion_types.h:133
SV_3DOF_G_BASIC::fR
float fR[3][3]
Definition: fusion_types.h:66
SV_6DOF_GB_BASIC::fLPR
float fLPR[3][3]
Definition: fusion_types.h:135
SV_3DOF_B_BASIC::fLPPhi
float fLPPhi
Definition: fusion_types.h:78
feCompassNED
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[])
Definition: orientation.c:262
fInit_9DOF_GBY_KALMAN
void fInit_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, int16 ithisCoordSystem, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:234
qAeqBxC
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
Definition: orientation.c:1015
SV_6DOF_GY_KALMAN::faSePl
float faSePl[3]
Definition: fusion_types.h:182
FQVG_9DOF_GBY_KALMAN
#define FQVG_9DOF_GBY_KALMAN
Definition: fusion.c:63
SV_9DOF_GBY_KALMAN::faErrSePl
float faErrSePl[3]
Definition: fusion_types.h:229
approximations.h
fRun_3DOF_Y_BASIC
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro, int32 loopcounter, int16 ithisCoordSystem, int16 iOverSampleRatio)
Definition: fusion.c:461
FQWINITAA_9DOF_GBY_KALMAN
#define FQWINITAA_9DOF_GBY_KALMAN
Definition: fusion.c:71
SV_3DOF_B_BASIC::fR
float fR[3][3]
Definition: fusion_types.h:92
SV_3DOF_Y_BASIC::resetflag
int8 resetflag
Definition: fusion_types.h:121
FQWA_6DOF_GY_KALMAN
#define FQWA_6DOF_GY_KALMAN
Definition: fusion.c:48
SV_3DOF_Y_BASIC::fdeltat
float fdeltat
Definition: fusion_types.h:120
SV_3DOF_Y_BASIC::fPhi
float fPhi
Definition: fusion_types.h:104
SV_3DOF_Y_BASIC
Definition: fusion_types.h:100
fInit_1DOF_P_BASIC
void fInit_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:81
SV_3DOF_G_BASIC::flpf
float flpf
Definition: fusion_types.h:69
SV_3DOF_Y_BASIC::fRho
float fRho
Definition: fusion_types.h:107
SV_9DOF_GBY_KALMAN::fdeltatsq
float fdeltatsq
Definition: fusion_types.h:252
GyroSensor::fYpBuffer
float fYpBuffer[GYRO_OVERSAMPLE_RATIO][3]
Definition: sensor_data_types.h:56
feCompassAndroid
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGp[])
Definition: orientation.c:330
FCA_6DOF_GY_KALMAN
#define FCA_6DOF_GY_KALMAN
Definition: fusion.c:55
fqAeqNormqA
void fqAeqNormqA(struct fquaternion *pqA)
Definition: orientation.c:1056
SV_3DOF_B_BASIC::flpf
float flpf
Definition: fusion_types.h:95
f3DOFTiltAndroid
void f3DOFTiltAndroid(float fR[][3], float fGp[])
Definition: orientation.c:114
SV_6DOF_GY_KALMAN::fThePl
float fThePl
Definition: fusion_types.h:158
fusion_config.h
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
SV_9DOF_GBY_KALMAN::fOmega
float fOmega[3]
Definition: fusion_types.h:216
SV_1DOF_P_BASIC::fLPTp
float fLPTp
Definition: fusion_types.h:40
SV_6DOF_GY_KALMAN::fPhiPl
float fPhiPl
Definition: fusion_types.h:157
fQuaternionFromRotationVectorDeg
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
Definition: orientation.c:666
MagCalibration::fFourBsq
float fFourBsq
Definition: magnetic.h:47
SV_6DOF_GB_BASIC::fLPRho
float fLPRho
Definition: fusion_types.h:132
THISCOORDSYSTEM
#define THISCOORDSYSTEM
Definition: fusion_config.h:37
SV_9DOF_GBY_KALMAN::fK12x6
float fK12x6[12][6]
Definition: fusion_types.h:242
SV_6DOF_GY_KALMAN::fC3x9
float fC3x9[3][9]
Definition: fusion_types.h:191
MagCalibration::fB
float fB
Definition: magnetic.h:46
SV_6DOF_GB_BASIC::fq
struct fquaternion fq
Definition: fusion_types.h:144
SV_3DOF_B_BASIC::fOmega
float fOmega[3]
Definition: fusion_types.h:88
SV_3DOF_G_BASIC::fLPR
float fLPR[3][3]
Definition: fusion_types.h:58
PressureSensor::fHp
float fHp
Definition: sensor_data_types.h:19
qAeqAxB
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
Definition: orientation.c:1026
SV_3DOF_Y_BASIC::fq
struct fquaternion fq
Definition: fusion_types.h:111
SV_6DOF_GB_BASIC::fOmega
float fOmega[3]
Definition: fusion_types.h:139
fInit_6DOF_GY_KALMAN
void fInit_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
Definition: fusion.c:166
FQWINITTHB_6DOF_GY_KALMAN
#define FQWINITTHB_6DOF_GY_KALMAN
Definition: fusion.c:52
SV_9DOF_GBY_KALMAN::fChiPl
float fChiPl
Definition: fusion_types.h:210
FQWB_6DOF_GY_KALMAN
#define FQWB_6DOF_GY_KALMAN
Definition: fusion.c:47
FQVG_6DOF_GY_KALMAN
#define FQVG_6DOF_GY_KALMAN
Definition: fusion.c:46
SV_6DOF_GB_BASIC::resetflag
int8 resetflag
Definition: fusion_types.h:149
int32
long int32
This defines int32 as long.
Definition: isf_types.h:32
COSDELTAMAX
#define COSDELTAMAX
Definition: fusion.c:78
SV_9DOF_GBY_KALMAN::fRMi
float fRMi[3][3]
Definition: fusion_types.h:245
SV_3DOF_G_BASIC::fOmega
float fOmega[3]
Definition: fusion_types.h:62
Y
Definition: matrix.h:34
SV_3DOF_G_BASIC::fLPThe
float fLPThe
Definition: fusion_types.h:53
SV_6DOF_GY_KALMAN::fQw9x9
float fQw9x9[9][9]
Definition: fusion_types.h:190
int16
short int16
This defines int16 as short.
Definition: isf_types.h:23
SV_9DOF_GBY_KALMAN::fRhoPl
float fRhoPl
Definition: fusion_types.h:209
f3x3matrixAeqI
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
MagSensor::fBc
float fBc[3]
Definition: sensor_data_types.h:45
SV_9DOF_GBY_KALMAN::fFastdeltat
float fFastdeltat
Definition: fusion_types.h:250
SV_1DOF_P_BASIC::fLPHp
float fLPHp
Definition: fusion_types.h:39
MagCalibration
Definition: magnetic.h:42
SV_6DOF_GY_KALMAN::fRhoPl
float fRhoPl
Definition: fusion_types.h:160
NED
#define NED
Definition: fusion_types.h:14
AccelSensor::fGp
float fGp[3]
Definition: sensor_data_types.h:32
SV_6DOF_GY_KALMAN::fqMi
struct fquaternion fqMi
Definition: fusion_types.h:180
SV_9DOF_GBY_KALMAN::resetflag
int8 resetflag
Definition: fusion_types.h:255
SV_6DOF_GY_KALMAN::fThErrPl
float fThErrPl[3]
Definition: fusion_types.h:174
fRun_3DOF_G_BASIC
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:340
SV_9DOF_GBY_KALMAN::fThErrPl
float fThErrPl[3]
Definition: fusion_types.h:223
Z
Definition: matrix.h:35
SV_6DOF_GB_BASIC::fLPRVec
float fLPRVec[3]
Definition: fusion_types.h:137
SV_3DOF_G_BASIC::resetflag
int8 resetflag
Definition: fusion_types.h:70
SV_6DOF_GY_KALMAN::fbErrPl
float fbErrPl[3]
Definition: fusion_types.h:175
SV_6DOF_GY_KALMAN::fdeltat
float fdeltat
Definition: fusion_types.h:194
SV_9DOF_GBY_KALMAN::fQvAA
float fQvAA
Definition: fusion_types.h:239
SV_9DOF_GBY_KALMAN::fPhiPl
float fPhiPl
Definition: fusion_types.h:206
SV_9DOF_GBY_KALMAN::fPPlus12x12
float fPPlus12x12[12][12]
Definition: fusion_types.h:241
SV_1DOF_P_BASIC::fdeltat
float fdeltat
Definition: fusion_types.h:41
SV_3DOF_G_BASIC::fLPRho
float fLPRho
Definition: fusion_types.h:55
SV_3DOF_B_BASIC::resetflag
int8 resetflag
Definition: fusion_types.h:96
fLPFOrientationQuaternion
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[], int32 loopcounter)
Definition: orientation.c:932
SV_6DOF_GY_KALMAN::fChiPl
float fChiPl
Definition: fusion_types.h:161
SV_3DOF_B_BASIC::fLPChi
float fLPChi
Definition: fusion_types.h:82
FQWINITBB_6DOF_GY_KALMAN
#define FQWINITBB_6DOF_GY_KALMAN
Definition: fusion.c:51
ACCEL_OVERSAMPLE_RATIO
#define ACCEL_OVERSAMPLE_RATIO
Definition: fusion_config.h:22
SV_3DOF_Y_BASIC::fThe
float fThe
Definition: fusion_types.h:105
SV_3DOF_Y_BASIC::fDeltaq
struct fquaternion fDeltaq
Definition: fusion_types.h:118
fRotationMatrixFromQuaternion
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
Definition: orientation.c:775
FQWINITTHB_9DOF_GBY_KALMAN
#define FQWINITTHB_9DOF_GBY_KALMAN
Definition: fusion.c:70
SV_1DOF_P_BASIC::flpf
float flpf
Definition: fusion_types.h:42
SV_3DOF_G_BASIC::fLPPsi
float fLPPsi
Definition: fusion_types.h:54
DEFAULTB
#define DEFAULTB
Definition: magnetic_config.h:12
SV_3DOF_B_BASIC::fLPRho
float fLPRho
Definition: fusion_types.h:81
SV_3DOF_G_BASIC::fdeltat
float fdeltat
Definition: fusion_types.h:68
SV_6DOF_GB_BASIC
Definition: fusion_types.h:125
fRun_1DOF_P_BASIC
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, int32 loopcounter)
Definition: fusion.c:323
FCA_9DOF_GBY_KALMAN
#define FCA_9DOF_GBY_KALMAN
Definition: fusion.c:74
SV_9DOF_GBY_KALMAN::faGlPl
float faGlPl[3]
Definition: fusion_types.h:233
SV_3DOF_G_BASIC::fLPChi
float fLPChi
Definition: fusion_types.h:56
fmatrixAeqInvA
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)
Definition: matrix.c:325
fQuaternionFromRotationMatrix
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
Definition: orientation.c:734
FQWINITAA_6DOF_GY_KALMAN
#define FQWINITAA_6DOF_GY_KALMAN
Definition: fusion.c:53
FQVA_6DOF_GY_KALMAN
#define FQVA_6DOF_GY_KALMAN
Definition: fusion.c:45
SV_3DOF_G_BASIC
Definition: fusion_types.h:48
SV_6DOF_GB_BASIC::fLPThe
float fLPThe
Definition: fusion_types.h:130
SV_9DOF_GBY_KALMAN::fQvMM
float fQvMM
Definition: fusion_types.h:240
SV_3DOF_B_BASIC::fLPPsi
float fLPPsi
Definition: fusion_types.h:80
fLPFScalar
void fLPFScalar(float *pfS, float *pfLPS, float flpf, int32 loopcounter)
Definition: orientation.c:1000
SV_9DOF_GBY_KALMAN::fPsiPl
float fPsiPl
Definition: fusion_types.h:208
f3DOFTiltWin8
void f3DOFTiltWin8(float fR[][3], float fGp[])
Definition: orientation.c:123
SV_9DOF_GBY_KALMAN::fmGl
float fmGl[3]
Definition: fusion_types.h:238
SV_6DOF_GY_KALMAN::fQwbplusQvG
float fQwbplusQvG
Definition: fusion_types.h:196
GyroSensor
Definition: sensor_data_types.h:54
fRun_6DOF_GB_BASIC
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
SV_6DOF_GY_KALMAN::fgSeGyMi
float fgSeGyMi[3]
Definition: fusion_types.h:185
SV_9DOF_GBY_KALMAN::fdeltat
float fdeltat
Definition: fusion_types.h:251
SV_6DOF_GY_KALMAN::fK9x3
float fK9x3[9][3]
Definition: fusion_types.h:189
SV_6DOF_GB_BASIC::fLPq
struct fquaternion fLPq
Definition: fusion_types.h:136
SV_6DOF_GB_BASIC::fLPDelta
float fLPDelta
Definition: fusion_types.h:146
f3DOFMagnetometerMatrixAndroid
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
Definition: orientation.c:225
PressureSensor
Definition: sensor_data_types.h:15
feCompassWin8
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGp[])
Definition: orientation.c:398
SV_6DOF_GB_BASIC::fLPPhi
float fLPPhi
Definition: fusion_types.h:129
fWin8AnglesDegFromRotationMatrix
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:575
- © Freescale Semiconductor, Inc. 2015. All Rights Reserved.