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