Fusion.cpp revision a01b4e237d57b74689576a3d486a2b2b903e74f4 
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 FREE_FALL_THRESHOLD = 0.981f;
51static const float SYMMETRY_TOLERANCE = 1e-10f;
52
53// -----------------------------------------------------------------------
54
55template <typename TYPE, size_t C, size_t R>
56static mat<TYPE, R, R> scaleCovariance(
57        const mat<TYPE, C, R>& A,
58        const mat<TYPE, C, C>& P) {
59    // A*P*transpose(A);
60    mat<TYPE, R, R> APAt;
61    for (size_t r=0 ; r<R ; r++) {
62        for (size_t j=r ; j<R ; j++) {
63            double apat(0);
64            for (size_t c=0 ; c<C ; c++) {
65                double v(A[c][r]*P[c][c]*0.5);
66                for (size_t k=c+1 ; k<C ; k++)
67                    v += A[k][r] * P[c][k];
68                apat += 2 * v * A[c][j];
69            }
70            APAt[j][r] = apat;
71            APAt[r][j] = apat;
72        }
73    }
74    return APAt;
75}
76
77template <typename TYPE, typename OTHER_TYPE>
78static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
79    mat<TYPE, 3, 3> r;
80    r[0][0] = diag;
81    r[1][1] = diag;
82    r[2][2] = diag;
83    r[0][1] = p.z;
84    r[1][0] =-p.z;
85    r[0][2] =-p.y;
86    r[2][0] = p.y;
87    r[1][2] = p.x;
88    r[2][1] =-p.x;
89    return r;
90}
91
92
93template<typename TYPE, size_t SIZE>
94class Covariance {
95    mat<TYPE, SIZE, SIZE> mSumXX;
96    vec<TYPE, SIZE> mSumX;
97    size_t mN;
98public:
99    Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
100    void update(const vec<TYPE, SIZE>& x) {
101        mSumXX += x*transpose(x);
102        mSumX  += x;
103        mN++;
104    }
105    mat<TYPE, SIZE, SIZE> operator()() const {
106        const float N = 1.0f / mN;
107        return mSumXX*N - (mSumX*transpose(mSumX))*(N*N);
108    }
109    void reset() {
110        mN = 0;
111        mSumXX = 0;
112        mSumX = 0;
113    }
114    size_t getCount() const {
115        return mN;
116    }
117};
118
119// -----------------------------------------------------------------------
120
121Fusion::Fusion() {
122    Phi[0][1] = 0;
123    Phi[1][1] = 1;
124
125    Ba.x = 0;
126    Ba.y = 0;
127    Ba.z = 1;
128
129    Bm.x = 0;
130    Bm.y = 1;
131    Bm.z = 0;
132
133    init();
134}
135
136void Fusion::init() {
137    mInitState = 0;
138
139    mGyroRate = 0;
140
141    mCount[0] = 0;
142    mCount[1] = 0;
143    mCount[2] = 0;
144
145    mData = 0;
146}
147
148void Fusion::initFusion(const vec4_t& q, float dT)
149{
150    // initial estimate: E{ x(t0) }
151    x0 = q;
152    x1 = 0;
153
154    // process noise covariance matrix: G.Q.Gt, with
155    //
156    //  G = | -1 0 |        Q = | q00 q10 |
157    //      |  0 1 |            | q01 q11 |
158    //
159    // q00 = sv^2.dt + 1/3.su^2.dt^3
160    // q10 = q01 = 1/2.su^2.dt^2
161    // q11 = su^2.dt
162    //
163
164    // variance of integrated output at 1/dT Hz
165    // (random drift)
166    const float q00 = gyroVAR * dT;
167
168    // variance of drift rate ramp
169    const float q11 = biasVAR * dT;
170
171    const float u   = q11 / dT;
172    const float q10 = 0.5f*u*dT*dT;
173    const float q01 = q10;
174
175    GQGt[0][0] =  q00;      // rad^2
176    GQGt[1][0] = -q10;
177    GQGt[0][1] = -q01;
178    GQGt[1][1] =  q11;      // (rad/s)^2
179
180    // initial covariance: Var{ x(t0) }
181    // TODO: initialize P correctly
182    P = 0;
183}
184
185bool Fusion::hasEstimate() const {
186    return (mInitState == (MAG|ACC|GYRO));
187}
188
189bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
190    if (hasEstimate())
191        return true;
192
193    if (what == ACC) {
194        mData[0] += d * (1/length(d));
195        mCount[0]++;
196        mInitState |= ACC;
197    } else if (what == MAG) {
198        mData[1] += d * (1/length(d));
199        mCount[1]++;
200        mInitState |= MAG;
201    } else if (what == GYRO) {
202        mGyroRate = dT;
203        mData[2] += d*dT;
204        mCount[2]++;
205        if (mCount[2] == 64) {
206            // 64 samples is good enough to estimate the gyro drift and
207            // doesn't take too much time.
208            mInitState |= GYRO;
209        }
210    }
211
212    if (mInitState == (MAG|ACC|GYRO)) {
213        // Average all the values we collected so far
214        mData[0] *= 1.0f/mCount[0];
215        mData[1] *= 1.0f/mCount[1];
216        mData[2] *= 1.0f/mCount[2];
217
218        // calculate the MRPs from the data collection, this gives us
219        // a rough estimate of our initial state
220        mat33_t R;
221        vec3_t up(mData[0]);
222        vec3_t east(cross_product(mData[1], up));
223        east *= 1/length(east);
224        vec3_t north(cross_product(up, east));
225        R << east << north << up;
226        const vec4_t q = matrixToQuat(R);
227
228        initFusion(q, mGyroRate);
229    }
230
231    return false;
232}
233
234void Fusion::handleGyro(const vec3_t& w, float dT) {
235    if (!checkInitComplete(GYRO, w, dT))
236        return;
237
238    predict(w, dT);
239}
240
241status_t Fusion::handleAcc(const vec3_t& a) {
242    // ignore acceleration data if we're close to free-fall
243    if (length(a) < FREE_FALL_THRESHOLD)
244        return BAD_VALUE;
245
246    if (!checkInitComplete(ACC, a))
247        return BAD_VALUE;
248
249    const float l = 1/length(a);
250    update(a*l, Ba, accSTDEV*l);
251    return NO_ERROR;
252}
253
254status_t Fusion::handleMag(const vec3_t& m) {
255    // the geomagnetic-field should be between 30uT and 60uT
256    // reject obviously wrong magnetic-fields
257    if (length(m) > 100)
258        return BAD_VALUE;
259
260    if (!checkInitComplete(MAG, m))
261        return BAD_VALUE;
262
263    const vec3_t up( getRotationMatrix() * Ba );
264    const vec3_t east( cross_product(m, up) );
265    vec3_t north( cross_product(up, east) );
266
267    const float l = 1 / length(north);
268    north *= l;
269
270    update(north, Bm, magSTDEV*l);
271    return NO_ERROR;
272}
273
274void Fusion::checkState() {
275    // P needs to stay positive semidefinite or the fusion diverges. When we
276    // detect divergence, we reset the fusion.
277    // TODO(braun): Instead, find the reason for the divergence and fix it.
278
279    if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) ||
280        !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) {
281        LOGW("Sensor fusion diverged; resetting state.");
282        P = 0;
283    }
284}
285
286vec4_t Fusion::getAttitude() const {
287    return x0;
288}
289
290vec3_t Fusion::getBias() const {
291    return x1;
292}
293
294mat33_t Fusion::getRotationMatrix() const {
295    return quatToMatrix(x0);
296}
297
298mat34_t Fusion::getF(const vec4_t& q) {
299    mat34_t F;
300    F[0].x = q.w;   F[1].x =-q.z;   F[2].x = q.y;
301    F[0].y = q.z;   F[1].y = q.w;   F[2].y =-q.x;
302    F[0].z =-q.y;   F[1].z = q.x;   F[2].z = q.w;
303    F[0].w =-q.x;   F[1].w =-q.y;   F[2].w =-q.z;
304    return F;
305}
306
307void Fusion::predict(const vec3_t& w, float dT) {
308    const vec4_t q  = x0;
309    const vec3_t b  = x1;
310    const vec3_t we = w - b;
311    const vec4_t dq = getF(q)*((0.5f*dT)*we);
312    x0 = normalize_quat(q + dq);
313
314    // P(k+1) = F*P(k)*Ft + G*Q*Gt
315
316    //  Phi = | Phi00 Phi10 |
317    //        |   0     1   |
318    const mat33_t I33(1);
319    const mat33_t I33dT(dT);
320    const mat33_t wx(crossMatrix(we, 0));
321    const mat33_t wx2(wx*wx);
322    const float lwedT = length(we)*dT;
323    const float ilwe = 1/length(we);
324    const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
325    const float k1 = sinf(lwedT);
326
327    Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
328    Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
329
330    P = Phi*P*transpose(Phi) + GQGt;
331
332    checkState();
333}
334
335void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
336    vec4_t q(x0);
337    // measured vector in body space: h(p) = A(p)*Bi
338    const mat33_t A(quatToMatrix(q));
339    const vec3_t Bb(A*Bi);
340
341    // Sensitivity matrix H = dh(p)/dp
342    // H = [ L 0 ]
343    const mat33_t L(crossMatrix(Bb, 0));
344
345    // gain...
346    // K = P*Ht / [H*P*Ht + R]
347    vec<mat33_t, 2> K;
348    const mat33_t R(sigma*sigma);
349    const mat33_t S(scaleCovariance(L, P[0][0]) + R);
350    const mat33_t Si(invert(S));
351    const mat33_t LtSi(transpose(L)*Si);
352    K[0] = P[0][0] * LtSi;
353    K[1] = transpose(P[1][0])*LtSi;
354
355    // update...
356    // P -= K*H*P;
357    const mat33_t K0L(K[0] * L);
358    const mat33_t K1L(K[1] * L);
359    P[0][0] -= K0L*P[0][0];
360    P[1][1] -= K1L*P[1][0];
361    P[1][0] -= K0L*P[1][0];
362    P[0][1] = transpose(P[1][0]);
363
364    const vec3_t e(z - Bb);
365    const vec3_t dq(K[0]*e);
366    const vec3_t db(K[1]*e);
367
368    q += getF(q)*(0.5f*dq);
369    x0 = normalize_quat(q);
370    x1 += db;
371
372    checkState();
373}
374
375// -----------------------------------------------------------------------
376
377}; // namespace android
378
379