我正在为陀螺仪和加速度计使用互补滤波器，仅用于方位角......我从这个网站得到它：http ://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/1 /
过滤器的核心是：
/*
* Fix for 179° <--> -179° transition problem:
* Check whether one of the two orientation angles (gyro or accMag) is negative while the other one is positive.
* If so, add 360° (2 * math.PI) to the negative value, perform the sensor fusion, and remove the 360° from the result
* if it is greater than 180°. This stabilizes the output in positive-to-negative-transition cases.
*/
// azimuth
if (gyroOrientation[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) {
fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[0]);
fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
Log.d("test","gyro Is Negative");
}
else if (accMagOrientation[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) {
fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * (accMagOrientation[0] + 2.0 * Math.PI));
fusedOrientation[0] -= (fusedOrientation[0] > Math.PI)? 2.0 * Math.PI : 0;
Log.d("test","accel Is Negative");
}
else {
fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0];
}
gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
我想将此与具有漂移的真实陀螺仪数据进行比较...为此，我使用了 gyroOreintationReal... 并添加了一些代码来保存 gyroOreintation，如下所示：
if(initState) {
float[] initMatrix = new float[9];
initMatrix = getRotationMatrixFromOrientation(accMagOrientation);
float[] test = new float[3];
SensorManager.getOrientation(initMatrix, test);
gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);
gyroMatrixReal = matrixMultiplication(gyroMatrixReal, initMatrix);
initState = false;
}
// copy the new gyro values into the gyro array
// convert the raw gyro data into a rotation vector
float[] deltaVector = new float[4];
float[] deltaVectorReal = new float[4];
if(timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
System.arraycopy(event.values, 0, gyro, 0, 3);
System.arraycopy(event.values, 0, gyroReal, 0, 3);
getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
getRotationVectorFromGyro(gyroReal, deltaVectorReal, dT / 2.0f);
}
// measurement done, save current time for next interval
timestamp = event.timestamp;
// convert rotation vector into rotation matrix
float[] deltaMatrix = new float[9];
float[] deltaMatrixReal = new float[9];
SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
SensorManager.getRotationMatrixFromVector(deltaMatrixReal, deltaVectorReal);
// apply the new rotation interval on the gyroscope based rotation matrix
gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);
gyroMatrixReal = matrixMultiplication(gyroMatrixReal, deltaMatrixReal);
// get the gyroscope based orientation from the rotation matrix
SensorManager.getOrientation(gyroMatrix, gyroOrientation);
SensorManager.getOrientation(gyroMatrixReal, gyroOrientationReal);
我保存了结果并用matlab绘制了它们......但绘图显示陀螺仪的方向是负的......但fusedOreientation小于+150并且加速度方向略大于+150......
我该如何解决这个问题？我在互补过滤器的核心添加了一些代码：
//RealGyro
if (gyroOrientationReal[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) {
gyroOrientationReal[0] = (float) (gyroOrientation[0] + 2.0 * Math.PI);
gyroOrientationReal[0] -= (gyroOrientationReal[0] > Math.PI) ? 2.0 * Math.PI : 0;
}
有时没关系但是我不知道如果我有负加速度数据和正陀螺数据该怎么办？
