ADHOC/replicant-frameworks_native
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

use quaternions instead of MRPs

Browse Source 
also use correct time propagation equation
disable the fused sensors when gyro is not present since
they were unusable in practice.

Change-Id: Iad797425784e67dc6c5690e97c71c583418cc5b5
This commit is contained in:
Mathias Agopian 2011-05-27 18:18:13 -07:00
parent 984826cc15
commit 3301542828
16 changed files with 366 additions and 548 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
services/sensorservice/Android.mk
View File

@ -8,7 +8,6 @@ LOCAL_SRC_FILES:= \
LinearAccelerationSensor.cpp \
OrientationSensor.cpp \
RotationVectorSensor.cpp \
SecondOrderLowPassFilter.cpp \
SensorDevice.cpp \
SensorFusion.cpp \
SensorInterface.cpp \

2
services/sensorservice/CorrectedGyroSensor.cpp
View File

@ -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;

273
services/sensorservice/Fusion.cpp
View File

@ -24,15 +24,14 @@ namespace android {
// -----------------------------------------------------------------------
template <typename TYPE>
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 <typename T>
static inline T clamp(T v) {
return v < 0 ? 0 : v;
}
static const float FREE_FALL_THRESHOLD = 0.981f;
// -----------------------------------------------------------------------
template <typename TYPE, size_t C, size_t R>
static mat<TYPE, R, R> scaleCovariance(
@ -71,33 +70,6 @@ static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
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 {
@ -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<mat33_t, 2> 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<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;
// 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;
}
// -----------------------------------------------------------------------

35
services/sensorservice/Fusion.h
View File

@ -19,42 +19,39 @@
#include <utils/Errors.h>
#include "vec.h"
#include "quat.h"
#include "mat.h"
#include "vec.h"
namespace android {
typedef mat<float, 3, 4> mat34_t;
class Fusion {
/*
* the state vector is made of two sub-vector containing respectively:
* - modified Rodrigues parameters
* - the estimated gyro bias
*/
vec<vec3_t, 2> 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<mat33_t, 2, 2> 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<mat33_t, 2> 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<mat33_t, 2, 2> 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<mat33_t, 2, 2> Phi;
vec3_t Ba, Bm;
uint32_t mInitState;
float mGyroRate;
vec<vec3_t, 3> 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

67
services/sensorservice/GravitySensor.cpp
View File

@ -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 ; i<count ; i++) {
if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
@ -50,30 +47,14 @@ bool GravitySensor::process(sensors_event_t* outEvent,
const static double NS2S = 1.0 / 1000000000.0;
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
vec3_t g;
if (mSensorFusion.hasGyro()) {
if (!mSensorFusion.hasEstimate())
return false;
const mat33_t R(mSensorFusion.getRotationMatrix());
// FIXME: we need to estimate the length of gravity because
// the accelerometer may have a small scaling error. This
// translates to an offset in the linear-acceleration sensor.
g = R[2] * GRAVITY_EARTH;
} else {
const double now = event.timestamp * NS2S;
if (mAccTime == 0) {
g.x = mX.init(event.acceleration.x);
g.y = mY.init(event.acceleration.y);
g.z = mZ.init(event.acceleration.z);
} else {
double dT = now - mAccTime;
mLowPass.setSamplingPeriod(dT);
g.x = mX(event.acceleration.x);
g.y = mY(event.acceleration.y);
g.z = mZ(event.acceleration.z);
}
g *= (GRAVITY_EARTH / length(g));
mAccTime = now;
}
if (!mSensorFusion.hasEstimate())
return false;
const mat33_t R(mSensorFusion.getRotationMatrix());
// FIXME: we need to estimate the length of gravity because
// the accelerometer may have a small scaling error. This
// translates to an offset in the linear-acceleration sensor.
g = R[2] * GRAVITY_EARTH;
*outEvent = event;
outEvent->data[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;
}

5
services/sensorservice/GravitySensor.h
View File

@ -23,7 +23,6 @@
#include <gui/Sensor.h>
#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<float> mX, mY, mZ;
public:
GravitySensor(sensor_t const* list, size_t count);

7
services/sensorservice/OrientationSensor.cpp
View File

@ -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;

83
services/sensorservice/RotationVectorSensor.cpp
View File

@ -27,11 +27,6 @@
namespace android {
// ---------------------------------------------------------------------------
template <typename T>
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

15
services/sensorservice/RotationVectorSensor.h
View File

@ -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

103
services/sensorservice/SecondOrderLowPassFilter.cpp
View File

@ -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 <stdint.h>
#include <sys/types.h>
#include <math.h>
#include <cutils/log.h>
#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<typename T>
BiquadFilter<T>::BiquadFilter(const SecondOrderLowPassFilter& s)
: s(s)
{
}
template<typename T>
T BiquadFilter<T>::init(const T& x)
{
x1 = x2 = x;
y1 = y2 = x;
return x;
}
template<typename T>
T BiquadFilter<T>::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<typename T>
CascadedBiquadFilter<T>::CascadedBiquadFilter(const SecondOrderLowPassFilter& s)
: mA(s), mB(s)
{
}
template<typename T>
T CascadedBiquadFilter<T>::init(const T& x)
{
mA.init(x);
mB.init(x);
return x;
}
template<typename T>
T CascadedBiquadFilter<T>::operator()(const T& x)
{
return mB(mA(x));
}
// ---------------------------------------------------------------------------
template class BiquadFilter<float>;
template class CascadedBiquadFilter<float>;
template class BiquadFilter<vec3_t>;
template class CascadedBiquadFilter<vec3_t>;
// ---------------------------------------------------------------------------
}; // namespace android

77
services/sensorservice/SecondOrderLowPassFilter.h
View File

@ -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 <stdint.h>
#include <sys/types.h>
// ---------------------------------------------------------------------------
namespace android {
// ---------------------------------------------------------------------------
template<typename T>
class BiquadFilter;
/*
* State of a 2nd order low-pass IIR filter
*/
class SecondOrderLowPassFilter {
template<typename T>
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<typename T>
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<typename T>
class CascadedBiquadFilter {
BiquadFilter<T> mA;
BiquadFilter<T> 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

82
services/sensorservice/SensorFusion.cpp
View File

@ -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);

14
services/sensorservice/SensorFusion.h
View File

@ -27,7 +27,6 @@
#include <gui/Sensor.h>
#include "Fusion.h"
#include "SecondOrderLowPassFilter.h"
// ---------------------------------------------------------------------------
@ -45,15 +44,10 @@ class SensorFusion : public Singleton<SensorFusion> {
Sensor mGyro;
Fusion mFusion;
bool mEnabled;
bool mHasGyro;
float mGyroRate;
nsecs_t mTargetDelayNs;
nsecs_t mGyroTime;
mat33_t mRotationMatrix;
SecondOrderLowPassFilter mLowPass;
BiquadFilter<vec3_t> mAccData;
vec3_t mFilteredMag;
vec3_t mFilteredAcc;
vec4_t mAttitude;
SortedVector<void*> 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; }

43
services/sensorservice/SensorService.cpp
View File

@ -18,6 +18,8 @@
#include <math.h>
#include <sys/types.h>
#include <cutils/properties.h>
#include <utils/SortedVector.h>
#include <utils/KeyedVector.h>
#include <utils/threads.h>
@ -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<<SENSOR_TYPE_GRAVITY) |
(1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
@ -69,6 +82,9 @@ void SensorService::onFirstRef()
for (int i=0 ; i<count ; i++) {
registerSensor( new HardwareSensor(list[i]) );
switch (list[i].type) {
case SENSOR_TYPE_GYROSCOPE:
hasGyro = true;
break;
case SENSOR_TYPE_GRAVITY:
case SENSOR_TYPE_LINEAR_ACCELERATION:
case SENSOR_TYPE_ROTATION_VECTOR:
@ -82,21 +98,26 @@ void SensorService::onFirstRef()
// registered)
const SensorFusion& fusion(SensorFusion::getInstance());
// Always instantiate Android's virtual sensors. Since they are
// instantiated behind sensors from the HAL, they won't
// interfere with applications, unless they looks specifically
// for them (by name).
if (hasGyro) {
// Always instantiate Android's virtual sensors. Since they are
// instantiated behind sensors from the HAL, they won't
// interfere with applications, unless they looks specifically
// for them (by name).
registerVirtualSensor( new RotationVectorSensor() );
registerVirtualSensor( new GravitySensor(list, count) );
registerVirtualSensor( new LinearAccelerationSensor(list, count) );
registerVirtualSensor( new RotationVectorSensor() );
registerVirtualSensor( new GravitySensor(list, count) );
registerVirtualSensor( new LinearAccelerationSensor(list, count) );
// if we have a gyro, we have the option of enabling these
// "better" orientation and gyro sensors
if (fusion.hasGyro()) {
// FIXME: OrientationSensor buggy when not pointing north
// these are optional
registerVirtualSensor( new OrientationSensor() );
registerVirtualSensor( new CorrectedGyroSensor(list, count) );
// virtual debugging sensors...
char value[PROPERTY_VALUE_MAX];
property_get("debug.sensors", value, "0");
if (atoi(value)) {
registerVirtualSensor( new GyroDriftSensor() );
}
}
run("SensorService", PRIORITY_URGENT_DISPLAY);

98
services/sensorservice/quat.h Normal file
View File

@ -0,0 +1,98 @@
/*
* 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.
*/
#ifndef ANDROID_QUAT_H
#define ANDROID_QUAT_H
#include <math.h>
#include "vec.h"
#include "mat.h"
// -----------------------------------------------------------------------
namespace android {
// -----------------------------------------------------------------------
template <typename TYPE>
mat<TYPE, 3, 3> quatToMatrix(const vec<TYPE, 4>& q) {
mat<TYPE, 3, 3> 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 <typename TYPE>
vec<TYPE, 4> matrixToQuat(const mat<TYPE, 3, 3>& R) {
// matrix to quaternion
struct {
inline TYPE operator()(TYPE v) {
return v < 0 ? 0 : v;
}
} clamp;
vec<TYPE, 4> 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 <typename TYPE>
vec<TYPE, 4> normalize_quat(const vec<TYPE, 4>& q) {
vec<TYPE, 4> r(q);
if (r.w < 0) {
r = -r;
}
return normalize(r);
}
// -----------------------------------------------------------------------
typedef vec4_t quat_t;
// -----------------------------------------------------------------------
}; // namespace android
#endif /* ANDROID_QUAT_H */

9
services/sensorservice/vec.h
View File

@ -207,6 +207,15 @@ TYPE PURE length(const V<TYPE, SIZE>& v) {
return sqrt(dot_product(v, v));
}
template <
template<typename T, size_t S> class V,
typename TYPE,
size_t SIZE
>
V<TYPE, SIZE> PURE normalize(const V<TYPE, SIZE>& v) {
return v * (1/length(v));
}
template <
template<typename T, size_t S> class VLHS,
template<typename T, size_t S> class VRHS,