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 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 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 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 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 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 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 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 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 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 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 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 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 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 632 void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude) { 633 *attitude = fusion->x0; 634 } 635 636 void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias) { 637 *bias = fusion->x1; 638 } 639 640 void fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R) { 641 quatToMatrix(R, &fusion->x0); 642 } 643
