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