From 3301542828febc768e1df42892cfac4992c35474 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Fri, 27 May 2011 18:18:13 -0700 Subject: [PATCH] use quaternions instead of MRPs also use correct time propagation equation disable the fused sensors when gyro is not present since they were unusable in practice. Change-Id: Iad797425784e67dc6c5690e97c71c583418cc5b5 --- services/sensorservice/Android.mk | 1 - .../sensorservice/CorrectedGyroSensor.cpp | 2 +- services/sensorservice/Fusion.cpp | 273 ++++++------------ services/sensorservice/Fusion.h | 35 ++- services/sensorservice/GravitySensor.cpp | 67 +---- services/sensorservice/GravitySensor.h | 5 - services/sensorservice/OrientationSensor.cpp | 7 +- .../sensorservice/RotationVectorSensor.cpp | 83 ++++-- services/sensorservice/RotationVectorSensor.h | 15 +- .../SecondOrderLowPassFilter.cpp | 103 ------- .../sensorservice/SecondOrderLowPassFilter.h | 77 ----- services/sensorservice/SensorFusion.cpp | 82 ++---- services/sensorservice/SensorFusion.h | 14 +- services/sensorservice/SensorService.cpp | 43 ++- services/sensorservice/quat.h | 98 +++++++ services/sensorservice/vec.h | 9 + 16 files changed, 366 insertions(+), 548 deletions(-) delete mode 100644 services/sensorservice/SecondOrderLowPassFilter.cpp delete mode 100644 services/sensorservice/SecondOrderLowPassFilter.h create mode 100644 services/sensorservice/quat.h diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk index 57a3b1569..ba3e6e5bf 100644 --- a/services/sensorservice/Android.mk +++ b/services/sensorservice/Android.mk @@ -8,7 +8,6 @@ LOCAL_SRC_FILES:= \ LinearAccelerationSensor.cpp \ OrientationSensor.cpp \ RotationVectorSensor.cpp \ - SecondOrderLowPassFilter.cpp \ SensorDevice.cpp \ SensorFusion.cpp \ SensorInterface.cpp \ diff --git a/services/sensorservice/CorrectedGyroSensor.cpp b/services/sensorservice/CorrectedGyroSensor.cpp index 9b75b7024..1857443e8 100644 --- a/services/sensorservice/CorrectedGyroSensor.cpp +++ b/services/sensorservice/CorrectedGyroSensor.cpp @@ -45,7 +45,7 @@ bool CorrectedGyroSensor::process(sensors_event_t* outEvent, const sensors_event_t& event) { if (event.type == SENSOR_TYPE_GYROSCOPE) { - const vec3_t bias(mSensorFusion.getGyroBias() * mSensorFusion.getEstimatedRate()); + const vec3_t bias(mSensorFusion.getGyroBias()); *outEvent = event; outEvent->data[0] -= bias.x; outEvent->data[1] -= bias.y; diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index 56ac9f931..b5f97e035 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -24,15 +24,14 @@ namespace android { // ----------------------------------------------------------------------- -template -static inline TYPE sqr(TYPE x) { - return x*x; -} +static const float gyroSTDEV = 3.16e-4; // rad/s^3/2 +static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) +static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) +static const float biasSTDEV = 3.16e-5; // rad/s^1/2 (guessed) -template -static inline T clamp(T v) { - return v < 0 ? 0 : v; -} +static const float FREE_FALL_THRESHOLD = 0.981f; + +// ----------------------------------------------------------------------- template static mat scaleCovariance( @@ -71,33 +70,6 @@ static mat crossMatrix(const vec& p, OTHER_TYPE diag) { return r; } -template -static mat MRPsToMatrix(const vec& p) { - mat res(1); - const mat 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 -vec matrixToMRPs(const mat& R) { - // matrix to MRPs - vec 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 class Covariance { @@ -128,11 +100,8 @@ public: // ----------------------------------------------------------------------- Fusion::Fusion() { - // process noise covariance matrix - const float w1 = gyroSTDEV; - const float w2 = biasSTDEV; - Q[0] = w1*w1; - Q[1] = w2*w2; + Phi[0][1] = 0; + Phi[1][1] = 1; Ba.x = 0; Ba.y = 0; @@ -146,25 +115,46 @@ Fusion::Fusion() { } void Fusion::init() { - // initial estimate: E{ x(t0) } - x = 0; - - // initial covariance: Var{ x(t0) } - P = 0; - mInitState = 0; + mGyroRate = 0; mCount[0] = 0; mCount[1] = 0; mCount[2] = 0; mData = 0; } +void Fusion::initFusion(const vec4_t& q, float dT) +{ + // initial estimate: E{ x(t0) } + x0 = q; + x1 = 0; + + // process noise covariance matrix + // G = | -1 0 | + // | 0 1 | + + const float v = gyroSTDEV; + const float u = biasSTDEV; + const float q00 = v*v*dT + 0.33333f*(dT*dT*dT)*u*u; + const float q10 = 0.5f*(dT*dT) *u*u; + const float q01 = q10; + const float q11 = u*u*dT; + GQGt[0][0] = q00; + GQGt[1][0] = -q10; + GQGt[0][1] = -q01; + GQGt[1][1] = q11; + + + // initial covariance: Var{ x(t0) } + P = 0; +} + bool Fusion::hasEstimate() const { return (mInitState == (MAG|ACC|GYRO)); } -bool Fusion::checkInitComplete(int what, const vec3_t& d) { - if (mInitState == (MAG|ACC|GYRO)) +bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) { + if (hasEstimate()) return true; if (what == ACC) { @@ -176,7 +166,8 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) { mCount[1]++; mInitState |= MAG; } else if (what == GYRO) { - mData[2] += d; + mGyroRate = dT; + mData[2] += d*dT; mCount[2]++; if (mCount[2] == 64) { // 64 samples is good enough to estimate the gyro drift and @@ -199,37 +190,29 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) { east *= 1/length(east); vec3_t north(cross_product(up, east)); R << east << north << up; - x[0] = matrixToMRPs(R); + const vec4_t q = matrixToQuat(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; + initFusion(q, mGyroRate); } 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)) + if (!checkInitComplete(GYRO, w, dT)) return; - predict(wdT); + predict(w, dT); } status_t Fusion::handleAcc(const vec3_t& a) { - if (length(a) < 0.981f) + // ignore acceleration data if we're close to free-fall + if (length(a) < FREE_FALL_THRESHOLD) 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; @@ -251,20 +234,6 @@ status_t Fusion::handleMag(const vec3_t& m) { 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; } @@ -273,7 +242,7 @@ bool Fusion::checkState(const vec3_t& v) { if (isnanf(length(v))) { LOGW("9-axis fusion diverged. reseting state."); P = 0; - x[1] = 0; + x1 = 0; mInitState = 0; mCount[0] = 0; mCount[1] = 0; @@ -284,145 +253,89 @@ bool Fusion::checkState(const vec3_t& v) { return true; } -vec3_t Fusion::getAttitude() const { - return x[0]; +vec4_t Fusion::getAttitude() const { + return x0; } vec3_t Fusion::getBias() const { - return x[1]; + return x1; } mat33_t Fusion::getRotationMatrix() const { - return MRPsToMatrix(x[0]); + return quatToMatrix(x0); } -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); +mat34_t Fusion::getF(const vec4_t& q) { + mat34_t F; + F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y; + F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x; + F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w; + F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z; return F; } -mat33_t Fusion::getdFdp(const vec3_t& p, const vec3_t& we) { +void Fusion::predict(const vec3_t& w, float dT) { + const vec4_t q = x0; + const vec3_t b = x1; + const vec3_t we = w - b; + const vec4_t dq = getF(q)*((0.5f*dT)*we); + x0 = normalize_quat(q + dq); - // dF = | A = df/dp -F | - // | 0 0 | + // P(k+1) = F*P(k)*Ft + G*Q*Gt - 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; -} + // Phi = | Phi00 Phi10 | + // | 0 1 | + const mat33_t I33(1); + const mat33_t I33dT(dT); + const mat33_t wx(crossMatrix(we, 0)); + const mat33_t wx2(wx*wx); + const float lwedT = length(we)*dT; + const float ilwe = 1/length(we); + const float k0 = (1-cosf(lwedT))*(ilwe*ilwe); + const float k1 = sinf(lwedT); -void Fusion::predict(const vec3_t& w) { - // f(p, w) - vec3_t& p(x[0]); + Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0; + Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1); - // 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]; + P = Phi*P*transpose(Phi) + GQGt; } void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { - const vec3_t p(x[0]); + vec4_t q(x0); // measured vector in body space: h(p) = A(p)*Bi - const mat33_t A(MRPsToMatrix(p)); + const mat33_t A(quatToMatrix(q)); 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)); + const mat33_t L(crossMatrix(Bb, 0)); - // update... + // gain... + // K = P*Ht / [H*P*Ht + R] + vec K; 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 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; - + // update... // 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]; + P[0][1] = transpose(P[1][0]); + + const vec3_t e(z - Bb); + const vec3_t dq(K[0]*e); + const vec3_t db(K[1]*e); + + q += getF(q)*(0.5f*dq); + x0 = normalize_quat(q); + x1 += db; } // ----------------------------------------------------------------------- diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h index 571a41527..556944bbe 100644 --- a/services/sensorservice/Fusion.h +++ b/services/sensorservice/Fusion.h @@ -19,42 +19,39 @@ #include -#include "vec.h" +#include "quat.h" #include "mat.h" +#include "vec.h" namespace android { +typedef mat mat34_t; + class Fusion { /* * the state vector is made of two sub-vector containing respectively: * - modified Rodrigues parameters * - the estimated gyro bias */ - vec x; + quat_t x0; + vec3_t x1; /* * the predicated covariance matrix is made of 4 3x3 sub-matrices and it * semi-definite positive. * * P = | P00 P10 | = | P00 P10 | - * | P01 P11 | | P10t Q1 | + * | P01 P11 | | P10t P11 | * * Since P01 = transpose(P10), the code below never calculates or - * stores P01. P11 is always equal to Q1, so we don't store it either. + * stores P01. */ mat P; /* - * the process noise covariance matrix is made of 2 3x3 sub-matrices - * Q0 encodes the attitude's noise - * Q1 encodes the bias' noise + * the process noise covariance matrix */ - vec Q; - - static const float gyroSTDEV = 1.0e-5; // rad/s (measured 1.2e-5) - static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) - static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) - static const float biasSTDEV = 2e-9; // rad/s^2 (guessed) + mat GQGt; public: Fusion(); @@ -62,23 +59,25 @@ public: void handleGyro(const vec3_t& w, float dT); status_t handleAcc(const vec3_t& a); status_t handleMag(const vec3_t& m); - vec3_t getAttitude() const; + vec4_t getAttitude() const; vec3_t getBias() const; mat33_t getRotationMatrix() const; bool hasEstimate() const; private: + mat Phi; vec3_t Ba, Bm; uint32_t mInitState; + float mGyroRate; vec mData; size_t mCount[3]; enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; - bool checkInitComplete(int, const vec3_t&); + bool checkInitComplete(int, const vec3_t& w, float d = 0); + void initFusion(const vec4_t& q0, float dT); bool checkState(const vec3_t& v); - void predict(const vec3_t& w); + void predict(const vec3_t& w, float dT); void update(const vec3_t& z, const vec3_t& Bi, float sigma); - static mat33_t getF(const vec3_t& p); - static mat33_t getdFdp(const vec3_t& p, const vec3_t& we); + static mat34_t getF(const vec4_t& p); }; }; // namespace android diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp index 541fad26f..c57715f0d 100644 --- a/services/sensorservice/GravitySensor.cpp +++ b/services/sensorservice/GravitySensor.cpp @@ -31,10 +31,7 @@ namespace android { GravitySensor::GravitySensor(sensor_t const* list, size_t count) : mSensorDevice(SensorDevice::getInstance()), - mSensorFusion(SensorFusion::getInstance()), - mAccTime(0), - mLowPass(M_SQRT1_2, 1.5f), - mX(mLowPass), mY(mLowPass), mZ(mLowPass) + mSensorFusion(SensorFusion::getInstance()) { for (size_t i=0 ; idata[0] = g.x; outEvent->data[1] = g.y; @@ -86,42 +67,24 @@ bool GravitySensor::process(sensors_event_t* outEvent, } status_t GravitySensor::activate(void* ident, bool enabled) { - status_t err; - if (mSensorFusion.hasGyro()) { - err = mSensorFusion.activate(this, enabled); - } else { - err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled); - if (err == NO_ERROR) { - if (enabled) { - mAccTime = 0; - } - } - } - return err; + return mSensorFusion.activate(this, enabled); } -status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) -{ - if (mSensorFusion.hasGyro()) { - return mSensorFusion.setDelay(this, ns); - } else { - return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns); - } +status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) { + return mSensorFusion.setDelay(this, ns); } Sensor GravitySensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Gravity Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; + hwSensor.version = 3; hwSensor.handle = '_grv'; hwSensor.type = SENSOR_TYPE_GRAVITY; hwSensor.maxRange = GRAVITY_EARTH * 2; hwSensor.resolution = mAccelerometer.getResolution(); - hwSensor.power = mSensorFusion.hasGyro() ? - mSensorFusion.getPowerUsage() : mAccelerometer.getPowerUsage(); - hwSensor.minDelay = mSensorFusion.hasGyro() ? - mSensorFusion.getMinDelay() : mAccelerometer.getMinDelay(); + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.getMinDelay(); Sensor sensor(&hwSensor); return sensor; } diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h index 0ca3a3c0e..ac177c465 100644 --- a/services/sensorservice/GravitySensor.h +++ b/services/sensorservice/GravitySensor.h @@ -23,7 +23,6 @@ #include #include "SensorInterface.h" -#include "SecondOrderLowPassFilter.h" // --------------------------------------------------------------------------- namespace android { @@ -36,10 +35,6 @@ class GravitySensor : public SensorInterface { SensorDevice& mSensorDevice; SensorFusion& mSensorFusion; Sensor mAccelerometer; - double mAccTime; - - SecondOrderLowPassFilter mLowPass; - CascadedBiquadFilter mX, mY, mZ; public: GravitySensor(sensor_t const* list, size_t count); diff --git a/services/sensorservice/OrientationSensor.cpp b/services/sensorservice/OrientationSensor.cpp index c9e50803f..037adaa2e 100644 --- a/services/sensorservice/OrientationSensor.cpp +++ b/services/sensorservice/OrientationSensor.cpp @@ -50,9 +50,10 @@ bool OrientationSensor::process(sensors_event_t* outEvent, g[0] += 360; *outEvent = event; - outEvent->data[0] = g.x; - outEvent->data[1] = g.y; - outEvent->data[2] = g.z; + outEvent->orientation.azimuth = g.x; + outEvent->orientation.pitch = g.y; + outEvent->orientation.roll = g.z; + outEvent->orientation.status = SENSOR_STATUS_ACCURACY_HIGH; outEvent->sensor = '_ypr'; outEvent->type = SENSOR_TYPE_ORIENTATION; return true; diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp index cba89c9fc..5ea95683f 100644 --- a/services/sensorservice/RotationVectorSensor.cpp +++ b/services/sensorservice/RotationVectorSensor.cpp @@ -27,11 +27,6 @@ namespace android { // --------------------------------------------------------------------------- -template -static inline T clamp(T v) { - return v < 0 ? 0 : v; -} - RotationVectorSensor::RotationVectorSensor() : mSensorDevice(SensorDevice::getInstance()), mSensorFusion(SensorFusion::getInstance()) @@ -43,29 +38,12 @@ bool RotationVectorSensor::process(sensors_event_t* outEvent, { if (event.type == SENSOR_TYPE_ACCELEROMETER) { if (mSensorFusion.hasEstimate()) { - const mat33_t R(mSensorFusion.getRotationMatrix()); - - // matrix to rotation vector (normalized quaternion) - const float Hx = R[0].x; - const float My = R[1].y; - const float Az = R[2].z; - - float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f ); - float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ); - float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ); - float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ); - qx = copysignf(qx, R[2].y - R[1].z); - qy = copysignf(qy, R[0].z - R[2].x); - qz = copysignf(qz, R[1].x - R[0].y); - - // this quaternion is guaranteed to be normalized, by construction - // of the rotation matrix. - + const vec4_t q(mSensorFusion.getAttitude()); *outEvent = event; - outEvent->data[0] = qx; - outEvent->data[1] = qy; - outEvent->data[2] = qz; - outEvent->data[3] = qw; + outEvent->data[0] = q.x; + outEvent->data[1] = q.y; + outEvent->data[2] = q.z; + outEvent->data[3] = q.w; outEvent->sensor = '_rov'; outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; return true; @@ -86,7 +64,7 @@ Sensor RotationVectorSensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Rotation Vector Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; + hwSensor.version = 3; hwSensor.handle = '_rov'; hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR; hwSensor.maxRange = 1; @@ -97,6 +75,55 @@ Sensor RotationVectorSensor::getSensor() const { return sensor; } +// --------------------------------------------------------------------------- + +GyroDriftSensor::GyroDriftSensor() + : mSensorDevice(SensorDevice::getInstance()), + mSensorFusion(SensorFusion::getInstance()) +{ +} + +bool GyroDriftSensor::process(sensors_event_t* outEvent, + const sensors_event_t& event) +{ + if (event.type == SENSOR_TYPE_ACCELEROMETER) { + if (mSensorFusion.hasEstimate()) { + const vec3_t b(mSensorFusion.getGyroBias()); + *outEvent = event; + outEvent->data[0] = b.x; + outEvent->data[1] = b.y; + outEvent->data[2] = b.z; + outEvent->sensor = '_gbs'; + outEvent->type = SENSOR_TYPE_ACCELEROMETER; + return true; + } + } + return false; +} + +status_t GyroDriftSensor::activate(void* ident, bool enabled) { + return mSensorFusion.activate(this, enabled); +} + +status_t GyroDriftSensor::setDelay(void* ident, int handle, int64_t ns) { + return mSensorFusion.setDelay(this, ns); +} + +Sensor GyroDriftSensor::getSensor() const { + sensor_t hwSensor; + hwSensor.name = "Gyroscope Bias (debug)"; + hwSensor.vendor = "Google Inc."; + hwSensor.version = 1; + hwSensor.handle = '_gbs'; + hwSensor.type = SENSOR_TYPE_ACCELEROMETER; + hwSensor.maxRange = 1; + hwSensor.resolution = 1.0f / (1<<24); + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.getMinDelay(); + Sensor sensor(&hwSensor); + return sensor; +} + // --------------------------------------------------------------------------- }; // namespace android diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h index ac76487e4..bb97fe189 100644 --- a/services/sensorservice/RotationVectorSensor.h +++ b/services/sensorservice/RotationVectorSensor.h @@ -24,7 +24,6 @@ #include "SensorDevice.h" #include "SensorInterface.h" -#include "SecondOrderLowPassFilter.h" #include "Fusion.h" #include "SensorFusion.h" @@ -47,6 +46,20 @@ public: virtual bool isVirtual() const { return true; } }; +class GyroDriftSensor : public SensorInterface { + SensorDevice& mSensorDevice; + SensorFusion& mSensorFusion; + +public: + GyroDriftSensor(); + virtual bool process(sensors_event_t* outEvent, + const sensors_event_t& event); + virtual status_t activate(void* ident, bool enabled); + virtual status_t setDelay(void* ident, int handle, int64_t ns); + virtual Sensor getSensor() const; + virtual bool isVirtual() const { return true; } +}; + // --------------------------------------------------------------------------- }; // namespace android diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp deleted file mode 100644 index c76dd4cff..000000000 --- a/services/sensorservice/SecondOrderLowPassFilter.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (C) 2010 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 -#include -#include - -#include - -#include "SecondOrderLowPassFilter.h" -#include "vec.h" - -// --------------------------------------------------------------------------- - -namespace android { -// --------------------------------------------------------------------------- - -SecondOrderLowPassFilter::SecondOrderLowPassFilter(float Q, float fc) - : iQ(1.0f / Q), fc(fc) -{ -} - -void SecondOrderLowPassFilter::setSamplingPeriod(float dT) -{ - K = tanf(float(M_PI) * fc * dT); - iD = 1.0f / (K*K + K*iQ + 1); - a0 = K*K*iD; - a1 = 2.0f * a0; - b1 = 2.0f*(K*K - 1)*iD; - b2 = (K*K - K*iQ + 1)*iD; -} - -// --------------------------------------------------------------------------- - -template -BiquadFilter::BiquadFilter(const SecondOrderLowPassFilter& s) - : s(s) -{ -} - -template -T BiquadFilter::init(const T& x) -{ - x1 = x2 = x; - y1 = y2 = x; - return x; -} - -template -T BiquadFilter::operator()(const T& x) -{ - T y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2; - x2 = x1; - y2 = y1; - x1 = x; - y1 = y; - return y; -} - -// --------------------------------------------------------------------------- - -template -CascadedBiquadFilter::CascadedBiquadFilter(const SecondOrderLowPassFilter& s) - : mA(s), mB(s) -{ -} - -template -T CascadedBiquadFilter::init(const T& x) -{ - mA.init(x); - mB.init(x); - return x; -} - -template -T CascadedBiquadFilter::operator()(const T& x) -{ - return mB(mA(x)); -} - -// --------------------------------------------------------------------------- - -template class BiquadFilter; -template class CascadedBiquadFilter; -template class BiquadFilter; -template class CascadedBiquadFilter; - -// --------------------------------------------------------------------------- -}; // namespace android diff --git a/services/sensorservice/SecondOrderLowPassFilter.h b/services/sensorservice/SecondOrderLowPassFilter.h deleted file mode 100644 index 0cc2446cc..000000000 --- a/services/sensorservice/SecondOrderLowPassFilter.h +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright (C) 2010 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. - */ - -#ifndef ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H -#define ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H - -#include -#include - -// --------------------------------------------------------------------------- - -namespace android { -// --------------------------------------------------------------------------- - -template -class BiquadFilter; - -/* - * State of a 2nd order low-pass IIR filter - */ -class SecondOrderLowPassFilter { - template - friend class BiquadFilter; - float iQ, fc; - float K, iD; - float a0, a1; - float b1, b2; -public: - SecondOrderLowPassFilter(float Q, float fc); - void setSamplingPeriod(float dT); -}; - -/* - * Implements a Biquad IIR filter - */ -template -class BiquadFilter { - T x1, x2; - T y1, y2; - const SecondOrderLowPassFilter& s; -public: - BiquadFilter(const SecondOrderLowPassFilter& s); - T init(const T& in); - T operator()(const T& in); -}; - -/* - * Two cascaded biquad IIR filters - * (4-poles IIR) - */ -template -class CascadedBiquadFilter { - BiquadFilter mA; - BiquadFilter mB; -public: - CascadedBiquadFilter(const SecondOrderLowPassFilter& s); - T init(const T& in); - T operator()(const T& in); -}; - -// --------------------------------------------------------------------------- -}; // namespace android - -#endif // ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp index d4226ec11..4ec0c8cbd 100644 --- a/services/sensorservice/SensorFusion.cpp +++ b/services/sensorservice/SensorFusion.cpp @@ -25,9 +25,7 @@ ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion) SensorFusion::SensorFusion() : mSensorDevice(SensorDevice::getInstance()), - mEnabled(false), mHasGyro(false), mGyroTime(0), mRotationMatrix(1), - mLowPass(M_SQRT1_2, 1.0f), mAccData(mLowPass), - mFilteredMag(0.0f), mFilteredAcc(0.0f) + mEnabled(false), mGyroTime(0) { sensor_t const* list; size_t count = mSensorDevice.getSensorList(&list); @@ -42,55 +40,32 @@ SensorFusion::SensorFusion() mGyro = Sensor(list + i); // 200 Hz for gyro events is a good compromise between precision // and power/cpu usage. - mTargetDelayNs = 1000000000LL/200; - mGyroRate = 1000000000.0f / mTargetDelayNs; - mHasGyro = true; + mGyroRate = 200; + mTargetDelayNs = 1000000000LL/mGyroRate; } } mFusion.init(); - mAccData.init(vec3_t(0.0f)); } void SensorFusion::process(const sensors_event_t& event) { - - if (event.type == SENSOR_TYPE_GYROSCOPE && mHasGyro) { + if (event.type == SENSOR_TYPE_GYROSCOPE) { if (mGyroTime != 0) { const float dT = (event.timestamp - mGyroTime) / 1000000000.0f; const float freq = 1 / dT; - const float alpha = 2 / (2 + dT); // 2s time-constant - mGyroRate = mGyroRate*alpha + freq*(1 - alpha); + if (freq >= 100 && freq<1000) { // filter values obviously wrong + const float alpha = 1 / (1 + dT); // 1s time-constant + mGyroRate = freq + (mGyroRate - freq)*alpha; + } } mGyroTime = event.timestamp; mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate); } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { const vec3_t mag(event.data); - if (mHasGyro) { - mFusion.handleMag(mag); - } else { - const float l(length(mag)); - if (l>5 && l<100) { - mFilteredMag = mag * (1/l); - } - } + mFusion.handleMag(mag); } else if (event.type == SENSOR_TYPE_ACCELEROMETER) { const vec3_t acc(event.data); - if (mHasGyro) { - mFusion.handleAcc(acc); - mRotationMatrix = mFusion.getRotationMatrix(); - } else { - const float l(length(acc)); - if (l > 0.981f) { - // remove the linear-acceleration components - mFilteredAcc = mAccData(acc * (1/l)); - } - if (length(mFilteredAcc)>0 && length(mFilteredMag)>0) { - vec3_t up(mFilteredAcc); - vec3_t east(cross_product(mFilteredMag, up)); - east *= 1/length(east); - vec3_t north(cross_product(up, east)); - mRotationMatrix << east << north << up; - } - } + mFusion.handleAcc(acc); + mAttitude = mFusion.getAttitude(); } } @@ -116,40 +91,31 @@ status_t SensorFusion::activate(void* ident, bool enabled) { mSensorDevice.activate(ident, mAcc.getHandle(), enabled); mSensorDevice.activate(ident, mMag.getHandle(), enabled); - if (mHasGyro) { - mSensorDevice.activate(ident, mGyro.getHandle(), enabled); - } + mSensorDevice.activate(ident, mGyro.getHandle(), enabled); const bool newState = mClients.size() != 0; if (newState != mEnabled) { mEnabled = newState; if (newState) { mFusion.init(); + mGyroTime = 0; } } return NO_ERROR; } status_t SensorFusion::setDelay(void* ident, int64_t ns) { - if (mHasGyro) { - mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); - mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); - mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); - } else { - const static double NS2S = 1.0 / 1000000000.0; - mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); - mSensorDevice.setDelay(ident, mMag.getHandle(), max(ns, mMag.getMinDelayNs())); - mLowPass.setSamplingPeriod(ns*NS2S); - } + mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); + mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); + mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); return NO_ERROR; } float SensorFusion::getPowerUsage() const { - float power = mAcc.getPowerUsage() + mMag.getPowerUsage(); - if (mHasGyro) { - power += mGyro.getPowerUsage(); - } + float power = mAcc.getPowerUsage() + + mMag.getPowerUsage() + + mGyro.getPowerUsage(); return power; } @@ -159,17 +125,17 @@ int32_t SensorFusion::getMinDelay() const { void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) { const Fusion& fusion(mFusion); - snprintf(buffer, SIZE, "Fusion (%s) %s (%d clients), gyro-rate=%7.2fHz, " - "MRPS=< %g, %g, %g > (%g), " - "BIAS=< %g, %g, %g >\n", - mHasGyro ? "9-axis" : "6-axis", + snprintf(buffer, SIZE, "9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, " + "q=< %g, %g, %g, %g > (%g), " + "b=< %g, %g, %g >\n", mEnabled ? "enabled" : "disabled", mClients.size(), mGyroRate, fusion.getAttitude().x, fusion.getAttitude().y, fusion.getAttitude().z, - dot_product(fusion.getAttitude(), fusion.getAttitude()), + fusion.getAttitude().w, + length(fusion.getAttitude()), fusion.getBias().x, fusion.getBias().y, fusion.getBias().z); diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h index c7eab1262..4c99bcb75 100644 --- a/services/sensorservice/SensorFusion.h +++ b/services/sensorservice/SensorFusion.h @@ -27,7 +27,6 @@ #include #include "Fusion.h" -#include "SecondOrderLowPassFilter.h" // --------------------------------------------------------------------------- @@ -45,15 +44,10 @@ class SensorFusion : public Singleton { Sensor mGyro; Fusion mFusion; bool mEnabled; - bool mHasGyro; float mGyroRate; nsecs_t mTargetDelayNs; nsecs_t mGyroTime; - mat33_t mRotationMatrix; - SecondOrderLowPassFilter mLowPass; - BiquadFilter mAccData; - vec3_t mFilteredMag; - vec3_t mFilteredAcc; + vec4_t mAttitude; SortedVector mClients; SensorFusion(); @@ -62,9 +56,9 @@ public: void process(const sensors_event_t& event); bool isEnabled() const { return mEnabled; } - bool hasGyro() const { return mHasGyro; } - bool hasEstimate() const { return !mHasGyro || mFusion.hasEstimate(); } - mat33_t getRotationMatrix() const { return mRotationMatrix; } + bool hasEstimate() const { return mFusion.hasEstimate(); } + mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); } + vec4_t getAttitude() const { return mAttitude; } vec3_t getGyroBias() const { return mFusion.getBias(); } float getEstimatedRate() const { return mGyroRate; } diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 5b86d10d0..d1b10f799 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -18,6 +18,8 @@ #include #include +#include + #include #include #include @@ -46,6 +48,16 @@ namespace android { // --------------------------------------------------------------------------- +/* + * Notes: + * + * - what about a gyro-corrected magnetic-field sensor? + * - option to "hide" the HAL sensors + * - run mag sensor from time to time to force calibration + * - gravity sensor length is wrong (=> drift in linear-acc sensor) + * + */ + SensorService::SensorService() : mDump("android.permission.DUMP"), mInitCheck(NO_INIT) @@ -59,6 +71,7 @@ void SensorService::onFirstRef() SensorDevice& dev(SensorDevice::getInstance()); if (dev.initCheck() == NO_ERROR) { + bool hasGyro = false; uint32_t virtualSensorsNeeds = (1< + +#include "vec.h" +#include "mat.h" + +// ----------------------------------------------------------------------- +namespace android { +// ----------------------------------------------------------------------- + +template +mat quatToMatrix(const vec& q) { + mat R; + TYPE q0(q.w); + TYPE q1(q.x); + TYPE q2(q.y); + TYPE q3(q.z); + TYPE sq_q1 = 2 * q1 * q1; + TYPE sq_q2 = 2 * q2 * q2; + TYPE sq_q3 = 2 * q3 * q3; + TYPE q1_q2 = 2 * q1 * q2; + TYPE q3_q0 = 2 * q3 * q0; + TYPE q1_q3 = 2 * q1 * q3; + TYPE q2_q0 = 2 * q2 * q0; + TYPE q2_q3 = 2 * q2 * q3; + TYPE q1_q0 = 2 * q1 * q0; + R[0][0] = 1 - sq_q2 - sq_q3; + R[0][1] = q1_q2 - q3_q0; + R[0][2] = q1_q3 + q2_q0; + R[1][0] = q1_q2 + q3_q0; + R[1][1] = 1 - sq_q1 - sq_q3; + R[1][2] = q2_q3 - q1_q0; + R[2][0] = q1_q3 - q2_q0; + R[2][1] = q2_q3 + q1_q0; + R[2][2] = 1 - sq_q1 - sq_q2; + return R; +} + +template +vec matrixToQuat(const mat& R) { + // matrix to quaternion + + struct { + inline TYPE operator()(TYPE v) { + return v < 0 ? 0 : v; + } + } clamp; + + vec q; + const float Hx = R[0].x; + const float My = R[1].y; + const float Az = R[2].z; + q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ); + q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ); + q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ); + q.w = sqrtf( clamp( Hx + My + Az + 1) * 0.25f ); + 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); + // guaranteed to be unit-quaternion + return q; +} + +template +vec normalize_quat(const vec& q) { + vec r(q); + if (r.w < 0) { + r = -r; + } + return normalize(r); +} + +// ----------------------------------------------------------------------- + +typedef vec4_t quat_t; + +// ----------------------------------------------------------------------- +}; // namespace android + +#endif /* ANDROID_QUAT_H */ diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h index 736ff37b9..f74ccc579 100644 --- a/services/sensorservice/vec.h +++ b/services/sensorservice/vec.h @@ -207,6 +207,15 @@ TYPE PURE length(const V& v) { return sqrt(dot_product(v, v)); } +template < + template class V, + typename TYPE, + size_t SIZE +> +V PURE normalize(const V& v) { + return v * (1/length(v)); +} + template < template class VLHS, template class VRHS,