1/* 2 * Copyright (C) 2011 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#include <stdio.h> 18 19#include <utils/Log.h> 20 21#include "Fusion.h" 22 23namespace android { 24 25// ----------------------------------------------------------------------- 26 27/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/ 28 29/* Note: 30 * If a platform uses software fusion, it is necessary to tune the following 31 * parameters to fit the hardware sensors prior to release. 32 * 33 * The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode. 34 * The GEOMAG_ parameters will be used in FUSION_NOGYRO mode. 35 */ 36 37/* 38 * GYRO_VAR gives the measured variance of the gyro's output per 39 * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro, 40 * which is independent of the sampling frequency. 41 * 42 * The variance of gyro's output at a given sampling period can be 43 * calculated as: 44 * variance(T) = GYRO_VAR / T 45 * 46 * The variance of the INTEGRATED OUTPUT at a given sampling period can be 47 * calculated as: 48 * variance_integrate_output(T) = GYRO_VAR * T 49 */ 50static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz 51static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed) 52static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz 53static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed) 54 55/* 56 * Standard deviations of accelerometer and magnetometer 57 */ 58static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05) 59static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5) 60static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) 61static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5) 62 63 64/* ====================== END FUSION SENSOR PARAMETER ========================*/ 65 66static const float SYMMETRY_TOLERANCE = 1e-10f; 67 68/* 69 * Accelerometer updates will not be performed near free fall to avoid 70 * ill-conditioning and div by zeros. 71 * Threshhold: 10% of g, in m/s^2 72 */ 73static const float NOMINAL_GRAVITY = 9.81f; 74static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY); 75 76/* 77 * The geomagnetic-field should be between 30uT and 60uT. 78 * Fields strengths greater than this likely indicate a local magnetic 79 * disturbance which we do not want to update into the fused frame. 80 */ 81static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT 82static const float MAX_VALID_MAGNETIC_FIELD_SQ = 83 MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD; 84 85/* 86 * Values of the field smaller than this should be ignored in fusion to avoid 87 * ill-conditioning. This state can happen with anomalous local magnetic 88 * disturbances canceling the Earth field. 89 */ 90static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT 91static const float MIN_VALID_MAGNETIC_FIELD_SQ = 92 MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD; 93 94/* 95 * If the cross product of two vectors has magnitude squared less than this, 96 * we reject it as invalid due to alignment of the vectors. 97 * This threshold is used to check for the case where the magnetic field sample 98 * is parallel to the gravity field, which can happen in certain places due 99 * to magnetic field disturbances. 100 */ 101static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3; 102static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ = 103 MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG; 104 105static const float SQRT_3 = 1.732f; 106static const float WVEC_EPS = 1e-4f/SQRT_3; 107// ----------------------------------------------------------------------- 108 109template <typename TYPE, size_t C, size_t R> 110static mat<TYPE, R, R> scaleCovariance( 111 const mat<TYPE, C, R>& A, 112 const mat<TYPE, C, C>& P) { 113 // A*P*transpose(A); 114 mat<TYPE, R, R> APAt; 115 for (size_t r=0 ; r<R ; r++) { 116 for (size_t j=r ; j<R ; j++) { 117 double apat(0); 118 for (size_t c=0 ; c<C ; c++) { 119 double v(A[c][r]*P[c][c]*0.5); 120 for (size_t k=c+1 ; k<C ; k++) 121 v += A[k][r] * P[c][k]; 122 apat += 2 * v * A[c][j]; 123 } 124 APAt[j][r] = apat; 125 APAt[r][j] = apat; 126 } 127 } 128 return APAt; 129} 130 131template <typename TYPE, typename OTHER_TYPE> 132static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) { 133 mat<TYPE, 3, 3> r; 134 r[0][0] = diag; 135 r[1][1] = diag; 136 r[2][2] = diag; 137 r[0][1] = p.z; 138 r[1][0] =-p.z; 139 r[0][2] =-p.y; 140 r[2][0] = p.y; 141 r[1][2] = p.x; 142 r[2][1] =-p.x; 143 return r; 144} 145 146 147template<typename TYPE, size_t SIZE> 148class Covariance { 149 mat<TYPE, SIZE, SIZE> mSumXX; 150 vec<TYPE, SIZE> mSumX; 151 size_t mN; 152public: 153 Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { } 154 void update(const vec<TYPE, SIZE>& x) { 155 mSumXX += x*transpose(x); 156 mSumX += x; 157 mN++; 158 } 159 mat<TYPE, SIZE, SIZE> operator()() const { 160 const float N = 1.0f / mN; 161 return mSumXX*N - (mSumX*transpose(mSumX))*(N*N); 162 } 163 void reset() { 164 mN = 0; 165 mSumXX = 0; 166 mSumX = 0; 167 } 168 size_t getCount() const { 169 return mN; 170 } 171}; 172 173// ----------------------------------------------------------------------- 174 175Fusion::Fusion() { 176 Phi[0][1] = 0; 177 Phi[1][1] = 1; 178 179 Ba.x = 0; 180 Ba.y = 0; 181 Ba.z = 1; 182 183 Bm.x = 0; 184 Bm.y = 1; 185 Bm.z = 0; 186 187 x0 = 0; 188 x1 = 0; 189 190 init(); 191} 192 193void Fusion::init(int mode) { 194 mInitState = 0; 195 196 mGyroRate = 0; 197 198 mCount[0] = 0; 199 mCount[1] = 0; 200 mCount[2] = 0; 201 202 mData = 0; 203 mMode = mode; 204 205 if (mMode != FUSION_NOGYRO) { //normal or game rotation 206 mParam.gyroVar = DEFAULT_GYRO_VAR; 207 mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR; 208 mParam.accStdev = DEFAULT_ACC_STDEV; 209 mParam.magStdev = DEFAULT_MAG_STDEV; 210 } else { 211 mParam.gyroVar = GEOMAG_GYRO_VAR; 212 mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR; 213 mParam.accStdev = GEOMAG_ACC_STDEV; 214 mParam.magStdev = GEOMAG_MAG_STDEV; 215 } 216} 217 218void Fusion::initFusion(const vec4_t& q, float dT) 219{ 220 // initial estimate: E{ x(t0) } 221 x0 = q; 222 x1 = 0; 223 224 // process noise covariance matrix: G.Q.Gt, with 225 // 226 // G = | -1 0 | Q = | q00 q10 | 227 // | 0 1 | | q01 q11 | 228 // 229 // q00 = sv^2.dt + 1/3.su^2.dt^3 230 // q10 = q01 = 1/2.su^2.dt^2 231 // q11 = su^2.dt 232 // 233 234 const float dT2 = dT*dT; 235 const float dT3 = dT2*dT; 236 237 // variance of integrated output at 1/dT Hz (random drift) 238 const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3; 239 240 // variance of drift rate ramp 241 const float q11 = mParam.gyroBiasVar * dT; 242 const float q10 = 0.5f * mParam.gyroBiasVar * dT2; 243 const float q01 = q10; 244 245 GQGt[0][0] = q00; // rad^2 246 GQGt[1][0] = -q10; 247 GQGt[0][1] = -q01; 248 GQGt[1][1] = q11; // (rad/s)^2 249 250 // initial covariance: Var{ x(t0) } 251 // TODO: initialize P correctly 252 P = 0; 253} 254 255bool Fusion::hasEstimate() const { 256 return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) && 257 ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) && 258 (mInitState & ACC); 259} 260 261bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) { 262 if (hasEstimate()) 263 return true; 264 265 if (what == ACC) { 266 mData[0] += d * (1/length(d)); 267 mCount[0]++; 268 mInitState |= ACC; 269 if (mMode == FUSION_NOGYRO ) { 270 mGyroRate = dT; 271 } 272 } else if (what == MAG) { 273 mData[1] += d * (1/length(d)); 274 mCount[1]++; 275 mInitState |= MAG; 276 } else if (what == GYRO) { 277 mGyroRate = dT; 278 mData[2] += d*dT; 279 mCount[2]++; 280 mInitState |= GYRO; 281 } 282 283 if (hasEstimate()) { 284 // Average all the values we collected so far 285 mData[0] *= 1.0f/mCount[0]; 286 if (mMode != FUSION_NOMAG) { 287 mData[1] *= 1.0f/mCount[1]; 288 } 289 mData[2] *= 1.0f/mCount[2]; 290 291 // calculate the MRPs from the data collection, this gives us 292 // a rough estimate of our initial state 293 mat33_t R; 294 vec3_t up(mData[0]); 295 vec3_t east; 296 297 if (mMode != FUSION_NOMAG) { 298 east = normalize(cross_product(mData[1], up)); 299 } else { 300 east = getOrthogonal(up); 301 } 302 303 vec3_t north(cross_product(up, east)); 304 R << east << north << up; 305 const vec4_t q = matrixToQuat(R); 306 307 initFusion(q, mGyroRate); 308 } 309 310 return false; 311} 312 313void Fusion::handleGyro(const vec3_t& w, float dT) { 314 if (!checkInitComplete(GYRO, w, dT)) 315 return; 316 317 predict(w, dT); 318} 319 320status_t Fusion::handleAcc(const vec3_t& a, float dT) { 321 if (!checkInitComplete(ACC, a, dT)) 322 return BAD_VALUE; 323 324 // ignore acceleration data if we're close to free-fall 325 const float l = length(a); 326 if (l < FREE_FALL_THRESHOLD) { 327 return BAD_VALUE; 328 } 329 330 const float l_inv = 1.0f/l; 331 332 if ( mMode == FUSION_NOGYRO ) { 333 //geo mag 334 vec3_t w_dummy; 335 w_dummy = x1; //bias 336 predict(w_dummy, dT); 337 } 338 339 if ( mMode == FUSION_NOMAG) { 340 vec3_t m; 341 m = getRotationMatrix()*Bm; 342 update(m, Bm, mParam.magStdev); 343 } 344 345 vec3_t unityA = a * l_inv; 346 const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY)); 347 const float p = l_inv * mParam.accStdev*expf(d); 348 349 update(unityA, Ba, p); 350 return NO_ERROR; 351} 352 353status_t Fusion::handleMag(const vec3_t& m) { 354 if (!checkInitComplete(MAG, m)) 355 return BAD_VALUE; 356 357 // the geomagnetic-field should be between 30uT and 60uT 358 // reject if too large to avoid spurious magnetic sources 359 const float magFieldSq = length_squared(m); 360 if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) { 361 return BAD_VALUE; 362 } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) { 363 // Also reject if too small since we will get ill-defined (zero mag) 364 // cross-products below 365 return BAD_VALUE; 366 } 367 368 // Orthogonalize the magnetic field to the gravity field, mapping it into 369 // tangent to Earth. 370 const vec3_t up( getRotationMatrix() * Ba ); 371 const vec3_t east( cross_product(m, up) ); 372 373 // If the m and up vectors align, the cross product magnitude will 374 // approach 0. 375 // Reject this case as well to avoid div by zero problems and 376 // ill-conditioning below. 377 if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) { 378 return BAD_VALUE; 379 } 380 381 // If we have created an orthogonal magnetic field successfully, 382 // then pass it in as the update. 383 vec3_t north( cross_product(up, east) ); 384 385 const float l_inv = 1 / length(north); 386 north *= l_inv; 387 388 update(north, Bm, mParam.magStdev*l_inv); 389 return NO_ERROR; 390} 391 392void Fusion::checkState() { 393 // P needs to stay positive semidefinite or the fusion diverges. When we 394 // detect divergence, we reset the fusion. 395 // TODO(braun): Instead, find the reason for the divergence and fix it. 396 397 if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) || 398 !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) { 399 ALOGW("Sensor fusion diverged; resetting state."); 400 P = 0; 401 } 402} 403 404vec4_t Fusion::getAttitude() const { 405 return x0; 406} 407 408vec3_t Fusion::getBias() const { 409 return x1; 410} 411 412mat33_t Fusion::getRotationMatrix() const { 413 return quatToMatrix(x0); 414} 415 416mat34_t Fusion::getF(const vec4_t& q) { 417 mat34_t F; 418 419 // This is used to compute the derivative of q 420 // F = | [q.xyz]x | 421 // | -q.xyz | 422 423 F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y; 424 F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x; 425 F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w; 426 F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z; 427 return F; 428} 429 430void Fusion::predict(const vec3_t& w, float dT) { 431 const vec4_t q = x0; 432 const vec3_t b = x1; 433 vec3_t we = w - b; 434 435 if (length(we) < WVEC_EPS) { 436 we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS; 437 } 438 // q(k+1) = O(we)*q(k) 439 // -------------------- 440 // 441 // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi | 442 // | -psi' cos(0.5*||w||*dT) | 443 // 444 // psi = sin(0.5*||w||*dT)*w / ||w|| 445 // 446 // 447 // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G' 448 // ---------------------------------------- 449 // 450 // G = | -I33 0 | 451 // | 0 I33 | 452 // 453 // Phi = | Phi00 Phi10 | 454 // | 0 1 | 455 // 456 // Phi00 = I33 457 // - [w]x * sin(||w||*dt)/||w|| 458 // + [w]x^2 * (1-cos(||w||*dT))/||w||^2 459 // 460 // Phi10 = [w]x * (1 - cos(||w||*dt))/||w||^2 461 // - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3 462 // - I33*dT 463 464 const mat33_t I33(1); 465 const mat33_t I33dT(dT); 466 const mat33_t wx(crossMatrix(we, 0)); 467 const mat33_t wx2(wx*wx); 468 const float lwedT = length(we)*dT; 469 const float hlwedT = 0.5f*lwedT; 470 const float ilwe = 1.f/length(we); 471 const float k0 = (1-cosf(lwedT))*(ilwe*ilwe); 472 const float k1 = sinf(lwedT); 473 const float k2 = cosf(hlwedT); 474 const vec3_t psi(sinf(hlwedT)*ilwe*we); 475 const mat33_t O33(crossMatrix(-psi, k2)); 476 mat44_t O; 477 O[0].xyz = O33[0]; O[0].w = -psi.x; 478 O[1].xyz = O33[1]; O[1].w = -psi.y; 479 O[2].xyz = O33[2]; O[2].w = -psi.z; 480 O[3].xyz = psi; O[3].w = k2; 481 482 Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0; 483 Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1); 484 485 x0 = O*q; 486 487 if (x0.w < 0) 488 x0 = -x0; 489 490 P = Phi*P*transpose(Phi) + GQGt; 491 492 checkState(); 493} 494 495void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { 496 vec4_t q(x0); 497 // measured vector in body space: h(p) = A(p)*Bi 498 const mat33_t A(quatToMatrix(q)); 499 const vec3_t Bb(A*Bi); 500 501 // Sensitivity matrix H = dh(p)/dp 502 // H = [ L 0 ] 503 const mat33_t L(crossMatrix(Bb, 0)); 504 505 // gain... 506 // K = P*Ht / [H*P*Ht + R] 507 vec<mat33_t, 2> K; 508 const mat33_t R(sigma*sigma); 509 const mat33_t S(scaleCovariance(L, P[0][0]) + R); 510 const mat33_t Si(invert(S)); 511 const mat33_t LtSi(transpose(L)*Si); 512 K[0] = P[0][0] * LtSi; 513 K[1] = transpose(P[1][0])*LtSi; 514 515 // update... 516 // P = (I-K*H) * P 517 // P -= K*H*P 518 // | K0 | * | L 0 | * P = | K0*L 0 | * | P00 P10 | = | K0*L*P00 K0*L*P10 | 519 // | K1 | | K1*L 0 | | P01 P11 | | K1*L*P00 K1*L*P10 | 520 // Note: the Joseph form is numerically more stable and given by: 521 // P = (I-KH) * P * (I-KH)' + K*R*R' 522 const mat33_t K0L(K[0] * L); 523 const mat33_t K1L(K[1] * L); 524 P[0][0] -= K0L*P[0][0]; 525 P[1][1] -= K1L*P[1][0]; 526 P[1][0] -= K0L*P[1][0]; 527 P[0][1] = transpose(P[1][0]); 528 529 const vec3_t e(z - Bb); 530 const vec3_t dq(K[0]*e); 531 532 q += getF(q)*(0.5f*dq); 533 x0 = normalize_quat(q); 534 535 if (mMode != FUSION_NOMAG) { 536 const vec3_t db(K[1]*e); 537 x1 += db; 538 } 539 540 checkState(); 541} 542 543vec3_t Fusion::getOrthogonal(const vec3_t &v) { 544 vec3_t w; 545 if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) { 546 w[0]=0.f; 547 w[1] = v[2]; 548 w[2] = -v[1]; 549 } else if (fabsf(v[1]) <= fabsf(v[2])) { 550 w[0] = v[2]; 551 w[1] = 0.f; 552 w[2] = -v[0]; 553 }else { 554 w[0] = v[1]; 555 w[1] = -v[0]; 556 w[2] = 0.f; 557 } 558 return normalize(w); 559} 560 561 562// ----------------------------------------------------------------------- 563 564}; // namespace android 565 566