Lines Matching refs:fusion
66 void initFusion(struct Fusion *fusion, uint32_t flags) { in initFusion() argument
67 fusion->flags = flags; in initFusion()
71 fusion->param.gyro_var = DEFAULT_GYRO_VAR; in initFusion()
72 fusion->param.gyro_bias_var = DEFAULT_GYRO_BIAS_VAR; in initFusion()
73 fusion->param.acc_stdev = DEFAULT_ACC_STDEV; in initFusion()
74 fusion->param.mag_stdev = DEFAULT_MAG_STDEV; in initFusion()
77 fusion->param.gyro_var = GEOMAG_GYRO_VAR; in initFusion()
78 fusion->param.gyro_bias_var = GEOMAG_GYRO_BIAS_VAR; in initFusion()
79 fusion->param.acc_stdev = GEOMAG_ACC_STDEV; in initFusion()
80 fusion->param.mag_stdev = GEOMAG_MAG_STDEV; in initFusion()
85 initVec3(&fusion->Ba, 0.0f, 0.0f, 1.0f); in initFusion()
86 initVec3(&fusion->Bm, 0.0f, 1.0f, 0.0f); in initFusion()
88 initVec4(&fusion->x0, 0.0f, 0.0f, 0.0f, 0.0f); in initFusion()
89 initVec3(&fusion->x1, 0.0f, 0.0f, 0.0f); in initFusion()
91 fusion->mInitState = 0; in initFusion()
93 fusion->mPredictDt = 0.0f; in initFusion()
94 fusion->mCount[0] = fusion->mCount[1] = fusion->mCount[2] = 0; in initFusion()
96 initVec3(&fusion->mData[0], 0.0f, 0.0f, 0.0f); in initFusion()
97 initVec3(&fusion->mData[1], 0.0f, 0.0f, 0.0f); in initFusion()
98 initVec3(&fusion->mData[2], 0.0f, 0.0f, 0.0f); in initFusion()
102 fusion->mInitState &= (ACC in initFusion()
103 | ((fusion->flags & FUSION_USE_MAG) ? MAG : 0) in initFusion()
104 | ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0)); in initFusion()
107 fusionSetMagTrust(fusion, NORMAL); in initFusion()
108 fusion->lastMagInvalid = false; in initFusion()
111 int fusionHasEstimate(const struct Fusion *fusion) { in fusionHasEstimate() argument
113 return fusion->mInitState == (ACC in fusionHasEstimate()
114 | ((fusion->flags & FUSION_USE_MAG) ? MAG : 0) in fusionHasEstimate()
115 | ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0)); in fusionHasEstimate()
118 static void updateDt(struct Fusion *fusion, float dT) { in updateDt() argument
119 if (fabsf(fusion->mPredictDt - dT) > DELTA_TIME_MARGIN) { in updateDt()
123 float q00 = fusion->param.gyro_var * dT + in updateDt()
124 0.33333f * fusion->param.gyro_bias_var * dT3; in updateDt()
125 float q11 = fusion->param.gyro_bias_var * dT; in updateDt()
126 float q10 = 0.5f * fusion->param.gyro_bias_var * dT2; in updateDt()
129 initDiagonalMatrix(&fusion->GQGt[0][0], q00); in updateDt()
130 initDiagonalMatrix(&fusion->GQGt[0][1], -q10); in updateDt()
131 initDiagonalMatrix(&fusion->GQGt[1][0], -q01); in updateDt()
132 initDiagonalMatrix(&fusion->GQGt[1][1], q11); in updateDt()
133 fusion->mPredictDt = dT; in updateDt()
137 static int fusion_init_complete(struct Fusion *fusion, int what, const struct Vec3 *d, float dT) { in fusion_init_complete() argument
138 if (fusionHasEstimate(fusion)) { in fusion_init_complete()
145 if (!(fusion->flags & FUSION_USE_GYRO)) { in fusion_init_complete()
146 updateDt(fusion, dT); in fusion_init_complete()
151 vec3Add(&fusion->mData[0], &unityD); in fusion_init_complete()
152 ++fusion->mCount[0]; in fusion_init_complete()
154 if (fusion->mCount[0] == 8) { in fusion_init_complete()
155 fusion->mInitState |= ACC; in fusion_init_complete()
165 vec3Add(&fusion->mData[1], &unityD); in fusion_init_complete()
166 ++fusion->mCount[1]; in fusion_init_complete()
168 fusion->mInitState |= MAG; in fusion_init_complete()
174 updateDt(fusion, dT); in fusion_init_complete()
179 vec3Add(&fusion->mData[2], &scaledD); in fusion_init_complete()
180 ++fusion->mCount[2]; in fusion_init_complete()
182 fusion->mInitState |= GYRO; in fusion_init_complete()
191 if (fusionHasEstimate(fusion)) { in fusion_init_complete()
192 vec3ScalarMul(&fusion->mData[0], 1.0f / fusion->mCount[0]); in fusion_init_complete()
194 if (fusion->flags & FUSION_USE_MAG) { in fusion_init_complete()
195 vec3ScalarMul(&fusion->mData[1], 1.0f / fusion->mCount[1]); in fusion_init_complete()
197 fusion->fake_mag_decimation = 0.f; in fusion_init_complete()
200 struct Vec3 up = fusion->mData[0]; in fusion_init_complete()
203 if (fusion->flags & FUSION_USE_MAG) { in fusion_init_complete()
204 vec3Cross(&east, &fusion->mData[1], &up); in fusion_init_complete()
219 initQuat(&fusion->x0, &R); in fusion_init_complete()
220 initVec3(&fusion->x1, 0.0f, 0.0f, 0.0f); in fusion_init_complete()
222 initZeroMatrix(&fusion->P[0][0]); in fusion_init_complete()
223 initZeroMatrix(&fusion->P[0][1]); in fusion_init_complete()
224 initZeroMatrix(&fusion->P[1][0]); in fusion_init_complete()
225 initZeroMatrix(&fusion->P[1][1]); in fusion_init_complete()
227 fusionSetMagTrust(fusion, INITIALIZATION); in fusion_init_complete()
245 static void fusionCheckState(struct Fusion *fusion) { in fusionCheckState() argument
247 if (!mat33IsPositiveSemidefinite(&fusion->P[0][0], SYMMETRY_TOLERANCE) in fusionCheckState()
249 &fusion->P[1][1], SYMMETRY_TOLERANCE)) { in fusionCheckState()
251 initZeroMatrix(&fusion->P[0][0]); in fusionCheckState()
252 initZeroMatrix(&fusion->P[0][1]); in fusionCheckState()
253 initZeroMatrix(&fusion->P[1][0]); in fusionCheckState()
254 initZeroMatrix(&fusion->P[1][1]); in fusionCheckState()
261 static void fusionPredict(struct Fusion *fusion, const struct Vec3 *w) { in fusionPredict() argument
262 const float dT = fusion->mPredictDt; in fusionPredict()
264 Quat q = fusion->x0; in fusionPredict()
265 struct Vec3 b = fusion->x1; in fusionPredict()
325 fusion->Phi0[0] = I33; in fusionPredict()
326 mat33Sub(&fusion->Phi0[0], &tmp); in fusionPredict()
331 mat33Add(&fusion->Phi0[0], &tmp); in fusionPredict()
335 fusion->Phi0[1] = tmp; in fusionPredict()
337 mat33Sub(&fusion->Phi0[1], &I33dT); in fusionPredict()
342 mat33Sub(&fusion->Phi0[1], &tmp); in fusionPredict()
344 mat44Apply(&fusion->x0, &O, &q); in fusionPredict()
346 if (fusion->x0.w < 0.0f) { in fusionPredict()
347 fusion->x0.x = -fusion->x0.x; in fusionPredict()
348 fusion->x0.y = -fusion->x0.y; in fusionPredict()
349 fusion->x0.z = -fusion->x0.z; in fusionPredict()
350 fusion->x0.w = -fusion->x0.w; in fusionPredict()
356 mat33Multiply(&Pnew[0][0], &fusion->Phi0[0], &fusion->P[0][0]); in fusionPredict()
357 mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][0]); in fusionPredict()
360 mat33Multiply(&Pnew[0][1], &fusion->Phi0[0], &fusion->P[0][1]); in fusionPredict()
361 mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][1]); in fusionPredict()
364 Pnew[1][0] = fusion->P[1][0]; in fusionPredict()
365 Pnew[1][1] = fusion->P[1][1]; in fusionPredict()
369 mat33MultiplyTransposed2(&fusion->P[0][0], &Pnew[0][0], &fusion->Phi0[0]); in fusionPredict()
370 mat33MultiplyTransposed2(&tmp, &Pnew[0][1], &fusion->Phi0[1]); in fusionPredict()
371 mat33Add(&fusion->P[0][0], &tmp); in fusionPredict()
373 fusion->P[0][1] = Pnew[0][1]; in fusionPredict()
375 mat33MultiplyTransposed2(&fusion->P[1][0], &Pnew[1][0], &fusion->Phi0[0]); in fusionPredict()
376 mat33MultiplyTransposed2(&tmp, &Pnew[1][1], &fusion->Phi0[1]); in fusionPredict()
377 mat33Add(&fusion->P[1][0], &tmp); in fusionPredict()
379 fusion->P[1][1] = Pnew[1][1]; in fusionPredict()
381 mat33Add(&fusion->P[0][0], &fusion->GQGt[0][0]); in fusionPredict()
382 mat33Add(&fusion->P[0][1], &fusion->GQGt[0][1]); in fusionPredict()
383 mat33Add(&fusion->P[1][0], &fusion->GQGt[1][0]); in fusionPredict()
384 mat33Add(&fusion->P[1][1], &fusion->GQGt[1][1]); in fusionPredict()
386 fusionCheckState(fusion); in fusionPredict()
389 void fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT) { in fusionHandleGyro() argument
390 if (!fusion_init_complete(fusion, GYRO, w, dT)) { in fusionHandleGyro()
394 updateDt(fusion, dT); in fusionHandleGyro()
396 fusionPredict(fusion, w); in fusionHandleGyro()
431 struct Fusion *fusion, const struct Vec3 *z, const struct Vec3 *Bi, float sigma) { in fusionUpdate() argument
433 quatToMatrix(&A, &fusion->x0); in fusionUpdate()
445 scaleCovariance(&S, &L, &fusion->P[0][0]); in fusionUpdate()
456 mat33Multiply(&K[0], &fusion->P[0][0], &LtSi); in fusionUpdate()
457 mat33MultiplyTransposed(&K[1], &fusion->P[0][1], &LtSi); in fusionUpdate()
466 mat33Multiply(&tmp, &K0L, &fusion->P[0][0]); in fusionUpdate()
467 mat33Sub(&fusion->P[0][0], &tmp); in fusionUpdate()
469 mat33Multiply(&tmp, &K1L, &fusion->P[0][1]); in fusionUpdate()
470 mat33Sub(&fusion->P[1][1], &tmp); in fusionUpdate()
472 mat33Multiply(&tmp, &K0L, &fusion->P[0][1]); in fusionUpdate()
473 mat33Sub(&fusion->P[0][1], &tmp); in fusionUpdate()
475 mat33Transpose(&fusion->P[1][0], &fusion->P[0][1]); in fusionUpdate()
485 getF(F, &fusion->x0); in fusionUpdate()
490 q.x = fusion->x0.x + 0.5f * (F[0].x * dq.x + F[1].x * dq.y + F[2].x * dq.z); in fusionUpdate()
491 q.y = fusion->x0.y + 0.5f * (F[0].y * dq.x + F[1].y * dq.y + F[2].y * dq.z); in fusionUpdate()
492 q.z = fusion->x0.z + 0.5f * (F[0].z * dq.x + F[1].z * dq.y + F[2].z * dq.z); in fusionUpdate()
493 q.w = fusion->x0.w + 0.5f * (F[0].w * dq.x + F[1].w * dq.y + F[2].w * dq.z); in fusionUpdate()
495 fusion->x0 = q; in fusionUpdate()
496 quatNormalize(&fusion->x0); in fusionUpdate()
498 if (fusion->flags & FUSION_USE_MAG) { in fusionUpdate()
503 vec3Add(&fusion->x1, &db); in fusionUpdate()
506 fusionCheckState(fusion); in fusionUpdate()
513 int fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT) { in fusionHandleAcc() argument
514 if (!fusion_init_complete(fusion, ACC, a, dT)) { in fusionHandleAcc()
527 if (!(fusion->flags & FUSION_USE_GYRO)) { in fusionHandleAcc()
533 initVec3(&w_dummy, fusion->x1.x + kEps, fusion->x1.y + kEps, in fusionHandleAcc()
534 fusion->x1.z + kEps); in fusionHandleAcc()
536 updateDt(fusion, dT); in fusionHandleAcc()
537 fusionPredict(fusion, &w_dummy); in fusionHandleAcc()
541 fusionGetRotationMatrix(fusion, &R); in fusionHandleAcc()
543 if (!(fusion->flags & FUSION_USE_MAG) && in fusionHandleAcc()
544 (fusion->fake_mag_decimation += dT) > FAKE_MAG_INTERVAL) { in fusionHandleAcc()
548 mat33Apply(&m, &R, &fusion->Bm); in fusionHandleAcc()
550 fusionUpdate(fusion, &m, &fusion->Bm, in fusionHandleAcc()
551 fusion->param.mag_stdev); in fusionHandleAcc()
552 fusion->fake_mag_decimation = 0.f; in fusionHandleAcc()
560 if (fusion->flags & FUSION_USE_GYRO) { in fusionHandleAcc()
565 mat33Apply(&aa, &R, &fusion->Ba); in fusionHandleAcc()
572 p = fusion->param.acc_stdev * expf(3 * d - fc); in fusionHandleAcc()
578 p = fusion->param.acc_stdev * expf(sqrtf(d)); in fusionHandleAcc()
581 fusionUpdate(fusion, &unityA, &fusion->Ba, p); in fusionHandleAcc()
590 int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m, float dT) { in fusionHandleMag() argument
591 if (!fusion_init_complete(fusion, MAG, m, 0.0f /* dT */)) { in fusionHandleMag()
599 fusionSetMagTrust(fusion, NORMAL); in fusionHandleMag()
600 fusion->lastMagInvalid = true; in fusionHandleMag()
605 fusionGetRotationMatrix(fusion, &R); in fusionHandleMag()
608 mat33Apply(&up, &R, &fusion->Ba); in fusionHandleMag()
614 fusionSetMagTrust(fusion, NORMAL); in fusionHandleMag()
615 fusion->lastMagInvalid = true; in fusionHandleMag()
619 if (fusion->lastMagInvalid) { in fusionHandleMag()
620 fusion->lastMagInvalid = false; in fusionHandleMag()
621 fusionSetMagTrust(fusion, BACK_TO_VALID); in fusionHandleMag()
630 float p = fusion->param.mag_stdev; in fusionHandleMag()
632 if (fusion->flags & FUSION_USE_GYRO) { in fusionHandleMag()
634 mat33Apply(&mm, &R, &fusion->Bm); in fusionHandleMag()
637 if (fusion->trustedMagDuration > 0) { in fusionHandleMag()
643 fusion->trustedMagDuration -= dT; in fusionHandleMag()
646 fusionSetMagTrust(fusion, NORMAL); in fusionHandleMag()
658 fusionUpdate(fusion, &north, &fusion->Bm, p); in fusionHandleMag()
663 void fusionSetMagTrust(struct Fusion *fusion, int mode) { in fusionSetMagTrust() argument
666 fusion->trustedMagDuration = 0; // disable in fusionSetMagTrust()
670 fusion->trustedMagDuration = 0; // no special treatment for these two in fusionSetMagTrust()
673 fusion->trustedMagDuration = TRUST_DURATION_MANUAL_MAG_CAL; in fusionSetMagTrust()
676 fusion->trustedMagDuration = 0; // by default it is disable in fusionSetMagTrust()
681 void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude) { in fusionGetAttitude() argument
682 *attitude = fusion->x0; in fusionGetAttitude()
685 void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias) { in fusionGetBias() argument
686 *bias = fusion->x1; in fusionGetBias()
689 void fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R) { in fusionGetRotationMatrix() argument
690 quatToMatrix(R, &fusion->x0); in fusionGetRotationMatrix()