| ISF 2.2 rev 5 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

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