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