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
23 namespace 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 */
50 static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz
51 static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed)
52 static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz
53 static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed)
54
55 /*
56 * Standard deviations of accelerometer and magnetometer
57 */
58 static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)
59 static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
60 static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
61 static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
62
63
64 /* ====================== END FUSION SENSOR PARAMETER ========================*/
65
66 static 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 */
73 static const float NOMINAL_GRAVITY = 9.81f;
74 static 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 */
81 static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT
82 static 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 */
90 static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT
91 static 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 */
101 static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3;
102 static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
103 MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;
104
105 static const float SQRT_3 = 1.732f;
106 static const float WVEC_EPS = 1e-4f/SQRT_3;
107 // -----------------------------------------------------------------------
108
109 template <typename TYPE, size_t C, size_t R>
scaleCovariance(const mat<TYPE,C,R> & A,const mat<TYPE,C,C> & P)110 static 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
131 template <typename TYPE, typename OTHER_TYPE>
crossMatrix(const vec<TYPE,3> & p,OTHER_TYPE diag)132 static 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
147 template<typename TYPE, size_t SIZE>
148 class Covariance {
149 mat<TYPE, SIZE, SIZE> mSumXX;
150 vec<TYPE, SIZE> mSumX;
151 size_t mN;
152 public:
Covariance()153 Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
update(const vec<TYPE,SIZE> & x)154 void update(const vec<TYPE, SIZE>& x) {
155 mSumXX += x*transpose(x);
156 mSumX += x;
157 mN++;
158 }
operator ()() const159 mat<TYPE, SIZE, SIZE> operator()() const {
160 const float N = 1.0f / mN;
161 return mSumXX*N - (mSumX*transpose(mSumX))*(N*N);
162 }
reset()163 void reset() {
164 mN = 0;
165 mSumXX = 0;
166 mSumX = 0;
167 }
getCount() const168 size_t getCount() const {
169 return mN;
170 }
171 };
172
173 // -----------------------------------------------------------------------
174
Fusion()175 Fusion::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
init(int mode)193 void 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
initFusion(const vec4_t & q,float dT)218 void 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
hasEstimate() const255 bool Fusion::hasEstimate() const {
256 return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) &&
257 ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) &&
258 (mInitState & ACC);
259 }
260
checkInitComplete(int what,const vec3_t & d,float dT)261 bool 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
handleGyro(const vec3_t & w,float dT)313 void Fusion::handleGyro(const vec3_t& w, float dT) {
314 if (!checkInitComplete(GYRO, w, dT))
315 return;
316
317 predict(w, dT);
318 }
319
handleAcc(const vec3_t & a,float dT)320 status_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
handleMag(const vec3_t & m)353 status_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
checkState()392 void 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
getAttitude() const404 vec4_t Fusion::getAttitude() const {
405 return x0;
406 }
407
getBias() const408 vec3_t Fusion::getBias() const {
409 return x1;
410 }
411
getRotationMatrix() const412 mat33_t Fusion::getRotationMatrix() const {
413 return quatToMatrix(x0);
414 }
415
getF(const vec4_t & q)416 mat34_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
predict(const vec3_t & w,float dT)430 void 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
update(const vec3_t & z,const vec3_t & Bi,float sigma)495 void 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
getOrthogonal(const vec3_t & v)543 vec3_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