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