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