The document describes the android::Fusion class which performs sensor fusion to estimate attitude and gyro bias from gyroscope, accelerometer, and magnetometer sensors. The Fusion class contains public and private member functions for initialization, sensor data handling, prediction, updating the state estimate, and retrieving results. It uses quaternions to represent attitude and a Kalman filter to fuse the sensor data.

1. Android Sensor Fusion
Shereef Shehata

3. android::Fusion Class

4. android::Fusion Class
• Public Member Functions
Fusion ()
void init ()
void handleGyro (const vec3_t &w, float dT)
status_t handleAcc (const vec3_t &a)
status_t handleMag (const vec3_t &m)
vec4_t getAttitude () const
vec3_t getBias () const
mat33_t getRotationMatrix () const
bool hasEstimate () const

5. android::Fusion Class
• Private Types
ACC = 0x1
MAG = 0x2
GYRO = 0x4
enum { ACC = 0x1, MAG = 0x2, GYRO = 0x4 }

6. android::Fusion Class
• Private Member Functions
bool checkInitComplete (int, const vec3_t &w, float d=0)
void initFusion (const vec4_t &q0, float dT)
void checkState ()
void predict (const vec3_t &w, float dT)
void update (const vec3_t &z, const vec3_t &Bi, float sigma)

7. android::Fusion Class
• Static Private Member Functions
static mat34_t getF (const vec4_t &p)

8. android::Fusion Class
• Private Attributes
quat_t X0 Modified Rodrigues parameters
The function getAttitue() returns x0
vec3_t X1 The estimated gyro bias
The function getBias() returns x1
mat< mat33_t, 2, 2 >
P
The predicated covariance matrix is
made of 4 3x3 sub-matrices and it is
semi-definite positive.
mat< mat33_t, 2, 2 > GQGt The process noise covariance matrix
mat< mat33_t, 2, 2 > Phi
vec3_t Ba
vec3_t Bm
uint32_t mInitState
float mGyroRate
vec< vec3_t, 3 > mData
size_t mCount [3]

9. android::Fusion Class
Public Member Functions

10. Class Constructor android::Fusion::Fusion()
• Class Constructor
Fusion::Fusion() {
Phi[0][1] = 0;
Phi[1][1] = 1;
Ba.x = 0;
Ba.y = 0;
Ba.z = 1;
Bm.x = 0;
Bm.y = 1;
Bm.z = 0;
x0 = 0;
x1 = 0;
init();
}

11. android::Fusion::init()
• Init()
void Fusion::init() {
mInitState = 0;
mGyroRate = 0;
mCount[0] = 0;
mCount[1] = 0;
mCount[2] = 0;
mData = 0;
}

12. android::Fusion::handleGyro ()
• Caller Graph
• Call graph
void Fusion::handleGyro(const vec3_t& w, float dT) {
if (!checkInitComplete(GYRO, w, dT))
return;
predict(w, dT);
}

13. android::Fusion::handleAcc (const vec3_t& a )
• Caller graph
• Call graph

14. android::Fusion::handleAcc (const vec3_t& a )
status_t Fusion::handleAcc(const vec3_t& a) {
// ignore acceleration data if we're close to free-fall
if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) {
return BAD_VALUE;
}
if (!checkInitComplete(ACC, a))
return BAD_VALUE;
const float l = 1/length(a);
update(a*l, Ba, accSTDEV*l);
return NO_ERROR;
}

15. android::Fusion::handleMag (const vec3_t& m )
• Caller graph
• Call graph

16. android::Fusion::handleMag (const vec3_t& m )
status_t Fusion::handleMag(const vec3_t& m) {
// the geomagnetic-field should be between 30uT and 60uT
// reject if too large to avoid spurious magnetic sources
const float magFieldSq = length_squared(m);
if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) {
return BAD_VALUE;
} else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
// Also reject if too small since we will get ill-defined (zero mag)
// cross-products below
return BAD_VALUE;
}
if (!checkInitComplete(MAG, m))
return BAD_VALUE;
// Orthogonalize the magnetic field to the gravity field, mapping it into
// tangent to Earth.
const vec3_t up( getRotationMatrix() * Ba );
const vec3_t east( cross_product(m, up) );
// If the m and up vectors align, the cross product magnitude will
// approach 0.
// Reject this case as well to avoid div by zero problems and
// ill-conditioning below.
if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
return BAD_VALUE;
}
// If we have created an orthogonal magnetic field successfully,
// then pass it in as the update.
vec3_t north( cross_product(up, east) );
const float l = 1 / length(north);
north *= l;
update(north, Bm, magSTDEV*l);
return NO_ERROR;
}

17. android::Fusion::getAttitude()
• Caller graph
vec4_t Fusion::getAttitude() const {
return x0;
}

18. android::Fusion::getBias()
• Caller graph
vec3_t Fusion::getBias() const {
return x1;
}

19. android::Fusion::getRotationMatrix ()
• Caller graph
• Call graph

20. android::Fusion::hasEstimate () const
• Caller graph
• Call graph

21. android::Fusion Class
Private Member Functions

22. android::Fusion::checkInitComplete()
• Caller graph
• Call graph

23. android::Fusion::InitFusion()
• Caller graph
• Call graph

24. android::Fusion::checkState()
• Caller graph
• Call graph

25. android::Fusion::predict()
• Caller graph
• Call graph

26. 00372 void Fusion::predict(const vec3_t& w, float dT) {
00373 const vec4_t q = x0;
00374 const vec3_t b = x1;
00375 const vec3_t we = w - b;
00376
00377 // q(k+1) = O(we)*q(k)
00378 // --------------------
00379 //
00380 // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi |
00381 // | -psi' cos(0.5*||w||*dT) |
00382 //
00383 // psi = sin(0.5*||w||*dT)*w / ||w||
00384 //
00385 //
00386 // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
00387 // ----------------------------------------
00388 //
00389 // G = | -I33 0 |
00390 // | 0 I33 |
00391 //
00392 // Phi = | Phi00 Phi10 |
00393 // | 0 1 |
00394 //
00395 // Phi00 = I33
00396 // - [w]x * sin(||w||*dt)/||w||
00397 // + [w]x^2 * (1-cos(||w||*dT))/||w||^2
00398 //
00399 // Phi10 = [w]x * (1 - cos(||w||*dt))/||w||^2
00400 // - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
00401 // - I33*dT
00402
00403 const mat33_t I33(1);
00404 const mat33_t I33dT(dT);
00405 const mat33_t wx(crossMatrix(we, 0));
00406 const mat33_t wx2(wx*wx);
00407 const float lwedT = length(we)*dT;
00408 const float hlwedT = 0.5f*lwedT;
00409 const float ilwe = 1/length(we);
00410 const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
00411 const float k1 = sinf(lwedT);
00412 const float k2 = cosf(hlwedT);
00413 const vec3_t psi(sinf(hlwedT)*ilwe*we);
00414 const mat33_t O33(crossMatrix(-psi, k2));
00415 mat44_t O;
00416 O[0].xyz = O33[0]; O[0].w = -psi.x;
00417 O[1].xyz = O33[1]; O[1].w = -psi.y;
00418 O[2].xyz = O33[2]; O[2].w = -psi.z;
00419 O[3].xyz = psi; O[3].w = k2;
00420
00421 Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
00422 Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
00423
00424 x0 = O*q;
00425 if (x0.w < 0)
00426 x0 = -x0;
00427
00428 P = Phi*P*transpose(Phi) + GQGt;
00429
00430 checkState();
00431 }

27. android::Fusion::update()
• Caller graph
• Call graph

28. android:: Class
Helper Functions

29. android::Fusion::scaleCovariance ()
scaleCovariance (
const mat<TYPE, C, R>& A,
const mat<TYPE, C, C>& P) computes the following:
• A * P * transpose(A)

30. android::Fusion::scaleCovariance ()
• Caller graph
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;
}

31. android::Fusion::crossMatrix ()
crossMatix (
const vec<TYPE, 3>& p,
OTHER_TYPE diag) computes the following:
• Given a value “diag” and a vector p, with components X,Y,Z, it construct a 3x3
matrix as follows:
diag Z -Y
-Z diag X
Y -X diag

32. android::Fusion::crossMatrix ()
• Caller graph
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;
}

33. android::Fusion::quatToMatrix ()
quatToMatrix (const vec< TYPE, 4 > & q) computes the following:
• If the Quaternion operator qvq* (triple product quaternion) is applied to vector
v
• The equivalent matrix that produce the same result as the triple prod
• w = qvq* = Q v

34. android::Fusion::quatToMatrix ()
• Caller graph
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;
}

35. gyroVAR
gyroVAR gives the measured variance of the gyro's output per Hz (or variance at 1
Hz).
• This is an "intrinsic" parameter of the gyro, which is independent of the sampling
frequency.
• static const float gyroVAR = 1e-7; // (rad/s)^2 / Hz
• The variance of gyro's output at a given sampling period can be calculated as:
variance(T) = gyroVAR / T
* The variance of the INTEGRATED OUTPUT at a given sampling period can be
calculated as:
variance_integrate_output(T) = gyroVAR * T

36. Standard deviations of accelerometer and magnetometer
• Standard deviation of the Accelerometer
static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
• Standard deviation of the Magnetometer
static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5)

37. Accelerometer Updates at or near freefall
• Accelerometer updates will not be performed near free fall to avoid ill-
conditioning and div by zeros.
• Threshhold:
– 10% of g, in m/s^2
– Since g is 9.8 m/s^2
– 10% of g is 0.98 m/s^2
static const float FREE_FALL_THRESHOLD = 0.981f;
static const float FREE_FALL_THRESHOLD_SQ = FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD;
• The value of g
Location
Distance from Earth's center
(m)
Value of g
(m/s2)
Earth's surface 6.38 x 106 m 9.8

38. Standard deviations of accelerometer
• Accelerometer Standard Deviation, accSTDEV
static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
• How is the standard deviation of the Accelerometer measures?
• What is CCD?

39. Standard deviations of magnetometer
• Accelerometer Standard Deviation, magSTDEV
• static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5)
• How is the standard deviation of the magnetometer measures?
• What is CCD?

40. The geomagnetic-field, MAX values
• The geomagnetic-field should be between 30uT and 60uT.
• Fields strengths greater than this likely indicate a local magnetic disturbance which
we do not want to update into the fused frame.
static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT
static const float MAX_VALID_MAGNETIC_FIELD_SQ = MAX_VALID_MAGNETIC_FIELD * MAX_VALID_MAGNETIC_FIELD;

41. The geomagnetic-field, MIN values
• Values of the field smaller than this should be ignored in fusion to avoid ill-
conditioning.
• This state can happen with anomalous local magnetic disturbances canceling the
Earth field.
static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT
static const float MIN_VALID_MAGNETIC_FIELD_SQ = MIN_VALID_MAGNETIC_FIELD * MIN_VALID_MAGNETIC_FIELD;

42. Cross product of two vectors, Minimum Values
• If the cross product of two vectors has magnitude squared less than this, we reject
it as invalid due to alignment of the vectors.
• This threshold is used to check for the case where the magnetic field sample is
parallel to the gravity field, which can happen in certain places due to magnetic
field disturbances.
static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3;
static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ = MIN_VALID_CROSS_PRODUCT_MAG * MIN_VALID_CROSS_PRODUCT_MAG;