/** Calculate the fused orientation. */
private void calculateFusedOrientation() {
float oneMinusCoeff = (1.0f - filterCoefficient);
// Apply the complementary filter. // We multiply each rotation by their
// coefficients (scalar matrices)...
// Scale our quaternion for the gyroscope
quatGyro = quatGyro.multiply(filterCoefficient);
// Scale our quaternion for the accel/mag
quatAccelMag = quatAccelMag.multiply(oneMinusCoeff);
// ...and then add the two quaternions together.
// output[0] = alpha * output[0] + (1 - alpha) * input[0];
quatGyro = quatGyro.add(quatAccelMag);
// Now we get a structure we can pass to get a rotation matrix, and then
// an orientation vector from Android.
fusedVector[0] = (float) quatGyro.getVectorPart()[0];
fusedVector[1] = (float) quatGyro.getVectorPart()[1];
fusedVector[2] = (float) quatGyro.getVectorPart()[2];
fusedVector[3] = (float) quatGyro.getScalarPart();
// We need a rotation matrix so we can get the orientation vector...
// Getting Euler
// angles from a quaternion is not trivial, so this is the easiest way,
// but perhaps
// not the fastest way of doing this.
SensorManager.getRotationMatrixFromVector(fusedMatrix, fusedVector);
// Get the fused orienatation
SensorManager.getOrientation(fusedMatrix, fusedOrientation);
}
/** Calculates orientation angles from accelerometer and magnetometer output. */
private void calculateOrientation() {
// To get the orientation vector from the acceleration and magnetic
// sensors, we let Android do the heavy lifting. This call will
// automatically compensate for the tilt of the compass and fail if the
// magnitude of the acceleration is not close to 9.82m/sec^2. You could
// perform these steps yourself, but in my opinion, this is the best way
// to do it.
if (SensorManager.getRotationMatrix(rotationMatrix, null, acceleration, magnetic)) {
SensorManager.getOrientation(rotationMatrix, baseOrientation);
getRotationVectorFromAccelMag(baseOrientation);
if (!hasOrientation) {
quatGyro = new Quaternion(quatAccelMag.getScalarPart(), quatAccelMag.getVectorPart());
}
hasOrientation = true;
}
}
