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