Проблемы с гироскопом с ориентацией устройства

Я получаю данные и данные из гироскопа моего устройства, используя этот учебник: http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/

Все показания чрезвычайно точные (в учебнике используется фильтр для устранения дрейфа гироскопа). К сожалению, код работает только тогда, когда мое устройство размещено на поверхности, параллельной земле. Самая идеальная позиция для моего приложения – работать с верхней частью устройства, направленным прямо вверх (т. Е. Устройство перпендикулярно земле с экраном, обращенным к пользователю). Всякий раз, когда я ориентирую свое устройство в этом положении, значения тангажа достигают +90 градусов (как и ожидалось). То, что я хотел бы сделать, установить это положение как 0-градусная точка (или начальная позиция) для моего устройства, чтобы показания тангажа составляли 0 градусов, когда мое устройство ориентировано вертикально (в портретном режиме) с экраном, обращенным к пользователю.

Я попросил автора учебника помочь в этом вопросе, и он ответил:

«Если вы хотите иметь вертикальное положение в качестве начального, вам нужно будет соответствующим образом повернуть свою систему координат. Самый простой способ – повернуть полученную матрицу поворота на -90 градусов относительно оси x. Но вы должны Будьте осторожны, в какой момент алгоритм применил это вращение. Всегда помните, что вращения не являются коммутативными операциями. Чтобы быть более конкретными в этом, мне пришлось бы снова просмотреть код, так как я не работал с ним некоторое время Теперь."

Я действительно очень смущен и туговат, как повернуть мою систему отсчета. Думаю, в итоге я не знаю, как повернуть матрицу на -90 градусов по оси x. Если бы кто-то мог помочь мне с этой частью, это было бы фантастически. Вот мой код на тот случай, если кто-то захочет обратиться к нему:

public class AttitudeDisplayIndicator extends SherlockActivity implements SensorEventListener { private SensorManager mSensorManager = null; // angular speeds from gyro private float[] gyro = new float[3]; // rotation matrix from gyro data private float[] gyroMatrix = new float[9]; // orientation angles from gyro matrix private float[] gyroOrientation = new float[3]; // magnetic field vector private float[] magnet = new float[3]; // accelerometer vector private float[] accel = new float[3]; // orientation angles from accel and magnet private float[] accMagOrientation = new float[3]; // final orientation angles from sensor fusion private float[] fusedOrientation = new float[3]; // accelerometer and magnetometer based rotation matrix private float[] rotationMatrix = new float[9]; public static final float EPSILON = 0.000000001f; private static final float NS2S = 1.0f / 1000000000.0f; private float timestamp; private boolean initState = true; public static final int TIME_CONSTANT = 30; public static final float FILTER_COEFFICIENT = 0.98f; private Timer fuseTimer = new Timer(); // The following members are only for displaying the sensor output. public Handler mHandler; DecimalFormat d = new DecimalFormat("#.##"); //ADI background image. private ImageView adiBackground; //ADI axes. private ImageView adiAxes; //ADI frame. private ImageView adiFrame; //Layout. private RelativeLayout layout; //Pitch and Roll TextViews. private TextView pitchAngleText; private TextView bankAngleText; //Instantaneous output values from sensors as the device moves. public static double pitch; public static double roll; //Matrix for rotating the ADI (roll). Matrix mMatrix = new Matrix(); @Override public void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); setContentView(R.layout.activity_attitude_display_indicator); gyroOrientation[0] = 0.0f; gyroOrientation[1] = 0.0f; gyroOrientation[2] = 0.0f; // initialise gyroMatrix with identity matrix gyroMatrix[0] = 1.0f; gyroMatrix[1] = 0.0f; gyroMatrix[2] = 0.0f; gyroMatrix[3] = 0.0f; gyroMatrix[4] = 1.0f; gyroMatrix[5] = 0.0f; gyroMatrix[6] = 0.0f; gyroMatrix[7] = 0.0f; gyroMatrix[8] = 1.0f; // get sensorManager and initialise sensor listeners mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE); initListeners(); // wait for one second until gyroscope and magnetometer/accelerometer // data is initialised then scedule the complementary filter task fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT); mHandler = new Handler(); adiBackground = (ImageView) findViewById(R.id.adi_background); adiFrame = (ImageView) findViewById(R.id.adi_frame); adiAxes = (ImageView) findViewById(R.id.adi_axes); layout = (RelativeLayout) findViewById(R.id.adi_layout); new Color(); layout.setBackgroundColor(Color.rgb(150, 150, 150)); pitchAngleText = (TextView) findViewById(R.id.pitch_angle_text); bankAngleText = (TextView) findViewById(R.id.bank_angle_text); } // This function registers sensor listeners for the accelerometer, magnetometer and gyroscope. public void initListeners(){ mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_FASTEST); mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_FASTEST); mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_FASTEST); } //@Override public void onAccuracyChanged(Sensor sensor, int accuracy) { } //@Override public void onSensorChanged(SensorEvent event) { switch(event.sensor.getType()) { case Sensor.TYPE_ACCELEROMETER: // copy new accelerometer data into accel array and calculate orientation System.arraycopy(event.values, 0, accel, 0, 3); calculateAccMagOrientation(); break; case Sensor.TYPE_GYROSCOPE: // process gyro data gyroFunction(event); break; case Sensor.TYPE_MAGNETIC_FIELD: // copy new magnetometer data into magnet array System.arraycopy(event.values, 0, magnet, 0, 3); break; } } // calculates orientation angles from accelerometer and magnetometer output public void calculateAccMagOrientation() { if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) { SensorManager.getOrientation(rotationMatrix, accMagOrientation); } } // This function is borrowed from the Android reference // at http://developer.android.com/reference/android/hardware/SensorEvent.html#values // It calculates a rotation vector from the gyroscope angular speed values. private void getRotationVectorFromGyro(float[] gyroValues, float[] deltaRotationVector, float timeFactor) { float[] normValues = new float[3]; // Calculate the angular speed of the sample float omegaMagnitude = (float)Math.sqrt(gyroValues[0] * gyroValues[0] + gyroValues[1] * gyroValues[1] + gyroValues[2] * gyroValues[2]); // Normalize the rotation vector if it's big enough to get the axis if(omegaMagnitude > EPSILON) { normValues[0] = gyroValues[0] / omegaMagnitude; normValues[1] = gyroValues[1] / omegaMagnitude; normValues[2] = gyroValues[2] / 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 * timeFactor; float sinThetaOverTwo = (float)Math.sin(thetaOverTwo); float cosThetaOverTwo = (float)Math.cos(thetaOverTwo); deltaRotationVector[0] = sinThetaOverTwo * normValues[0]; deltaRotationVector[1] = sinThetaOverTwo * normValues[1]; deltaRotationVector[2] = sinThetaOverTwo * normValues[2]; deltaRotationVector[3] = cosThetaOverTwo; } // This function performs the integration of the gyroscope data. // It writes the gyroscope based orientation into gyroOrientation. public void gyroFunction(SensorEvent event) { // don't start until first accelerometer/magnetometer orientation has been acquired if (accMagOrientation == null) return; // initialisation of the gyroscope based rotation matrix if(initState) { float[] initMatrix = new float[9]; initMatrix = getRotationMatrixFromOrientation(accMagOrientation); float[] test = new float[3]; SensorManager.getOrientation(initMatrix, test); gyroMatrix = matrixMultiplication(gyroMatrix, 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]; if(timestamp != 0) { final float dT = (event.timestamp - timestamp) * NS2S; System.arraycopy(event.values, 0, gyro, 0, 3); getRotationVectorFromGyro(gyro, deltaVector, 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]; SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector); // apply the new rotation interval on the gyroscope based rotation matrix gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix); // get the gyroscope based orientation from the rotation matrix SensorManager.getOrientation(gyroMatrix, gyroOrientation); } private float[] getRotationMatrixFromOrientation(float[] o) { float[] xM = new float[9]; float[] yM = new float[9]; float[] zM = new float[9]; float sinX = (float)Math.sin(o[1]); float cosX = (float)Math.cos(o[1]); float sinY = (float)Math.sin(o[2]); float cosY = (float)Math.cos(o[2]); float sinZ = (float)Math.sin(o[0]); float cosZ = (float)Math.cos(o[0]); // rotation about x-axis (pitch) xM[0] = 1.0f; xM[1] = 0.0f; xM[2] = 0.0f; xM[3] = 0.0f; xM[4] = cosX; xM[5] = sinX; xM[6] = 0.0f; xM[7] = -sinX; xM[8] = cosX; // rotation about y-axis (roll) yM[0] = cosY; yM[1] = 0.0f; yM[2] = sinY; yM[3] = 0.0f; yM[4] = 1.0f; yM[5] = 0.0f; yM[6] = -sinY; yM[7] = 0.0f; yM[8] = cosY; // rotation about z-axis (azimuth) zM[0] = cosZ; zM[1] = sinZ; zM[2] = 0.0f; zM[3] = -sinZ; zM[4] = cosZ; zM[5] = 0.0f; zM[6] = 0.0f; zM[7] = 0.0f; zM[8] = 1.0f; // rotation order is y, x, z (roll, pitch, azimuth) float[] resultMatrix = matrixMultiplication(xM, yM); resultMatrix = matrixMultiplication(zM, resultMatrix); return resultMatrix; } private float[] matrixMultiplication(float[] A, float[] B) { float[] result = new float[9]; result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6]; result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7]; result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8]; result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6]; result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7]; result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8]; result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6]; result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7]; result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8]; return result; } class calculateFusedOrientationTask extends TimerTask { public void run() { float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT; /* * 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; } 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; } else { fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0]; } // pitch if (gyroOrientation[1] < -0.5 * Math.PI && accMagOrientation[1] > 0.0) { fusedOrientation[1] = (float) (FILTER_COEFFICIENT * (gyroOrientation[1] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[1]); fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0; } else if (accMagOrientation[1] < -0.5 * Math.PI && gyroOrientation[1] > 0.0) { fusedOrientation[1] = (float) (FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * (accMagOrientation[1] + 2.0 * Math.PI)); fusedOrientation[1] -= (fusedOrientation[1] > Math.PI)? 2.0 * Math.PI : 0; } else { fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * accMagOrientation[1]; } // roll if (gyroOrientation[2] < -0.5 * Math.PI && accMagOrientation[2] > 0.0) { fusedOrientation[2] = (float) (FILTER_COEFFICIENT * (gyroOrientation[2] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[2]); fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0; } else if (accMagOrientation[2] < -0.5 * Math.PI && gyroOrientation[2] > 0.0) { fusedOrientation[2] = (float) (FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * (accMagOrientation[2] + 2.0 * Math.PI)); fusedOrientation[2] -= (fusedOrientation[2] > Math.PI)? 2.0 * Math.PI : 0; } else { fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * accMagOrientation[2]; } // overwrite gyro matrix and orientation with fused orientation // to comensate gyro drift gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation); System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3); // update sensor output in GUI mHandler.post(updateOrientationDisplayTask); } } 

Заранее спасибо за вашу помощь!

Solutions Collecting From Web of "Проблемы с гироскопом с ориентацией устройства"

Теория…

Я не совсем уверен в формате, в котором представлена ​​ваша матрица «рамки ссылки», но, как правило, вращения выполняются с умножением матрицы.

В принципе, вы возьмете свою «матрицу опорных матриц» и умножьте ее на матрицу поворота на 90 градусов.

Такую матрицу можно найти в Википедии: трехмерные матрицы вращения

Так как ваш угол равен 90 градусам, ваши синусы и косинусы разрешат 1 или 0, которые вы можете подключить непосредственно к матрице вместо вычисления синусов и косинусов. Например, матрица, которая будет вращаться на 90 градусов против часовой стрелки вокруг оси x, будет выглядеть так:

 1 0 0 0 0 1 0 -1 0 

Кроме того, пожалуйста, не считайте, что такие матрицы работают на векторах строк координат xyz. Например, если у вас есть точка в пространстве, которая находится в (2,5,7), и вы хотите повернуть ее, используя указанную выше матрицу, вам нужно будет выполнить следующую операцию:

 |2 5 7| |1 0 0| |0 0 1| |0 -1 0| 

Что дает [2 -7 5]

… применяется к вашему коду

Я быстро взглянул на ваш код, и похоже, что модификация, которую нужно сделать, включает вывод функции calculateAccMagOrientation (), поскольку он используется для инициализации ориентации устройства.

 1: public void calculateAccMagOrientation() { 2: if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) { 3: SensorManager.getOrientation(rotationMatrix, accMagOrientation); 4: } 5: } 

На строке 2 в приведенном выше фрагменте вы получите начальную матрицу вращения. Попробуйте умножить rotationMatrix на обработанную вручную матрицу поворота на 90 градусов, прежде чем вызвать getOrientation в строке 3. Я думаю, что это будет эффективно переустанавливать вашу ссылочную ориентацию:

 public void calculateAccMagOrientation() { if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) { rotationMatrix = matrixMultiplication(rotationMatrix, my90DegRotationMatrix); SensorManager.getOrientation(rotationMatrix, accMagOrientation); } } 

Обратите внимание, что в зависимости от того, как углы работают в Android, вам может потребоваться использовать матрицу вращения по часовой стрелке на 90 градусов вместо против часовой стрелки.

Альтернативное решение

Мне просто пришло в голову, может быть, вы могли бы просто вычесть 90 из финального результата тангажа перед его отображением?