Skip to content

Commit 438ed0f

Browse files
KalebKEKalebKE
authored
Merge pull request #31 from KalebKE/v1.2.2
v1.2.2
2 parents 2b96280 + ffd7d0f commit 438ed0f

File tree

4 files changed

+106
-80
lines changed

4 files changed

+106
-80
lines changed

‎fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/OrientationGyroscope.java

+14-26
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,10 @@
11
package com.kircherelectronics.fsensor.filter.gyroscope;
22

3-
import android.util.Log;
4-
53
import com.kircherelectronics.fsensor.BaseFilter;
4+
import com.kircherelectronics.fsensor.util.angle.AngleUtils;
65
import com.kircherelectronics.fsensor.util.rotation.RotationUtil;
76

87
import org.apache.commons.math3.complex.Quaternion;
9-
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
10-
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
11-
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
12-
13-
import java.util.Arrays;
148

159
/*
1610
* Copyright 2018, Kircher Electronics, LLC
@@ -105,6 +99,18 @@ public float[] getOutput() {
10599

106100
/**
107101
* Calculate the fused orientation of the device.
102+
*
103+
* Rotation is positive in the counterclockwise direction (right-hand rule). That is, an observer looking from some positive location on the x, y, or z axis at
104+
* a device positioned on the origin would report positive rotation if the device appeared to be rotating counter clockwise. Note that this is the
105+
* standard mathematical definition of positive rotation and does not agree with the aerospace definition of roll.
106+
*
107+
* See: https://source.android.com/devices/sensors/sensor-types#rotation_vector
108+
*
109+
* Returns a vector of size 3 ordered as:
110+
* [0]X points east and is tangential to the ground.
111+
* [1]Y points north and is tangential to the ground.
112+
* [2]Z points towards the sky and is perpendicular to the ground.
113+
*
108114
* @param gyroscope the gyroscope measurements.
109115
* @param timestamp the gyroscope timestamp
110116
* @return An orientation vector -> @link SensorManager#getOrientation(float[], float[])}
@@ -115,15 +121,7 @@ public float[] calculateOrientation(float[] gyroscope, long timestamp) {
115121
if (this.timestamp != 0) {
116122
final float dT = (timestamp - this.timestamp) * NS2S;
117123
rotationVectorGyroscope = RotationUtil.integrateGyroscopeRotation(rotationVectorGyroscope, gyroscope, dT, EPSILON);
118-
119-
Rotation rotation = new Rotation(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(),
120-
rotationVectorGyroscope.getQ3(), true);
121-
122-
try {
123-
output = doubleToFloat(rotation.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR));
124-
} catch(Exception e) {
125-
Log.d(TAG, "", e);
126-
}
124+
output = AngleUtils.getAngles(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(), rotationVectorGyroscope.getQ3());
127125
}
128126

129127
this.timestamp = timestamp;
@@ -156,14 +154,4 @@ public void reset() {
156154
public boolean isBaseOrientationSet() {
157155
return rotationVectorGyroscope != null;
158156
}
159-
160-
private static float[] doubleToFloat(double[] values) {
161-
float[] f = new float[values.length];
162-
163-
for(int i = 0; i < f.length; i++){
164-
f[i] = (float) values[i];
165-
}
166-
167-
return f;
168-
}
169157
}

‎fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/complementary/OrientationFusedComplementary.java

+15-26
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,10 @@
11
package com.kircherelectronics.fsensor.filter.gyroscope.fusion.complementary;
22

3-
import android.util.Log;
4-
53
import com.kircherelectronics.fsensor.filter.gyroscope.fusion.OrientationFused;
4+
import com.kircherelectronics.fsensor.util.angle.AngleUtils;
65
import com.kircherelectronics.fsensor.util.rotation.RotationUtil;
76

87
import org.apache.commons.math3.complex.Quaternion;
9-
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
10-
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
11-
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
128

139
/*
1410
* Copyright 2018, Kircher Electronics, LLC
@@ -97,11 +93,21 @@ public OrientationFusedComplementary(float timeConstant) {
9793

9894
/**
9995
* Calculate the fused orientation of the device.
96+
*
97+
* Rotation is positive in the counterclockwise direction (right-hand rule). That is, an observer looking from some positive location on the x, y, or z axis at
98+
* a device positioned on the origin would report positive rotation if the device appeared to be rotating counter clockwise. Note that this is the
99+
* standard mathematical definition of positive rotation and does not agree with the aerospace definition of roll.
100+
*
101+
* See: https://source.android.com/devices/sensors/sensor-types#rotation_vector
102+
*
103+
* Returns a vector of size 3 ordered as:
104+
* [0]X points east and is tangential to the ground.
105+
* [1]Y points north and is tangential to the ground.
106+
* [2]Z points towards the sky and is perpendicular to the ground.
107+
*
100108
* @param gyroscope the gyroscope measurements.
101109
* @param timestamp the gyroscope timestamp
102-
* @param acceleration the acceleration measurements
103-
* @param magnetic the magnetic measurements
104-
* @return the fused orientation estimation.
110+
* @return An orientation vector -> @link SensorManager#getOrientation(float[], float[])}
105111
*/
106112
public float[] calculateFusedOrientation(float[] gyroscope, long timestamp, float[] acceleration, float[] magnetic) {
107113
if (isBaseOrientationSet()) {
@@ -130,14 +136,7 @@ public float[] calculateFusedOrientation(float[] gyroscope, long timestamp, floa
130136
(scaledRotationVectorAccelerationMagnetic);
131137
}
132138

133-
Rotation rotation = new Rotation(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(),
134-
rotationVectorGyroscope.getQ3(), true);
135-
136-
try {
137-
output = doubleToFloat(rotation.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR));
138-
} catch(Exception e) {
139-
Log.d(TAG, "", e);
140-
}
139+
output = AngleUtils.getAngles(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(), rotationVectorGyroscope.getQ3());
141140
}
142141

143142
this.timestamp = timestamp;
@@ -147,14 +146,4 @@ public float[] calculateFusedOrientation(float[] gyroscope, long timestamp, floa
147146
throw new IllegalStateException("You must call setBaseOrientation() before calling calculateFusedOrientation()!");
148147
}
149148
}
150-
151-
private static float[] doubleToFloat(double[] values) {
152-
float[] f = new float[values.length];
153-
154-
for(int i = 0; i < f.length; i++){
155-
f[i] = (float) values[i];
156-
}
157-
158-
return f;
159-
}
160149
}

‎fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java

+19-28
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,10 @@
77
import com.kircherelectronics.fsensor.filter.gyroscope.fusion.kalman.filter.RotationKalmanFilter;
88
import com.kircherelectronics.fsensor.filter.gyroscope.fusion.kalman.filter.RotationMeasurementModel;
99
import com.kircherelectronics.fsensor.filter.gyroscope.fusion.kalman.filter.RotationProcessModel;
10+
import com.kircherelectronics.fsensor.util.angle.AngleUtils;
1011
import com.kircherelectronics.fsensor.util.rotation.RotationUtil;
1112

1213
import org.apache.commons.math3.complex.Quaternion;
13-
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
14-
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
15-
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
1614

1715
import java.util.Arrays;
1816

@@ -63,8 +61,6 @@ public class OrientationFusedKalman extends OrientationFused {
6361
private static final String TAG = OrientationFusedComplementary.class.getSimpleName();
6462

6563
private RotationKalmanFilter kalmanFilter;
66-
private RotationProcessModel pm;
67-
private RotationMeasurementModel mm;
6864
private volatile boolean run;
6965
private volatile float dT;
7066
private volatile float[] output = new float[3];
@@ -78,11 +74,7 @@ public OrientationFusedKalman() {
7874

7975
public OrientationFusedKalman(float timeConstant) {
8076
super(timeConstant);
81-
82-
pm = new RotationProcessModel();
83-
mm = new RotationMeasurementModel();
84-
85-
kalmanFilter = new RotationKalmanFilter(pm, mm);
77+
kalmanFilter = new RotationKalmanFilter(new RotationProcessModel(), new RotationMeasurementModel());
8678
}
8779

8880
public void startFusion() {
@@ -124,6 +116,22 @@ public float[] getOutput() {
124116
return output;
125117
}
126118

119+
/**
120+
* Calculate the fused orientation of the device.
121+
*
122+
* Rotation is positive in the counterclockwise direction (right-hand rule). That is, an observer looking from some positive location on the x, y, or z axis at
123+
* a device positioned on the origin would report positive rotation if the device appeared to be rotating counter clockwise. Note that this is the
124+
* standard mathematical definition of positive rotation and does not agree with the aerospace definition of roll.
125+
*
126+
* See: https://source.android.com/devices/sensors/sensor-types#rotation_vector
127+
*
128+
* Returns a vector of size 3 ordered as:
129+
* [0]X points east and is tangential to the ground.
130+
* [1]Y points north and is tangential to the ground.
131+
* [2]Z points towards the sky and is perpendicular to the ground.
132+
*
133+
* @return An orientation vector -> @link SensorManager#getOrientation(float[], float[])}
134+
*/
127135
private float[] calculate() {
128136
if (rotationVectorGyroscope != null && rotationOrientation != null && dT != 0) {
129137

@@ -151,14 +159,7 @@ private float[] calculate() {
151159
rotationVectorGyroscope = new Quaternion(kalmanFilter.getStateEstimation()[3],
152160
Arrays.copyOfRange(kalmanFilter.getStateEstimation(), 0, 3));
153161

154-
Rotation rotation = new Rotation(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(),
155-
rotationVectorGyroscope.getQ3(), true);
156-
157-
try {
158-
output = doubleToFloat(rotation.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR));
159-
} catch(Exception e) {
160-
Log.d(TAG, "", e);
161-
}
162+
output = AngleUtils.getAngles(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(), rotationVectorGyroscope.getQ3());
162163

163164
return output;
164165
}
@@ -191,14 +192,4 @@ public float[] calculateFusedOrientation(float[] gyroscope, long timestamp, floa
191192
throw new IllegalStateException("You must call setBaseOrientation() before calling calculateFusedOrientation()!");
192193
}
193194
}
194-
195-
private static float[] doubleToFloat(double[] values) {
196-
float[] f = new float[values.length];
197-
198-
for(int i = 0; i < f.length; i++){
199-
f[i] = (float) values[i];
200-
}
201-
202-
return f;
203-
}
204195
}

‎fsensor/src/main/java/com/kircherelectronics/fsensor/util/angle/AngleUtils.java

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
package com.kircherelectronics.fsensor.util.angle;
2+
3+
import android.util.Log;
4+
5+
public class AngleUtils {
6+
private static final String TAG = AngleUtils.class.getSimpleName();
7+
8+
/**
9+
* https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
10+
*
11+
* +X to the right
12+
* +Y straight up
13+
* +Z axis toward viewer
14+
*
15+
* Heading = rotation about y axis
16+
* Attitude = rotation about z axis
17+
* Bank = rotation about x axis
18+
*
19+
* Heading applied first
20+
* Attitude applied second
21+
* Bank applied last
22+
*
23+
* @param w
24+
* @param z
25+
* @param x
26+
* @param y
27+
* @return
28+
*/
29+
public static float[] getAngles(double w, double z, double x, double y) {
30+
double heading;
31+
double pitch;
32+
double roll;
33+
34+
double test = x*y + z*w;
35+
if (test > 0.499) { // singularity at north pole
36+
heading = 2 * Math.atan2(x,w);
37+
pitch = -Math.PI/2;
38+
roll = 0;
39+
Log.e(TAG, "singularity at north pole");
40+
return new float[]{(float)heading, (float)pitch, (float)roll};
41+
}
42+
if (test < -0.499) { // singularity at south pole
43+
heading = -2 * Math.atan2(x,w);
44+
pitch = Math.PI/2;
45+
roll = 0;
46+
Log.e(TAG, "singularity at south pole");
47+
return new float[]{(float)heading, (float)pitch, (float)roll};
48+
}
49+
double sqx = x*x;
50+
double sqy = y*y;
51+
double sqz = z*z;
52+
heading = Math.atan2(2*y*w-2*x*z , 1 - 2*sqy - 2*sqz);
53+
pitch = -Math.asin(2*test);
54+
roll = -Math.atan2(2*x*w-2*y*z , 1 - 2*sqx - 2*sqz);
55+
56+
return new float[]{(float)pitch,(float)roll, (float)heading};
57+
}
58+
}

0 commit comments

Comments
 (0)