Develop Reference

Detect Device to Near or away from Ear Android code

I need to know whether device is near to ear or not by using sensors
I tried using proximity, I want to combine accelerator and gyroscope sensors to exactly find the device is near or far from Ear.
Code for Proximity
#Override
public void onSensorChanged(SensorEvent event) {
float distance = event.values[0];
if (event.sensor.getType() == Sensor.TYPE_PROXIMITY) {
if (distance < mProximity.getMaximumRange()) {
iv.setText("Near");
} else {
iv.setText("far");
}
}
}

This is what I got from Android Documentation, I am sure you can dig more to get some answers to your problem, but this should be enough to get you started. You can also do some research about position sensors in android. The documentation is quite useful
// Create a constant to convert nanoseconds to seconds.
private static final float NS2S = 1.0f / 1000000000.0f;
private final float[] deltaRotationVector = new float[4]();
private float timestamp;
public void onSensorChanged(SensorEvent event) {
// This timestep's delta rotation to be multiplied by the current rotation
// after computing it from the gyro sample data.
if (timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
// Axis of the rotation sample, not normalized yet.
float axisX = event.values[0];
float axisY = event.values[1];
float axisZ = event.values[2];
// Calculate the angular speed of the sample
float omegaMagnitude = sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);
// Normalize the rotation vector if it's big enough to get the axis
// (that is, EPSILON should represent your maximum allowable margin of error)
if (omegaMagnitude > EPSILON) {
axisX /= omegaMagnitude;
axisY /= omegaMagnitude;
axisZ /= omegaMagnitude;
}
// Integrate around this axis with the angular speed by the timestep
// in order to get a delta rotation from this sample over the timestep
// We will convert this axis-angle representation of the delta rotation
// into a quaternion before turning it into the rotation matrix.
float thetaOverTwo = omegaMagnitude * dT / 2.0f;
float sinThetaOverTwo = sin(thetaOverTwo);
float cosThetaOverTwo = cos(thetaOverTwo);
deltaRotationVector[0] = sinThetaOverTwo * axisX;
deltaRotationVector[1] = sinThetaOverTwo * axisY;
deltaRotationVector[2] = sinThetaOverTwo * axisZ;
deltaRotationVector[3] = cosThetaOverTwo;
}
timestamp = event.timestamp;
float[] deltaRotationMatrix = new float[9];
SensorManager.getRotationMatrixFromVector(deltaRotationMatrix,
deltaRotationVector);
// User code should concatenate the delta rotation we computed with the current rotation
// in order to get the updated rotation.
// rotationCurrent = rotationCurrent * deltaRotationMatrix;
}
}

2D values from Quaternion

i'm working in a AR application in android with the Epson Moverio BT-200.
I have a quaternion that change his values with my sensor fusion algorithm.
In my application i'm trying to move a 2D item changing his margin left and margin top values when I move my head.
I'd like to know how can I extract, from the quaternion values, only the "horizontal" and "vertical" movements.
I could extract from the quaternion the pitch and roll values, but I read that there are several problems with euler angle. Could I do this only working with quaternions?

This is my actual code. I solved the problem using the Quaternions for the algorithm, and at the end I extract the euler angles from the rotation matrix.
This is the algorithm for take the values from the sensors:
private static final float NS2S = 1.0f / 1000000000.0f;
private final Quaternion deltaQuaternion = new Quaternion();
private Quaternion quaternionGyroscope = new Quaternion();
private Quaternion quaternionRotationVector = new Quaternion();
private long timestamp;
private static final double EPSILON = 0.1f;
private double gyroscopeRotationVelocity = 0;
private boolean positionInitialised = false;
private int panicCounter;
private static final float DIRECT_INTERPOLATION_WEIGHT = 0.005f;
private static final float OUTLIER_THRESHOLD = 0.85f;
private static final float OUTLIER_PANIC_THRESHOLD = 0.65f;
private static final int PANIC_THRESHOLD = 60;
#Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
// Process rotation vector (just safe it)
float[] q = new float[4];
// Calculate angle. Starting with API_18, Android will provide this value as event.values[3], but if not, we have to calculate it manually.
SensorManager.getQuaternionFromVector(q, event.values);
// Store in quaternion
quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
if (!positionInitialised) {
// Override
quaternionGyroscope.set(quaternionRotationVector);
positionInitialised = true;
}
} else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
// Process Gyroscope and perform fusion
// This timestep's delta rotation to be multiplied by the current rotation
// after computing it from the gyro sample data.
if (timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
// Axis of the rotation sample, not normalized yet.
float axisX = event.values[0];
float axisY = event.values[1];
float axisZ = event.values[2];
// Calculate the angular speed of the sample
gyroscopeRotationVelocity = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
// Normalize the rotation vector if it's big enough to get the axis
if (gyroscopeRotationVelocity > EPSILON) {
axisX /= gyroscopeRotationVelocity;
axisY /= gyroscopeRotationVelocity;
axisZ /= gyroscopeRotationVelocity;
}
// Integrate around this axis with the angular speed by the timestep
// in order to get a delta rotation from this sample over the timestep
// We will convert this axis-angle representation of the delta rotation
// into a quaternion before turning it into the rotation matrix.
double thetaOverTwo = gyroscopeRotationVelocity * dT / 2.0f;
double sinThetaOverTwo = Math.sin(thetaOverTwo);
double cosThetaOverTwo = Math.cos(thetaOverTwo);
deltaQuaternion.setX((float) (sinThetaOverTwo * axisX));
deltaQuaternion.setY((float) (sinThetaOverTwo * axisY));
deltaQuaternion.setZ((float) (sinThetaOverTwo * axisZ));
deltaQuaternion.setW(-(float) cosThetaOverTwo);
// Move current gyro orientation
deltaQuaternion.multiplyByQuat(quaternionGyroscope, quaternionGyroscope);
// Calculate dot-product to calculate whether the two orientation sensors have diverged
// (if the dot-product is closer to 0 than to 1), because it should be close to 1 if both are the same.
float dotProd = quaternionGyroscope.dotProduct(quaternionRotationVector);
// If they have diverged, rely on gyroscope only (this happens on some devices when the rotation vector "jumps").
if (Math.abs(dotProd) < OUTLIER_THRESHOLD) {
// Increase panic counter
if (Math.abs(dotProd) < OUTLIER_PANIC_THRESHOLD) {
panicCounter++;
}
// Directly use Gyro
setOrientationQuaternionAndMatrix(quaternionGyroscope);
} else {
// Both are nearly saying the same. Perform normal fusion.
// Interpolate with a fixed weight between the two absolute quaternions obtained from gyro and rotation vector sensors
// The weight should be quite low, so the rotation vector corrects the gyro only slowly, and the output keeps responsive.
Quaternion interpolate = new Quaternion();
quaternionGyroscope.slerp(quaternionRotationVector, interpolate, DIRECT_INTERPOLATION_WEIGHT);
// Use the interpolated value between gyro and rotationVector
setOrientationQuaternionAndMatrix(interpolate);
// Override current gyroscope-orientation
quaternionGyroscope.copyVec4(interpolate);
// Reset the panic counter because both sensors are saying the same again
panicCounter = 0;
}
if (panicCounter > PANIC_THRESHOLD) {
Log.d("Rotation Vector",
"Panic counter is bigger than threshold; this indicates a Gyroscope failure. Panic reset is imminent.");
if (gyroscopeRotationVelocity < 3) {
Log.d("Rotation Vector",
"Performing Panic-reset. Resetting orientation to rotation-vector value.");
// Manually set position to whatever rotation vector says.
setOrientationQuaternionAndMatrix(quaternionRotationVector);
// Override current gyroscope-orientation with corrected value
quaternionGyroscope.copyVec4(quaternionRotationVector);
panicCounter = 0;
} else {
Log.d("Rotation Vector",
String.format(
"Panic reset delayed due to ongoing motion (user is still shaking the device). Gyroscope Velocity: %.2f > 3",
gyroscopeRotationVelocity));
}
}
}
timestamp = event.timestamp;
}
}
private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
Quaternion correctedQuat = quaternion.clone();
// We inverted w in the deltaQuaternion, because currentOrientationQuaternion required it.
// Before converting it back to matrix representation, we need to revert this process
correctedQuat.w(-correctedQuat.w());
synchronized (syncToken) {
// Use gyro only
currentOrientationQuaternion.copyVec4(quaternion);
// Set the rotation matrix as well to have both representations
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.ToArray());
}
}
And this is how I take the euler angles rotation values:
/**
* #return Returns the current rotation of the device in the Euler-Angles
*/
public EulerAngles getEulerAngles() {
float[] angles = new float[3];
float[] remappedOrientationMatrix = new float[16];
SensorManager.remapCoordinateSystem(currentOrientationRotationMatrix.getMatrix(), SensorManager.AXIS_X,
SensorManager.AXIS_Z, remappedOrientationMatrix);
SensorManager.getOrientation(remappedOrientationMatrix, angles);
return new EulerAngles(angles[0], angles[1], angles[2]);
}
I solved my problem with this solution. Now won't be difficult to move my 2d Object with this sensors values. Sorry for lenght of my answer, but I hope that it could be useful for someone :)

Removing Noise from recorded accelerometer/gyroscope data

I have seen some answers to how reduce the noise of for example the accelerometer x,y,z values while listening, but my problem is a bit different.
I have some recorded data already (in csv-files) and I would like to remove/reduce the noise afterwards, if that's possible.
Here is the data that was recorded:
X,Y,Z from gyroscope
Delta 0-3 from gyroscope, which was calculated in this way:
axisX = 0;
axisY = 0;
axisZ = 0;
// This timestep's delta rotation to be multiplied by the
// current rotation
// after computing it from the gyro sample data.
if (timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
// Axis of the rotation sample, not normalized yet.
axisX = event.values[0];
axisY = event.values[1];
axisZ = event.values[2];
// Calculate the angular speed of the sample
float omegaMagnitude = FloatMath.sqrt(axisX * axisX + axisY
* axisY + axisZ * axisZ);
// Normalize the rotation vector if it's big enough to get
// the axis (that is, EPSILON should represent your maximum
// allowable margin of error)
if (omegaMagnitude > 0.000000001f) {
axisX /= omegaMagnitude;
axisY /= omegaMagnitude;
axisZ /= omegaMagnitude;
}
// Integrate around this axis with the angular speed by the
// timestep in order to get a delta rotation from this
// sample over the timestep We will convert this axis-angle
// representation of the delta rotation into a quaternion
// before turning it into the rotation matrix.
float thetaOverTwo = omegaMagnitude * dT / 2.0f;
float sinThetaOverTwo = FloatMath.sin(thetaOverTwo);
float cosThetaOverTwo = FloatMath.cos(thetaOverTwo);
deltaRotationVector[0] = sinThetaOverTwo * axisX;
deltaRotationVector[1] = sinThetaOverTwo * axisY;
deltaRotationVector[2] = sinThetaOverTwo * axisZ;
deltaRotationVector[3] = cosThetaOverTwo;
}
timestamp = event.timestamp;
float[] deltaRotationMatrix = new float[9];
SensorManager.getRotationMatrixFromVector(deltaRotationMatrix,deltaRotationVector);
Pitch/Roll/Azimuth/Inclination, which was calculated in this way:
// Calculation of the orientation through the
// magnetic-field and accelerometer sensors.
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
mGravity = event.values;
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
mGeomagnetic = event.values;
if (mGravity != null && mGeomagnetic != null) {
float R[] = new float[9];
float I[] = new float[9];
boolean success = SensorManager.getRotationMatrix(R, I, mGravity, mGeomagnetic);
if (success) {
float orientation[] = new float[3];
SensorManager.getOrientation(R, orientation);
// get the current orientation
// orientation consist of: azimut, pitch and roll in radians
azimut = orientation[0] * (180 / (float) java.lang.Math.PI);
pitch = orientation[1] * (180 / (float) java.lang.Math.PI);
roll = orientation[2] * (180 / (float) java.lang.Math.PI);
inclination = SensorManager.getInclination(I) * (180 / (float) java.lang.Math.PI);
}
}
The X/Y/Z from accelerometer wasn't written in the files.
So my question is:
Can I remove the noise from this data?
Thanks in advance.

I do not know if it is too late for you, just write in case you still need it.
You can implement some kind of filter with it. Low pass filter is typical. Otherwise, try Complementary Filter.
Me personally I preferred Kalman Filter, although it is a bit computationally expensive.

Since you don't have the accelerometer recorded, and if I understand correctly what you use is the orientation. I would recommend converting the euler angles to quaternions representation and using averaging to smooth the data, this is not regular averaging, see below.
You can implement rolling window filter by averaging using this this matlab code example:
https://stackoverflow.com/a/29315869/6589074
All the best,
Lev

Compute rotation matrix using the magnetic field

In get rotation matrix value it contains public static boolean getRotationMatrix (float[] R, float[] I, float[] gravity, float[] geomagnetic)
Here how can i calculate the float[] gravity?
I found a sample of code where it calculate the orientation using both Accelerometer and Magnetic field
boolean success = SensorManager.getRotationMatrix(
matrixR,
matrixI,
valuesAccelerometer,
valuesMagneticField);
if(success){
SensorManager.getOrientation(matrixR, matrixValues);
double azimuth = Math.toDegrees(matrixValues[0]);
double pitch = Math.toDegrees(matrixValues[1]);
double roll = Math.toDegrees(matrixValues[2]);
readingAzimuth.setText("Azimuth: " + String.valueOf(azimuth));
readingPitch.setText("Pitch: " + String.valueOf(pitch));
readingRoll.setText("Roll: "+String.valueOf(roll));
}
My questions are :
Is orientation value is the rotation matrix value?
If no then how can i implement this code to get the rotation matrix value using magnetic? field?
To get the rotation matrix i use this code
public void onSensorChanged(SensorEvent sensorEvent) {
if (timestamp != 0) {
final double dT = (sensorEvent.timestamp - timestamp) * NS2S;
double magneticX = sensorEvent.values[0];
double magneticY = sensorEvent.values[1];
double magneticZ = sensorEvent.values[2];
double omegaMagnitude =Math.sqrt(magneticX*magneticX + magneticY*magneticY + magneticZ*magneticZ);
if (omegaMagnitude > EPSILON) {
magneticX /= omegaMagnitude;
magneticY /= omegaMagnitude;
magneticZ /= omegaMagnitude;
}
double thetaOverTwo = omegaMagnitude * dT / 2.0f;
double sinThetaOverTwo =Math.sin(thetaOverTwo);
double cosThetaOverTwo = Math.cos(thetaOverTwo);
deltaRotationVector[0] = (double) (sinThetaOverTwo * magneticX);
deltaRotationVector[1] = (double) (sinThetaOverTwo * magneticY);
deltaRotationVector[2] = (double) (sinThetaOverTwo * magneticZ);
deltaRotationVector[3] = cosThetaOverTwo;
}
double[] deltaRotationMatrix = new double[9];
SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
}
But the problem is this getRotationMatrixFromVector is says undefine for sensor.Any idea?

Orientation is not a rotation matrix as it only provides you angles related to magnetic North. You can obtain the rotation matrix (Direction Cosine Matrix) that will help you to transform coordinates from your device frame to the Earth's frame this way :
with
= azimuth (radians)
= pitch (radians)
= roll (radians)

I know that this is an old thread but in case it helps, for Android I think the 3x3 rotation matrix is actually given by a variation of the approved answer. To be specific, in Android the rotation matrix is
(cos&#966 cos&#968 - sin&#966 sin&#968 sin&#952) sin&#966 cos&#952 ( cos&#966 sin&#968 + sin&#966 cos&#968 sin&#952)
-(sin&#966 cos&#968 + cos&#966 sin&#968 sin&#952) cos&#966 cos&#952 (-sin&#966 sin&#968 + cos&#966 cos&#968 sin&#952)
-sin&#968 cos&#952 -sin&#952 cos&#966 cos&#952
where
&#966 = azimuth
&#952 = pitch
&#968 = roll
which corresponds to the 3x3 Android rotation matrix R[0] to R[8] (matrixR in the question) via
R[0] R[1] R[2]
R[3] R[4] R[5]
R[6] R[7] R[8]

Remap rotation matrix to different axis system

I am using an adapted version of android's getRotationMatrix in a c++ program that reads the phone's sensor data over the network and calculates the device's matrix.
The function works fine and calculates the device's orientation. Unfortunately, Ogre3d has a different axis system than the device. So even though rotation about the x-axis works fine, the y and z axis are wrong. Holding the device level and pointing to north (identity matrix). When I pitch, the rotation is correct. But when I roll and yaw the rotations are alternated. Roll is yaw in Ogre3d and vice versa.
(Ogre3d) ([Device][5])
^ +y-axis ^ +z-axis
* *
* *
* * ^ +y-axis
* * *
* * *
* * *
************> + x-axis ************> +x-axis
*
*
v +z-axis
A quick look at the two axis system looks like Ogre's system (on the left) is essentially the device's system rotated 90 degrees counter clockwise about the x-axis.
I tried to experiment with various combinations when I fist assign sensor values before the matrix is calculated but no combination seems to work correctly. How would I make sure that the rotation matrix getRotationMatrix() produces displays correctly on Ogre3D?
For Reference here is the function that calculates the matrix:
bool getRotationMatrix() {
//sensor data coming through the network are
//stored in accel(accelerometer) and mag(geomagnetic)
//vars which the function has access to
float Ax = accel[0]; float Ay = accel[1]; float Az = accel[2];
float Ex = mag[0]; float Ey = mag[1]; float Ez = mag[2];
float Hx = Ey * Az - Ez * Ay;
float Hy = Ez * Ax - Ex * Az;
float Hz = Ex * Ay - Ey * Ax;
float normH = (float) Math::Sqrt(Hx * Hx + Hy * Hy + Hz * Hz);
if (normH < 0.1f) {
// device is close to free fall (or in space?), or close to
// magnetic north pole. Typical values are > 100.
return false;
}
float invH = 1.0f / normH;
Hx *= invH;
Hy *= invH;
Hz *= invH;
float invA = 1.0f / (float) Math::Sqrt(Ax * Ax + Ay * Ay + Az * Az);
Ax *= invA;
Ay *= invA;
Az *= invA;
float Mx = Ay * Hz - Az * Hy;
float My = Az * Hx - Ax * Hz;
float Mz = Ax * Hy - Ay * Hx;
//ogre3d's matrix3 is column-major whereas getrotatinomatrix produces
//a row-major matrix thus i have tranposed it here
orientation[0][0] = Hx; orientation[0][2] = Mx; orientation[0][2] = Ax;
orientation[1][0] = Hy; orientation[1][3] = My; orientation[1][2] = Ay;
orientation[2][0] = Hz; orientation[2][4] = Mz; orientation[2][2] = Az;
return true;
}

Why not just add the one additional rotation you've already identified before you use it in ogre?

I found the problem. In my function the unit vectors calculated after the cross products I put them in columns whereas I should be putting them in the rows in their appointed matrix3 cells as usual. Something about row-major and column-major confused me even though I was referring to the elements in 2d [][].
multiplying the outcome of the matrix calculation function with this matrix:
1 0 0
0 0 1
0 -1 0
Then pitching the whole result by another p/2 about axis solved the remap problem but I fear my geometry is inverted.

I don't know much about Matrix Rotation, but if the Systems rotates like you are showing, I think that youshould do the following:
X Axis stays the same way, so:
float Ax = accel[0];
float Ex = mag[0];
Y Axis in (Ogre3d) is Z axis in ([Device][5]), so:
float Ay = accel[2];
float Ey = mag[2];
Z Axis in (Ogre3d) is the oposite of Y axis in ([Device][5]), so:
float Az = accel[1] * (-1);
float Ez = mag[1] * (-1);
Try that