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    // variance of integrated output at 1/dT Hz
205    // (random drift)
206    const float q00 = gyroVAR * dT;
207
208    // variance of drift rate ramp
209    const float q11 = biasVAR * dT;
210
211    const float u   = q11 / dT;
212    const float q10 = 0.5f*u*dT*dT;
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
225bool Fusion::hasEstimate() const {
226    return (mInitState == (MAG|ACC|GYRO));
227}
228
229bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
230    if (hasEstimate())
231        return true;
232
233    if (what == ACC) {
234        mData[0] += d * (1/length(d));
235        mCount[0]++;
236        mInitState |= ACC;
237    } else if (what == MAG) {
238        mData[1] += d * (1/length(d));
239        mCount[1]++;
240        mInitState |= MAG;
241    } else if (what == GYRO) {
242        mGyroRate = dT;
243        mData[2] += d*dT;
244        mCount[2]++;
245        if (mCount[2] == 64) {
246            // 64 samples is good enough to estimate the gyro drift and
247            // doesn't take too much time.
248            mInitState |= GYRO;
249        }
250    }
251
252    if (mInitState == (MAG|ACC|GYRO)) {
253        // Average all the values we collected so far
254        mData[0] *= 1.0f/mCount[0];
255        mData[1] *= 1.0f/mCount[1];
256        mData[2] *= 1.0f/mCount[2];
257
258        // calculate the MRPs from the data collection, this gives us
259        // a rough estimate of our initial state
260        mat33_t R;
261        vec3_t up(mData[0]);
262        vec3_t east(cross_product(mData[1], up));
263        east *= 1/length(east);
264        vec3_t north(cross_product(up, east));
265        R << east << north << up;
266        const vec4_t q = matrixToQuat(R);
267
268        initFusion(q, mGyroRate);
269    }
270
271    return false;
272}
273
274void Fusion::handleGyro(const vec3_t& w, float dT) {
275    if (!checkInitComplete(GYRO, w, dT))
276        return;
277
278    predict(w, dT);
279}
280
281status_t Fusion::handleAcc(const vec3_t& a) {
282    // ignore acceleration data if we're close to free-fall
283    if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) {
284        return BAD_VALUE;
285    }
286
287    if (!checkInitComplete(ACC, a))
288        return BAD_VALUE;
289
290    const float l = 1/length(a);
291    update(a*l, Ba, accSTDEV*l);
292    return NO_ERROR;
293}
294
295status_t Fusion::handleMag(const vec3_t& m) {
296    // the geomagnetic-field should be between 30uT and 60uT
297    // reject if too large to avoid spurious magnetic sources
298    const float magFieldSq = length_squared(m);
299    if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) {
300        return BAD_VALUE;
301    } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
302        // Also reject if too small since we will get ill-defined (zero mag)
303        // cross-products below
304        return BAD_VALUE;
305    }
306
307    if (!checkInitComplete(MAG, m))
308        return BAD_VALUE;
309
310    // Orthogonalize the magnetic field to the gravity field, mapping it into
311    // tangent to Earth.
312    const vec3_t up( getRotationMatrix() * Ba );
313    const vec3_t east( cross_product(m, up) );
314
315    // If the m and up vectors align, the cross product magnitude will
316    // approach 0.
317    // Reject this case as well to avoid div by zero problems and
318    // ill-conditioning below.
319    if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
320        return BAD_VALUE;
321    }
322
323    // If we have created an orthogonal magnetic field successfully,
324    // then pass it in as the update.
325    vec3_t north( cross_product(up, east) );
326
327    const float l = 1 / length(north);
328    north *= l;
329
330    update(north, Bm, magSTDEV*l);
331    return NO_ERROR;
332}
333
334void Fusion::checkState() {
335    // P needs to stay positive semidefinite or the fusion diverges. When we
336    // detect divergence, we reset the fusion.
337    // TODO(braun): Instead, find the reason for the divergence and fix it.
338
339    if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) ||
340        !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) {
341        ALOGW("Sensor fusion diverged; resetting state.");
342        P = 0;
343    }
344}
345
346vec4_t Fusion::getAttitude() const {
347    return x0;
348}
349
350vec3_t Fusion::getBias() const {
351    return x1;
352}
353
354mat33_t Fusion::getRotationMatrix() const {
355    return quatToMatrix(x0);
356}
357
358mat34_t Fusion::getF(const vec4_t& q) {
359    mat34_t F;
360    F[0].x = q.w;   F[1].x =-q.z;   F[2].x = q.y;
361    F[0].y = q.z;   F[1].y = q.w;   F[2].y =-q.x;
362    F[0].z =-q.y;   F[1].z = q.x;   F[2].z = q.w;
363    F[0].w =-q.x;   F[1].w =-q.y;   F[2].w =-q.z;
364    return F;
365}
366
367void Fusion::predict(const vec3_t& w, float dT) {
368    const vec4_t q  = x0;
369    const vec3_t b  = x1;
370    const vec3_t we = w - b;
371    const vec4_t dq = getF(q)*((0.5f*dT)*we);
372    x0 = normalize_quat(q + dq);
373
374    // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
375    //
376    // G = | -I33    0 |
377    //     |    0  I33 |
378    //
379    //  Phi = | Phi00 Phi10 |
380    //        |   0     1   |
381    //
382    //  Phi00 =   I33
383    //          - [w]x   * sin(||w||*dt)/||w||
384    //          + [w]x^2 * (1-cos(||w||*dT))/||w||^2
385    //
386    //  Phi10 =   [w]x   * (1        - cos(||w||*dt))/||w||^2
387    //          - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
388    //          - I33*dT
389
390    const mat33_t I33(1);
391    const mat33_t I33dT(dT);
392    const mat33_t wx(crossMatrix(we, 0));
393    const mat33_t wx2(wx*wx);
394    const float lwedT = length(we)*dT;
395    const float ilwe = 1/length(we);
396    const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
397    const float k1 = sinf(lwedT);
398
399    Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
400    Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
401
402    P = Phi*P*transpose(Phi) + GQGt;
403
404    checkState();
405}
406
407void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
408    vec4_t q(x0);
409    // measured vector in body space: h(p) = A(p)*Bi
410    const mat33_t A(quatToMatrix(q));
411    const vec3_t Bb(A*Bi);
412
413    // Sensitivity matrix H = dh(p)/dp
414    // H = [ L 0 ]
415    const mat33_t L(crossMatrix(Bb, 0));
416
417    // gain...
418    // K = P*Ht / [H*P*Ht + R]
419    vec<mat33_t, 2> K;
420    const mat33_t R(sigma*sigma);
421    const mat33_t S(scaleCovariance(L, P[0][0]) + R);
422    const mat33_t Si(invert(S));
423    const mat33_t LtSi(transpose(L)*Si);
424    K[0] = P[0][0] * LtSi;
425    K[1] = transpose(P[1][0])*LtSi;
426
427    // update...
428    // P -= K*H*P;
429    const mat33_t K0L(K[0] * L);
430    const mat33_t K1L(K[1] * L);
431    P[0][0] -= K0L*P[0][0];
432    P[1][1] -= K1L*P[1][0];
433    P[1][0] -= K0L*P[1][0];
434    P[0][1] = transpose(P[1][0]);
435
436    const vec3_t e(z - Bb);
437    const vec3_t dq(K[0]*e);
438    const vec3_t db(K[1]*e);
439
440    q += getF(q)*(0.5f*dq);
441    x0 = normalize_quat(q);
442    x1 += db;
443
444    checkState();
445}
446
447// -----------------------------------------------------------------------
448
449}; // namespace android
450
451