1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 // adapted from frameworks/native/services/sensorservice/Fusion.cpp
18
19 #include <algos/fusion.h>
20 #include <algos/mat.h>
21
22 #include <errno.h>
23 #include <nanohub_math.h>
24 #include <stdio.h>
25
26 #include <seos.h>
27
28 #ifdef DEBUG_CH
29 // change to 0 to disable fusion debugging output
30 #define DEBUG_FUSION 0
31 #endif
32
33 #define ACC 1
34 #define MAG 2
35 #define GYRO 4
36
37 #define DEFAULT_GYRO_VAR 1e-7f
38 #define DEFAULT_GYRO_BIAS_VAR 1e-12f
39 #define DEFAULT_ACC_STDEV 5e-2f
40 #define DEFAULT_MAG_STDEV 5e-1f
41
42 #define GEOMAG_GYRO_VAR 2e-4f
43 #define GEOMAG_GYRO_BIAS_VAR 1e-4f
44 #define GEOMAG_ACC_STDEV 0.02f
45 #define GEOMAG_MAG_STDEV 0.02f
46
47 #define SYMMETRY_TOLERANCE 1e-10f
48 #define FAKE_MAG_INTERVAL 1.0f //sec
49
50 #define NOMINAL_GRAVITY 9.81f
51 #define FREE_FALL_THRESHOLD (0.1f * NOMINAL_GRAVITY)
52 #define FREE_FALL_THRESHOLD_SQ (FREE_FALL_THRESHOLD * FREE_FALL_THRESHOLD)
53
54 #define MAX_VALID_MAGNETIC_FIELD 75.0f
55 #define MAX_VALID_MAGNETIC_FIELD_SQ (MAX_VALID_MAGNETIC_FIELD * MAX_VALID_MAGNETIC_FIELD)
56
57 #define MIN_VALID_MAGNETIC_FIELD 30.0f
58 #define MIN_VALID_MAGNETIC_FIELD_SQ (MIN_VALID_MAGNETIC_FIELD * MIN_VALID_MAGNETIC_FIELD)
59
60 #define MIN_VALID_CROSS_PRODUCT_MAG 1.0e-3
61 #define MIN_VALID_CROSS_PRODUCT_MAG_SQ (MIN_VALID_CROSS_PRODUCT_MAG * MIN_VALID_CROSS_PRODUCT_MAG)
62
63 #define DELTA_TIME_MARGIN 1.0e-9f
64
initFusion(struct Fusion * fusion,uint32_t flags)65 void initFusion(struct Fusion *fusion, uint32_t flags) {
66 fusion->flags = flags;
67
68 if (flags & FUSION_USE_GYRO) {
69 // normal fusion mode
70 fusion->param.gyro_var = DEFAULT_GYRO_VAR;
71 fusion->param.gyro_bias_var = DEFAULT_GYRO_BIAS_VAR;
72 fusion->param.acc_stdev = DEFAULT_ACC_STDEV;
73 fusion->param.mag_stdev = DEFAULT_MAG_STDEV;
74 } else {
75 // geo mag mode
76 fusion->param.gyro_var = GEOMAG_GYRO_VAR;
77 fusion->param.gyro_bias_var = GEOMAG_GYRO_BIAS_VAR;
78 fusion->param.acc_stdev = GEOMAG_ACC_STDEV;
79 fusion->param.mag_stdev = GEOMAG_MAG_STDEV;
80 }
81
82 if (flags & FUSION_REINITIALIZE)
83 {
84 initVec3(&fusion->Ba, 0.0f, 0.0f, 1.0f);
85 initVec3(&fusion->Bm, 0.0f, 1.0f, 0.0f);
86
87 initVec4(&fusion->x0, 0.0f, 0.0f, 0.0f, 0.0f);
88 initVec3(&fusion->x1, 0.0f, 0.0f, 0.0f);
89
90 fusion->mInitState = 0;
91
92 fusion->mPredictDt = 0.0f;
93 fusion->mCount[0] = fusion->mCount[1] = fusion->mCount[2] = 0;
94
95 initVec3(&fusion->mData[0], 0.0f, 0.0f, 0.0f);
96 initVec3(&fusion->mData[1], 0.0f, 0.0f, 0.0f);
97 initVec3(&fusion->mData[2], 0.0f, 0.0f, 0.0f);
98 } else {
99 // mask off disabled sensor bit
100 fusion->mInitState &= (ACC
101 | ((fusion->flags & FUSION_USE_MAG) ? MAG : 0)
102 | ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0));
103 }
104 }
105
fusionHasEstimate(const struct Fusion * fusion)106 int fusionHasEstimate(const struct Fusion *fusion) {
107 // waive sensor init depends on the mode
108 return fusion->mInitState == (ACC
109 | ((fusion->flags & FUSION_USE_MAG) ? MAG : 0)
110 | ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0));
111 }
112
updateDt(struct Fusion * fusion,float dT)113 static void updateDt(struct Fusion *fusion, float dT) {
114 if (fabsf(fusion->mPredictDt - dT) > DELTA_TIME_MARGIN) {
115 float dT2 = dT * dT;
116 float dT3 = dT2 * dT;
117
118 float q00 = fusion->param.gyro_var * dT +
119 0.33333f * fusion->param.gyro_bias_var * dT3;
120 float q11 = fusion->param.gyro_bias_var * dT;
121 float q10 = 0.5f * fusion->param.gyro_bias_var * dT2;
122 float q01 = q10;
123
124 initDiagonalMatrix(&fusion->GQGt[0][0], q00);
125 initDiagonalMatrix(&fusion->GQGt[0][1], -q10);
126 initDiagonalMatrix(&fusion->GQGt[1][0], -q01);
127 initDiagonalMatrix(&fusion->GQGt[1][1], q11);
128 fusion->mPredictDt = dT;
129 }
130 }
131
fusion_init_complete(struct Fusion * fusion,int what,const struct Vec3 * d,float dT)132 static int fusion_init_complete(struct Fusion *fusion, int what, const struct Vec3 *d, float dT) {
133 if (fusionHasEstimate(fusion)) {
134 return 1;
135 }
136
137 switch (what) {
138 case ACC:
139 {
140 if (!(fusion->flags & FUSION_USE_GYRO)) {
141 updateDt(fusion, dT);
142 }
143 struct Vec3 unityD = *d;
144 vec3Normalize(&unityD);
145
146 vec3Add(&fusion->mData[0], &unityD);
147 ++fusion->mCount[0];
148
149 if (fusion->mCount[0] == 8) {
150 fusion->mInitState |= ACC;
151 }
152 break;
153 }
154
155 case MAG:
156 {
157 struct Vec3 unityD = *d;
158 vec3Normalize(&unityD);
159
160 vec3Add(&fusion->mData[1], &unityD);
161 ++fusion->mCount[1];
162
163 fusion->mInitState |= MAG;
164 break;
165 }
166
167 case GYRO:
168 {
169 updateDt(fusion, dT);
170
171 struct Vec3 scaledD = *d;
172 vec3ScalarMul(&scaledD, dT);
173
174 vec3Add(&fusion->mData[2], &scaledD);
175 ++fusion->mCount[2];
176
177 fusion->mInitState |= GYRO;
178 break;
179 }
180
181 default:
182 // assert(!"should not be here");
183 break;
184 }
185
186 if (fusionHasEstimate(fusion)) {
187 vec3ScalarMul(&fusion->mData[0], 1.0f / fusion->mCount[0]);
188
189 if (fusion->flags & FUSION_USE_MAG) {
190 vec3ScalarMul(&fusion->mData[1], 1.0f / fusion->mCount[1]);
191 } else {
192 fusion->fake_mag_decimation = 0.f;
193 }
194
195 struct Vec3 up = fusion->mData[0];
196
197 struct Vec3 east;
198 if (fusion->flags & FUSION_USE_MAG) {
199 vec3Cross(&east, &fusion->mData[1], &up);
200 vec3Normalize(&east);
201 } else {
202 findOrthogonalVector(up.x, up.y, up.z, &east.x, &east.y, &east.z);
203 }
204
205 struct Vec3 north;
206 vec3Cross(&north, &up, &east);
207
208 struct Mat33 R;
209 initMatrixColumns(&R, &east, &north, &up);
210
211 //Quat q;
212 //initQuat(&q, &R);
213
214 initQuat(&fusion->x0, &R);
215 initVec3(&fusion->x1, 0.0f, 0.0f, 0.0f);
216
217 initZeroMatrix(&fusion->P[0][0]);
218 initZeroMatrix(&fusion->P[0][1]);
219 initZeroMatrix(&fusion->P[1][0]);
220 initZeroMatrix(&fusion->P[1][1]);
221 }
222
223 return 0;
224 }
225
matrixCross(struct Mat33 * out,struct Vec3 * p,float diag)226 static void matrixCross(struct Mat33 *out, struct Vec3 *p, float diag) {
227 out->elem[0][0] = diag;
228 out->elem[1][1] = diag;
229 out->elem[2][2] = diag;
230 out->elem[1][0] = p->z;
231 out->elem[0][1] = -p->z;
232 out->elem[2][0] = -p->y;
233 out->elem[0][2] = p->y;
234 out->elem[2][1] = p->x;
235 out->elem[1][2] = -p->x;
236 }
237
fusionCheckState(struct Fusion * fusion)238 static void fusionCheckState(struct Fusion *fusion) {
239
240 if (!mat33IsPositiveSemidefinite(&fusion->P[0][0], SYMMETRY_TOLERANCE)
241 || !mat33IsPositiveSemidefinite(
242 &fusion->P[1][1], SYMMETRY_TOLERANCE)) {
243
244 initZeroMatrix(&fusion->P[0][0]);
245 initZeroMatrix(&fusion->P[0][1]);
246 initZeroMatrix(&fusion->P[1][0]);
247 initZeroMatrix(&fusion->P[1][1]);
248 }
249 }
250
251 #define kEps 1.0E-4f
252
253 UNROLLED
fusionPredict(struct Fusion * fusion,const struct Vec3 * w)254 static void fusionPredict(struct Fusion *fusion, const struct Vec3 *w) {
255 const float dT = fusion->mPredictDt;
256
257 Quat q = fusion->x0;
258 struct Vec3 b = fusion->x1;
259
260 struct Vec3 we = *w;
261 vec3Sub(&we, &b);
262
263 struct Mat33 I33;
264 initDiagonalMatrix(&I33, 1.0f);
265
266 struct Mat33 I33dT;
267 initDiagonalMatrix(&I33dT, dT);
268
269 struct Mat33 wx;
270 matrixCross(&wx, &we, 0.0f);
271
272 struct Mat33 wx2;
273 mat33Multiply(&wx2, &wx, &wx);
274
275 float norm_we = vec3Norm(&we);
276
277 if (fabsf(norm_we) < kEps) {
278 return;
279 }
280
281 float lwedT = norm_we * dT;
282 float hlwedT = 0.5f * lwedT;
283 float ilwe = 1.0f / norm_we;
284 float k0 = (1.0f - cosf(lwedT)) * (ilwe * ilwe);
285 float k1 = sinf(lwedT);
286 float k2 = cosf(hlwedT);
287
288 struct Vec3 psi = we;
289 vec3ScalarMul(&psi, sinf(hlwedT) * ilwe);
290
291 struct Vec3 negPsi = psi;
292 vec3ScalarMul(&negPsi, -1.0f);
293
294 struct Mat33 O33;
295 matrixCross(&O33, &negPsi, k2);
296
297 struct Mat44 O;
298 uint32_t i;
299 for (i = 0; i < 3; ++i) {
300 uint32_t j;
301 for (j = 0; j < 3; ++j) {
302 O.elem[i][j] = O33.elem[i][j];
303 }
304 }
305
306 O.elem[3][0] = -psi.x;
307 O.elem[3][1] = -psi.y;
308 O.elem[3][2] = -psi.z;
309 O.elem[3][3] = k2;
310
311 O.elem[0][3] = psi.x;
312 O.elem[1][3] = psi.y;
313 O.elem[2][3] = psi.z;
314
315 struct Mat33 tmp = wx;
316 mat33ScalarMul(&tmp, k1 * ilwe);
317
318 fusion->Phi0[0] = I33;
319 mat33Sub(&fusion->Phi0[0], &tmp);
320
321 tmp = wx2;
322 mat33ScalarMul(&tmp, k0);
323
324 mat33Add(&fusion->Phi0[0], &tmp);
325
326 tmp = wx;
327 mat33ScalarMul(&tmp, k0);
328 fusion->Phi0[1] = tmp;
329
330 mat33Sub(&fusion->Phi0[1], &I33dT);
331
332 tmp = wx2;
333 mat33ScalarMul(&tmp, ilwe * ilwe * ilwe * (lwedT - k1));
334
335 mat33Sub(&fusion->Phi0[1], &tmp);
336
337 mat44Apply(&fusion->x0, &O, &q);
338
339 if (fusion->x0.w < 0.0f) {
340 fusion->x0.x = -fusion->x0.x;
341 fusion->x0.y = -fusion->x0.y;
342 fusion->x0.z = -fusion->x0.z;
343 fusion->x0.w = -fusion->x0.w;
344 }
345
346 // Pnew = Phi * P
347
348 struct Mat33 Pnew[2][2];
349 mat33Multiply(&Pnew[0][0], &fusion->Phi0[0], &fusion->P[0][0]);
350 mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][0]);
351 mat33Add(&Pnew[0][0], &tmp);
352
353 mat33Multiply(&Pnew[0][1], &fusion->Phi0[0], &fusion->P[0][1]);
354 mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][1]);
355 mat33Add(&Pnew[0][1], &tmp);
356
357 Pnew[1][0] = fusion->P[1][0];
358 Pnew[1][1] = fusion->P[1][1];
359
360 // P = Pnew * Phi^T
361
362 mat33MultiplyTransposed2(&fusion->P[0][0], &Pnew[0][0], &fusion->Phi0[0]);
363 mat33MultiplyTransposed2(&tmp, &Pnew[0][1], &fusion->Phi0[1]);
364 mat33Add(&fusion->P[0][0], &tmp);
365
366 fusion->P[0][1] = Pnew[0][1];
367
368 mat33MultiplyTransposed2(&fusion->P[1][0], &Pnew[1][0], &fusion->Phi0[0]);
369 mat33MultiplyTransposed2(&tmp, &Pnew[1][1], &fusion->Phi0[1]);
370 mat33Add(&fusion->P[1][0], &tmp);
371
372 fusion->P[1][1] = Pnew[1][1];
373
374 mat33Add(&fusion->P[0][0], &fusion->GQGt[0][0]);
375 mat33Add(&fusion->P[0][1], &fusion->GQGt[0][1]);
376 mat33Add(&fusion->P[1][0], &fusion->GQGt[1][0]);
377 mat33Add(&fusion->P[1][1], &fusion->GQGt[1][1]);
378
379 fusionCheckState(fusion);
380 }
381
fusionHandleGyro(struct Fusion * fusion,const struct Vec3 * w,float dT)382 void fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT) {
383 if (!fusion_init_complete(fusion, GYRO, w, dT)) {
384 return;
385 }
386
387 updateDt(fusion, dT);
388
389 fusionPredict(fusion, w);
390 }
391
392 UNROLLED
scaleCovariance(struct Mat33 * out,const struct Mat33 * A,const struct Mat33 * P)393 static void scaleCovariance(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *P) {
394 uint32_t r;
395 for (r = 0; r < 3; ++r) {
396 uint32_t j;
397 for (j = r; j < 3; ++j) {
398 float apat = 0.0f;
399 uint32_t c;
400 for (c = 0; c < 3; ++c) {
401 float v = A->elem[c][r] * P->elem[c][c] * 0.5f;
402 uint32_t k;
403 for (k = c + 1; k < 3; ++k) {
404 v += A->elem[k][r] * P->elem[c][k];
405 }
406
407 apat += 2.0f * v * A->elem[c][j];
408 }
409
410 out->elem[r][j] = apat;
411 out->elem[j][r] = apat;
412 }
413 }
414 }
415
getF(struct Vec4 F[3],const struct Vec4 * q)416 static void getF(struct Vec4 F[3], const struct Vec4 *q) {
417 F[0].x = q->w; F[1].x = -q->z; F[2].x = q->y;
418 F[0].y = q->z; F[1].y = q->w; F[2].y = -q->x;
419 F[0].z = -q->y; F[1].z = q->x; F[2].z = q->w;
420 F[0].w = -q->x; F[1].w = -q->y; F[2].w = -q->z;
421 }
422
fusionUpdate(struct Fusion * fusion,const struct Vec3 * z,const struct Vec3 * Bi,float sigma)423 static void fusionUpdate(
424 struct Fusion *fusion, const struct Vec3 *z, const struct Vec3 *Bi, float sigma) {
425 struct Mat33 A;
426 quatToMatrix(&A, &fusion->x0);
427
428 struct Vec3 Bb;
429 mat33Apply(&Bb, &A, Bi);
430
431 struct Mat33 L;
432 matrixCross(&L, &Bb, 0.0f);
433
434 struct Mat33 R;
435 initDiagonalMatrix(&R, sigma * sigma);
436
437 struct Mat33 S;
438 scaleCovariance(&S, &L, &fusion->P[0][0]);
439
440 mat33Add(&S, &R);
441
442 struct Mat33 Si;
443 mat33Invert(&Si, &S);
444
445 struct Mat33 LtSi;
446 mat33MultiplyTransposed(&LtSi, &L, &Si);
447
448 struct Mat33 K[2];
449 mat33Multiply(&K[0], &fusion->P[0][0], &LtSi);
450 mat33MultiplyTransposed(&K[1], &fusion->P[0][1], &LtSi);
451
452 struct Mat33 K0L;
453 mat33Multiply(&K0L, &K[0], &L);
454
455 struct Mat33 K1L;
456 mat33Multiply(&K1L, &K[1], &L);
457
458 struct Mat33 tmp;
459 mat33Multiply(&tmp, &K0L, &fusion->P[0][0]);
460 mat33Sub(&fusion->P[0][0], &tmp);
461
462 mat33Multiply(&tmp, &K1L, &fusion->P[0][1]);
463 mat33Sub(&fusion->P[1][1], &tmp);
464
465 mat33Multiply(&tmp, &K0L, &fusion->P[0][1]);
466 mat33Sub(&fusion->P[0][1], &tmp);
467
468 mat33Transpose(&fusion->P[1][0], &fusion->P[0][1]);
469
470 struct Vec3 e = *z;
471 vec3Sub(&e, &Bb);
472
473 struct Vec3 dq;
474 mat33Apply(&dq, &K[0], &e);
475
476
477 struct Vec4 F[3];
478 getF(F, &fusion->x0);
479
480 // 4x3 * 3x1 => 4x1
481
482 struct Vec4 q;
483 q.x = fusion->x0.x + 0.5f * (F[0].x * dq.x + F[1].x * dq.y + F[2].x * dq.z);
484 q.y = fusion->x0.y + 0.5f * (F[0].y * dq.x + F[1].y * dq.y + F[2].y * dq.z);
485 q.z = fusion->x0.z + 0.5f * (F[0].z * dq.x + F[1].z * dq.y + F[2].z * dq.z);
486 q.w = fusion->x0.w + 0.5f * (F[0].w * dq.x + F[1].w * dq.y + F[2].w * dq.z);
487
488 fusion->x0 = q;
489 quatNormalize(&fusion->x0);
490
491 if (fusion->flags & FUSION_USE_MAG) {
492 // accumulate gyro bias (causes self spin) only if not
493 // game rotation vector
494 struct Vec3 db;
495 mat33Apply(&db, &K[1], &e);
496 vec3Add(&fusion->x1, &db);
497 }
498
499 fusionCheckState(fusion);
500 }
501
502 #define ACC_TRUSTWORTHY(abs_norm_err) ((abs_norm_err) < 1.f)
503 #define ACC_COS_CONV_FACTOR 0.01f
504 #define ACC_COS_CONV_LIMIT 3.f
505
fusionHandleAcc(struct Fusion * fusion,const struct Vec3 * a,float dT)506 int fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT) {
507 if (!fusion_init_complete(fusion, ACC, a, dT)) {
508 return -EINVAL;
509 }
510
511 float norm2 = vec3NormSquared(a);
512
513 if (norm2 < FREE_FALL_THRESHOLD_SQ) {
514 return -EINVAL;
515 }
516
517 float l = sqrtf(norm2);
518 float l_inv = 1.0f / l;
519
520 if (!(fusion->flags & FUSION_USE_GYRO)) {
521 // geo mag mode
522 // drive the Kalman filter with zero mean dummy gyro vector
523 struct Vec3 w_dummy;
524
525 // avoid (fabsf(norm_we) < kEps) in fusionPredict()
526 initVec3(&w_dummy, fusion->x1.x + kEps, fusion->x1.y + kEps,
527 fusion->x1.z + kEps);
528
529 updateDt(fusion, dT);
530 fusionPredict(fusion, &w_dummy);
531 }
532
533 struct Mat33 R;
534 fusionGetRotationMatrix(fusion, &R);
535
536 if (!(fusion->flags & FUSION_USE_MAG) &&
537 (fusion->fake_mag_decimation += dT) > FAKE_MAG_INTERVAL) {
538 // game rotation mode, provide fake mag update to prevent
539 // P to diverge over time
540 struct Vec3 m;
541 mat33Apply(&m, &R, &fusion->Bm);
542
543 fusionUpdate(fusion, &m, &fusion->Bm,
544 fusion->param.mag_stdev);
545 fusion->fake_mag_decimation = 0.f;
546 }
547
548 struct Vec3 unityA = *a;
549 vec3ScalarMul(&unityA, l_inv);
550
551 float d = fabsf(l - NOMINAL_GRAVITY);
552 float p;
553 if (fusion->flags & FUSION_USE_GYRO) {
554 float fc = 0;
555 // Enable faster convergence
556 if (ACC_TRUSTWORTHY(d)) {
557 struct Vec3 aa;
558 mat33Apply(&aa, &R, &fusion->Ba);
559 float cos_err = vec3Dot(&aa, &unityA);
560 cos_err = cos_err < (1.f - ACC_COS_CONV_FACTOR) ?
561 (1.f - ACC_COS_CONV_FACTOR) : cos_err;
562 fc = (1.f - cos_err) *
563 (1.0f / ACC_COS_CONV_FACTOR * ACC_COS_CONV_LIMIT);
564 }
565 p = fusion->param.acc_stdev * expf(3 * d - fc);
566 } else {
567 // Adaptive acc weighting (trust acc less as it deviates from nominal g
568 // more), acc_stdev *= e(sqrt(| |acc| - g_nominal|))
569 //
570 // The weighting equation comes from heuristics.
571 p = fusion->param.acc_stdev * expf(sqrtf(d));
572 }
573
574 fusionUpdate(fusion, &unityA, &fusion->Ba, p);
575
576 return 0;
577 }
578
579 #define MAG_COS_CONV_FACTOR 0.02f
580 #define MAG_COS_CONV_LIMIT 2.f
581
fusionHandleMag(struct Fusion * fusion,const struct Vec3 * m)582 int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m) {
583 if (!fusion_init_complete(fusion, MAG, m, 0.0f /* dT */)) {
584 return -EINVAL;
585 }
586
587 float magFieldSq = vec3NormSquared(m);
588
589 if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ
590 || magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
591 return -EINVAL;
592 }
593
594 struct Mat33 R;
595 fusionGetRotationMatrix(fusion, &R);
596
597 struct Vec3 up;
598 mat33Apply(&up, &R, &fusion->Ba);
599
600 struct Vec3 east;
601 vec3Cross(&east, m, &up);
602
603 if (vec3NormSquared(&east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
604 return -EINVAL;
605 }
606
607 struct Vec3 north;
608 vec3Cross(&north, &up, &east);
609
610 float invNorm = 1.0f / vec3Norm(&north);
611 vec3ScalarMul(&north, invNorm);
612
613 float p = fusion->param.mag_stdev;
614
615 if (fusion->flags & FUSION_USE_GYRO) {
616 struct Vec3 mm;
617 mat33Apply(&mm, &R, &fusion->Bm);
618 float cos_err = vec3Dot(&mm, &north);
619 cos_err = cos_err < (1.f - MAG_COS_CONV_FACTOR) ?
620 (1.f - MAG_COS_CONV_FACTOR) : cos_err;
621
622 float fc;
623 fc = (1.f - cos_err) * (1.0f / MAG_COS_CONV_FACTOR * MAG_COS_CONV_LIMIT);
624 p *= expf(-fc);
625 }
626
627 fusionUpdate(fusion, &north, &fusion->Bm, p);
628
629 return 0;
630 }
631
fusionGetAttitude(const struct Fusion * fusion,struct Vec4 * attitude)632 void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude) {
633 *attitude = fusion->x0;
634 }
635
fusionGetBias(const struct Fusion * fusion,struct Vec3 * bias)636 void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias) {
637 *bias = fusion->x1;
638 }
639
fusionGetRotationMatrix(const struct Fusion * fusion,struct Mat33 * R)640 void fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R) {
641 quatToMatrix(R, &fusion->x0);
642 }
643