summaryrefslogtreecommitdiffstats
path: root/services/sensorservice/Fusion.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'services/sensorservice/Fusion.cpp')
-rw-r--r--services/sensorservice/Fusion.cpp431
1 files changed, 431 insertions, 0 deletions
diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
new file mode 100644
index 0000000..56ac9f9
--- /dev/null
+++ b/services/sensorservice/Fusion.cpp
@@ -0,0 +1,431 @@
+/*
+ * Copyright (C) 2011 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdio.h>
+
+#include <utils/Log.h>
+
+#include "Fusion.h"
+
+namespace android {
+
+// -----------------------------------------------------------------------
+
+template <typename TYPE>
+static inline TYPE sqr(TYPE x) {
+ return x*x;
+}
+
+template <typename T>
+static inline T clamp(T v) {
+ return v < 0 ? 0 : v;
+}
+
+template <typename TYPE, size_t C, size_t R>
+static mat<TYPE, R, R> scaleCovariance(
+ const mat<TYPE, C, R>& A,
+ const mat<TYPE, C, C>& P) {
+ // A*P*transpose(A);
+ mat<TYPE, R, R> APAt;
+ for (size_t r=0 ; r<R ; r++) {
+ for (size_t j=r ; j<R ; j++) {
+ double apat(0);
+ for (size_t c=0 ; c<C ; c++) {
+ double v(A[c][r]*P[c][c]*0.5);
+ for (size_t k=c+1 ; k<C ; k++)
+ v += A[k][r] * P[c][k];
+ apat += 2 * v * A[c][j];
+ }
+ APAt[j][r] = apat;
+ APAt[r][j] = apat;
+ }
+ }
+ return APAt;
+}
+
+template <typename TYPE, typename OTHER_TYPE>
+static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
+ mat<TYPE, 3, 3> r;
+ r[0][0] = diag;
+ r[1][1] = diag;
+ r[2][2] = diag;
+ r[0][1] = p.z;
+ r[1][0] =-p.z;
+ r[0][2] =-p.y;
+ r[2][0] = p.y;
+ r[1][2] = p.x;
+ r[2][1] =-p.x;
+ return r;
+}
+
+template <typename TYPE>
+static mat<TYPE, 3, 3> MRPsToMatrix(const vec<TYPE, 3>& p) {
+ mat<TYPE, 3, 3> res(1);
+ const mat<TYPE, 3, 3> px(crossMatrix(p, 0));
+ const TYPE ptp(dot_product(p,p));
+ const TYPE t = 4/sqr(1+ptp);
+ res -= t * (1-ptp) * px;
+ res += t * 2 * sqr(px);
+ return res;
+}
+
+template <typename TYPE>
+vec<TYPE, 3> matrixToMRPs(const mat<TYPE, 3, 3>& R) {
+ // matrix to MRPs
+ vec<TYPE, 3> q;
+ const float Hx = R[0].x;
+ const float My = R[1].y;
+ const float Az = R[2].z;
+ const float w = 1 / (1 + sqrtf( clamp( Hx + My + Az + 1) * 0.25f ));
+ q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ) * w;
+ q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ) * w;
+ q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ) * w;
+ q.x = copysignf(q.x, R[2].y - R[1].z);
+ q.y = copysignf(q.y, R[0].z - R[2].x);
+ q.z = copysignf(q.z, R[1].x - R[0].y);
+ return q;
+}
+
+template<typename TYPE, size_t SIZE>
+class Covariance {
+ mat<TYPE, SIZE, SIZE> mSumXX;
+ vec<TYPE, SIZE> mSumX;
+ size_t mN;
+public:
+ Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
+ void update(const vec<TYPE, SIZE>& x) {
+ mSumXX += x*transpose(x);
+ mSumX += x;
+ mN++;
+ }
+ mat<TYPE, SIZE, SIZE> operator()() const {
+ const float N = 1.0f / mN;
+ return mSumXX*N - (mSumX*transpose(mSumX))*(N*N);
+ }
+ void reset() {
+ mN = 0;
+ mSumXX = 0;
+ mSumX = 0;
+ }
+ size_t getCount() const {
+ return mN;
+ }
+};
+
+// -----------------------------------------------------------------------
+
+Fusion::Fusion() {
+ // process noise covariance matrix
+ const float w1 = gyroSTDEV;
+ const float w2 = biasSTDEV;
+ Q[0] = w1*w1;
+ Q[1] = w2*w2;
+
+ Ba.x = 0;
+ Ba.y = 0;
+ Ba.z = 1;
+
+ Bm.x = 0;
+ Bm.y = 1;
+ Bm.z = 0;
+
+ init();
+}
+
+void Fusion::init() {
+ // initial estimate: E{ x(t0) }
+ x = 0;
+
+ // initial covariance: Var{ x(t0) }
+ P = 0;
+
+ mInitState = 0;
+ mCount[0] = 0;
+ mCount[1] = 0;
+ mCount[2] = 0;
+ mData = 0;
+}
+
+bool Fusion::hasEstimate() const {
+ return (mInitState == (MAG|ACC|GYRO));
+}
+
+bool Fusion::checkInitComplete(int what, const vec3_t& d) {
+ if (mInitState == (MAG|ACC|GYRO))
+ return true;
+
+ if (what == ACC) {
+ mData[0] += d * (1/length(d));
+ mCount[0]++;
+ mInitState |= ACC;
+ } else if (what == MAG) {
+ mData[1] += d * (1/length(d));
+ mCount[1]++;
+ mInitState |= MAG;
+ } else if (what == GYRO) {
+ mData[2] += d;
+ mCount[2]++;
+ if (mCount[2] == 64) {
+ // 64 samples is good enough to estimate the gyro drift and
+ // doesn't take too much time.
+ mInitState |= GYRO;
+ }
+ }
+
+ if (mInitState == (MAG|ACC|GYRO)) {
+ // Average all the values we collected so far
+ mData[0] *= 1.0f/mCount[0];
+ mData[1] *= 1.0f/mCount[1];
+ mData[2] *= 1.0f/mCount[2];
+
+ // calculate the MRPs from the data collection, this gives us
+ // a rough estimate of our initial state
+ mat33_t R;
+ vec3_t up(mData[0]);
+ vec3_t east(cross_product(mData[1], up));
+ east *= 1/length(east);
+ vec3_t north(cross_product(up, east));
+ R << east << north << up;
+ x[0] = matrixToMRPs(R);
+
+ // NOTE: we could try to use the average of the gyro data
+ // to estimate the initial bias, but this only works if
+ // the device is not moving. For now, we don't use that value
+ // and start with a bias of 0.
+ x[1] = 0;
+
+ // initial covariance
+ P = 0;
+ }
+
+ return false;
+}
+
+void Fusion::handleGyro(const vec3_t& w, float dT) {
+ const vec3_t wdT(w * dT); // rad/s * s -> rad
+ if (!checkInitComplete(GYRO, wdT))
+ return;
+
+ predict(wdT);
+}
+
+status_t Fusion::handleAcc(const vec3_t& a) {
+ if (length(a) < 0.981f)
+ return BAD_VALUE;
+
+ if (!checkInitComplete(ACC, a))
+ return BAD_VALUE;
+
+ // ignore acceleration data if we're close to free-fall
+ const float l = 1/length(a);
+ update(a*l, Ba, accSTDEV*l);
+ return NO_ERROR;
+}
+
+status_t Fusion::handleMag(const vec3_t& m) {
+ // the geomagnetic-field should be between 30uT and 60uT
+ // reject obviously wrong magnetic-fields
+ if (length(m) > 100)
+ return BAD_VALUE;
+
+ if (!checkInitComplete(MAG, m))
+ return BAD_VALUE;
+
+ const vec3_t up( getRotationMatrix() * Ba );
+ const vec3_t east( cross_product(m, up) );
+ vec3_t north( cross_product(up, east) );
+
+ const float l = 1 / length(north);
+ north *= l;
+
+#if 0
+ // in practice the magnetic-field sensor is so wrong
+ // that there is no point trying to use it to constantly
+ // correct the gyro. instead, we use the mag-sensor only when
+ // the device points north (just to give us a reference).
+ // We're hoping that it'll actually point north, if it doesn't
+ // we'll be offset, but at least the instantaneous posture
+ // of the device will be correct.
+
+ const float cos_30 = 0.8660254f;
+ if (dot_product(north, Bm) < cos_30)
+ return BAD_VALUE;
+#endif
+
+ update(north, Bm, magSTDEV*l);
+ return NO_ERROR;
+}
+
+bool Fusion::checkState(const vec3_t& v) {
+ if (isnanf(length(v))) {
+ LOGW("9-axis fusion diverged. reseting state.");
+ P = 0;
+ x[1] = 0;
+ mInitState = 0;
+ mCount[0] = 0;
+ mCount[1] = 0;
+ mCount[2] = 0;
+ mData = 0;
+ return false;
+ }
+ return true;
+}
+
+vec3_t Fusion::getAttitude() const {
+ return x[0];
+}
+
+vec3_t Fusion::getBias() const {
+ return x[1];
+}
+
+mat33_t Fusion::getRotationMatrix() const {
+ return MRPsToMatrix(x[0]);
+}
+
+mat33_t Fusion::getF(const vec3_t& p) {
+ const float p0 = p.x;
+ const float p1 = p.y;
+ const float p2 = p.z;
+
+ // f(p, w)
+ const float p0p1 = p0*p1;
+ const float p0p2 = p0*p2;
+ const float p1p2 = p1*p2;
+ const float p0p0 = p0*p0;
+ const float p1p1 = p1*p1;
+ const float p2p2 = p2*p2;
+ const float pp = 0.5f * (1 - (p0p0 + p1p1 + p2p2));
+
+ mat33_t F;
+ F[0][0] = 0.5f*(p0p0 + pp);
+ F[0][1] = 0.5f*(p0p1 + p2);
+ F[0][2] = 0.5f*(p0p2 - p1);
+ F[1][0] = 0.5f*(p0p1 - p2);
+ F[1][1] = 0.5f*(p1p1 + pp);
+ F[1][2] = 0.5f*(p1p2 + p0);
+ F[2][0] = 0.5f*(p0p2 + p1);
+ F[2][1] = 0.5f*(p1p2 - p0);
+ F[2][2] = 0.5f*(p2p2 + pp);
+ return F;
+}
+
+mat33_t Fusion::getdFdp(const vec3_t& p, const vec3_t& we) {
+
+ // dF = | A = df/dp -F |
+ // | 0 0 |
+
+ mat33_t A;
+ A[0][0] = A[1][1] = A[2][2] = 0.5f * (p.x*we.x + p.y*we.y + p.z*we.z);
+ A[0][1] = 0.5f * (p.y*we.x - p.x*we.y - we.z);
+ A[0][2] = 0.5f * (p.z*we.x - p.x*we.z + we.y);
+ A[1][2] = 0.5f * (p.z*we.y - p.y*we.z - we.x);
+ A[1][0] = -A[0][1];
+ A[2][0] = -A[0][2];
+ A[2][1] = -A[1][2];
+ return A;
+}
+
+void Fusion::predict(const vec3_t& w) {
+ // f(p, w)
+ vec3_t& p(x[0]);
+
+ // There is a discontinuity at 2.pi, to avoid it we need to switch to
+ // the shadow of p when pT.p gets too big.
+ const float ptp(dot_product(p,p));
+ if (ptp >= 2.0f) {
+ p = -p * (1/ptp);
+ }
+
+ const mat33_t F(getF(p));
+
+ // compute w with the bias correction:
+ // w_estimated = w - b_estimated
+ const vec3_t& b(x[1]);
+ const vec3_t we(w - b);
+
+ // prediction
+ const vec3_t dX(F*we);
+
+ if (!checkState(dX))
+ return;
+
+ p += dX;
+
+ const mat33_t A(getdFdp(p, we));
+
+ // G = | G0 0 | = | -F 0 |
+ // | 0 1 | | 0 1 |
+
+ // P += A*P + P*At + F*Q*Ft
+ const mat33_t AP(A*transpose(P[0][0]));
+ const mat33_t PAt(P[0][0]*transpose(A));
+ const mat33_t FPSt(F*transpose(P[1][0]));
+ const mat33_t PSFt(P[1][0]*transpose(F));
+ const mat33_t FQFt(scaleCovariance(F, Q[0]));
+ P[0][0] += AP + PAt - FPSt - PSFt + FQFt;
+ P[1][0] += A*P[1][0] - F*P[1][1];
+ P[1][1] += Q[1];
+}
+
+void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
+ const vec3_t p(x[0]);
+ // measured vector in body space: h(p) = A(p)*Bi
+ const mat33_t A(MRPsToMatrix(p));
+ const vec3_t Bb(A*Bi);
+
+ // Sensitivity matrix H = dh(p)/dp
+ // H = [ L 0 ]
+ const float ptp(dot_product(p,p));
+ const mat33_t px(crossMatrix(p, 0.5f*(ptp-1)));
+ const mat33_t ppt(p*transpose(p));
+ const mat33_t L((8 / sqr(1+ptp))*crossMatrix(Bb, 0)*(ppt-px));
+
+ // update...
+ const mat33_t R(sigma*sigma);
+ const mat33_t S(scaleCovariance(L, P[0][0]) + R);
+ const mat33_t Si(invert(S));
+ const mat33_t LtSi(transpose(L)*Si);
+
+ vec<mat33_t, 2> K;
+ K[0] = P[0][0] * LtSi;
+ K[1] = transpose(P[1][0])*LtSi;
+
+ const vec3_t e(z - Bb);
+ const vec3_t K0e(K[0]*e);
+ const vec3_t K1e(K[1]*e);
+
+ if (!checkState(K0e))
+ return;
+
+ if (!checkState(K1e))
+ return;
+
+ x[0] += K0e;
+ x[1] += K1e;
+
+ // P -= K*H*P;
+ const mat33_t K0L(K[0] * L);
+ const mat33_t K1L(K[1] * L);
+ P[0][0] -= K0L*P[0][0];
+ P[1][1] -= K1L*P[1][0];
+ P[1][0] -= K0L*P[1][0];
+}
+
+// -----------------------------------------------------------------------
+
+}; // namespace android
+
generated by cgit v1.1 at 2025-11-28 08:29:06 +0000