Использование гироскопа Android вместо акселерометра. Я нахожу много бит и кусочков, но не полный код

Видео Sensor Fusion выглядит великолепно, но кода нет: http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s

Вот мой код, который просто использует акселерометр и компас. Я также использую фильтр Калмана по 3 значениям ориентации, но это слишком много кода, чтобы показать здесь. В конечном счете, это работает нормально, но результат либо слишком нервный, либо слишком медленный, в зависимости от того, что я делаю с результатами, и насколько низким я делаю фильтрующие факторы.

/** Just accelerometer and magnetic sensors */ public abstract class SensorsListener2 implements SensorEventListener { /** The lower this is, the greater the preference which is given to previous values. (slows change) */ private static final float accelFilteringFactor = 0.1f; private static final float magFilteringFactor = 0.01f; public abstract boolean getIsLandscape(); @Override public void onSensorChanged(SensorEvent event) { Sensor sensor = event.sensor; int type = sensor.getType(); switch (type) { case Sensor.TYPE_MAGNETIC_FIELD: mags[0] = event.values[0] * magFilteringFactor + mags[0] * (1.0f - magFilteringFactor); mags[1] = event.values[1] * magFilteringFactor + mags[1] * (1.0f - magFilteringFactor); mags[2] = event.values[2] * magFilteringFactor + mags[2] * (1.0f - magFilteringFactor); isReady = true; break; case Sensor.TYPE_ACCELEROMETER: accels[0] = event.values[0] * accelFilteringFactor + accels[0] * (1.0f - accelFilteringFactor); accels[1] = event.values[1] * accelFilteringFactor + accels[1] * (1.0f - accelFilteringFactor); accels[2] = event.values[2] * accelFilteringFactor + accels[2] * (1.0f - accelFilteringFactor); break; default: return; } if(mags != null && accels != null && isReady) { isReady = false; SensorManager.getRotationMatrix(rot, inclination, accels, mags); boolean isLandscape = getIsLandscape(); if(isLandscape) { outR = rot; } else { // Remap the coordinates to work in portrait mode. SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR); } SensorManager.getOrientation(outR, values); double x180pi = 180.0 / Math.PI; float azimuth = (float)(values[0] * x180pi); float pitch = (float)(values[1] * x180pi); float roll = (float)(values[2] * x180pi); // In landscape mode swap pitch and roll and invert the pitch. if(isLandscape) { float tmp = pitch; pitch = -roll; roll = -tmp; azimuth = 180 - azimuth; } else { pitch = -pitch - 90; azimuth = 90 - azimuth; } onOrientationChanged(azimuth,pitch,roll); } } private float[] mags = new float[3]; private float[] accels = new float[3]; private boolean isReady; private float[] rot = new float[9]; private float[] outR = new float[9]; private float[] inclination = new float[9]; private float[] values = new float[3]; /** Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis. Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis. */ public abstract void onOrientationChanged(float azimuth, float pitch, float roll); } 

Я попытался выяснить, как добавить данные гироскопа, но я просто не делаю это правильно. В документе google по адресу http://developer.android.com/reference/android/hardware/SensorEvent.html показан код для получения дельта-матрицы из данных гироскопа. Идея заключается в том, что я закрутил фильтры для акселерометра и магнитных датчиков, чтобы они были действительно стабильными. Это будет отслеживать долгосрочную ориентацию.

Тогда я бы сохранил историю последних N дельта-матриц из гироскопа. Каждый раз, когда я получаю новый, я бы упал с самого старого и размножу все вместе, чтобы получить окончательную матрицу, которую я бы умножал на стабильную матрицу, возвращаемую акселерометром и магнитными датчиками.

Кажется, это не работает. Или, по крайней мере, моя реализация этого не работает. Результат гораздо более резкий, чем акселерометр. Увеличение размера истории гироскопа на самом деле увеличивает дрожание, что заставляет меня думать, что я не рассчитываю правильные значения из гироскопа.

 public abstract class SensorsListener3 implements SensorEventListener { /** The lower this is, the greater the preference which is given to previous values. (slows change) */ private static final float kFilteringFactor = 0.001f; private static final float magKFilteringFactor = 0.001f; public abstract boolean getIsLandscape(); @Override public void onSensorChanged(SensorEvent event) { Sensor sensor = event.sensor; int type = sensor.getType(); switch (type) { case Sensor.TYPE_MAGNETIC_FIELD: mags[0] = event.values[0] * magKFilteringFactor + mags[0] * (1.0f - magKFilteringFactor); mags[1] = event.values[1] * magKFilteringFactor + mags[1] * (1.0f - magKFilteringFactor); mags[2] = event.values[2] * magKFilteringFactor + mags[2] * (1.0f - magKFilteringFactor); isReady = true; break; case Sensor.TYPE_ACCELEROMETER: accels[0] = event.values[0] * kFilteringFactor + accels[0] * (1.0f - kFilteringFactor); accels[1] = event.values[1] * kFilteringFactor + accels[1] * (1.0f - kFilteringFactor); accels[2] = event.values[2] * kFilteringFactor + accels[2] * (1.0f - kFilteringFactor); break; case Sensor.TYPE_GYROSCOPE: gyroscopeSensorChanged(event); break; default: return; } if(mags != null && accels != null && isReady) { isReady = false; SensorManager.getRotationMatrix(rot, inclination, accels, mags); boolean isLandscape = getIsLandscape(); if(isLandscape) { outR = rot; } else { // Remap the coordinates to work in portrait mode. SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR); } if(gyroUpdateTime!=0) { matrixHistory.mult(matrixTmp,matrixResult); outR = matrixResult; } SensorManager.getOrientation(outR, values); double x180pi = 180.0 / Math.PI; float azimuth = (float)(values[0] * x180pi); float pitch = (float)(values[1] * x180pi); float roll = (float)(values[2] * x180pi); // In landscape mode swap pitch and roll and invert the pitch. if(isLandscape) { float tmp = pitch; pitch = -roll; roll = -tmp; azimuth = 180 - azimuth; } else { pitch = -pitch - 90; azimuth = 90 - azimuth; } onOrientationChanged(azimuth,pitch,roll); } } private void gyroscopeSensorChanged(SensorEvent event) { // This timestep's delta rotation to be multiplied by the current rotation // after computing it from the gyro sample data. if(gyroUpdateTime != 0) { final float dT = (event.timestamp - gyroUpdateTime) * 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 = (float)Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ); // Normalize the rotation vector if it's big enough to get the axis 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 = (float)Math.sin(thetaOverTwo); float cosThetaOverTwo = (float)Math.cos(thetaOverTwo); deltaRotationVector[0] = sinThetaOverTwo * axisX; deltaRotationVector[1] = sinThetaOverTwo * axisY; deltaRotationVector[2] = sinThetaOverTwo * axisZ; deltaRotationVector[3] = cosThetaOverTwo; } gyroUpdateTime = event.timestamp; 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; matrixHistory.add(deltaRotationMatrix); } private float[] mags = new float[3]; private float[] accels = new float[3]; private boolean isReady; private float[] rot = new float[9]; private float[] outR = new float[9]; private float[] inclination = new float[9]; private float[] values = new float[3]; // gyroscope stuff private long gyroUpdateTime = 0; private static final float NS2S = 1.0f / 1000000000.0f; private float[] deltaRotationMatrix = new float[9]; private final float[] deltaRotationVector = new float[4]; //TODO: I have no idea how small this value should be. private static final float EPSILON = 0.000001f; private float[] matrixMult = new float[9]; private MatrixHistory matrixHistory = new MatrixHistory(100); private float[] matrixTmp = new float[9]; private float[] matrixResult = new float[9]; /** Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis. Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis. */ public abstract void onOrientationChanged(float azimuth, float pitch, float roll); } public class MatrixHistory { public MatrixHistory(int size) { vals = new float[size][]; } public void add(float[] val) { synchronized(vals) { vals[ix] = val; ix = (ix + 1) % vals.length; if(ix==0) full = true; } } public void mult(float[] tmp, float[] output) { synchronized(vals) { if(full) { for(int i=0; i<vals.length; ++i) { if(i==0) { System.arraycopy(vals[i],0,output,0,vals[i].length); } else { MathUtils.multiplyMatrix3x3(output,vals[i],tmp); System.arraycopy(tmp,0,output,0,tmp.length); } } } else { if(ix==0) return; for(int i=0; i<ix; ++i) { if(i==0) { System.arraycopy(vals[i],0,output,0,vals[i].length); } else { MathUtils.multiplyMatrix3x3(output,vals[i],tmp); System.arraycopy(tmp,0,output,0,tmp.length); } } } } } private int ix = 0; private boolean full = false; private float[][] vals; } 

Второй блок кода содержит мои изменения из первого блока кода, которые добавляют гироскоп в микс.

В частности, коэффициент фильтрации для ускорения уменьшается (делая значение более стабильным). Класс MatrixHistory отслеживает последние 100 значений дельтаRotationMatrix гироскопа, которые вычисляются методом gyroscopeSensorChanged.

Я видел много вопросов на этом сайте по этой теме. Они помогли мне дойти до этого момента, но я не могу понять, что делать дальше. Я очень хочу, чтобы парень Sensor Fusion только что отправил какой-то код. Очевидно, он все собрал.

Solutions Collecting From Web of "Использование гироскопа Android вместо акселерометра. Я нахожу много бит и кусочков, но не полный код"

Хорошо, +1 вам даже известно, что такое фильтр Калмана. Если вы хотите, я отредактирую этот пост и дам вам код, который я написал пару лет назад, чтобы сделать то, что вы пытаетесь сделать.

Но сначала я скажу вам, почему он вам не нужен.

Современные реализации стека датчиков Android используют Sensor Fusion , как сказал Стэн. Это означает, что все доступные данные – ускорение, магнит, гироскоп – собираются вместе в одном алгоритме, а затем все выходы считываются обратно в виде датчиков Android.

Редактирование: я просто наткнулся на этот превосходный Google Tech Talk по теме: Sensor Fusion на устройствах Android: революция в обработке движения . Хорошо стоит 45 минут, чтобы посмотреть, если вы заинтересованы в этой теме.

По сути, Sensor Fusion – это черный ящик. Я изучил исходный код реализации Android, и это большой фильтр Калмана, написанный на C ++. Какой-то довольно хороший код там и гораздо сложнее, чем любой фильтр, который я когда-либо писал, и, вероятно, более сложный, что вы пишете. Помните, эти ребята делают это для жизни.

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

Наконец, как сказал Стэн, у Invensense есть своя собственная реализация слияния датчиков на уровне чипа.

Во всяком случае, все это сводится к тому, что встроенный слияние сенсора в вашем устройстве, вероятно, будет превосходить все, что вы или я могли бы согреться вместе. Так что вы действительно хотите сделать это, чтобы получить доступ к этому.

В Android есть как физические, так и виртуальные датчики. Виртуальные датчики – это те, которые синтезируются из доступных физических датчиков. Наиболее известным примером является TYPE_ORIENTATION, который принимает акселерометр и магнитометр и создает выход рулона / высоты тона / заголовка. (Кстати, вы не должны использовать этот датчик, у него слишком много ограничений.)

Но важно то, что более новые версии Android содержат эти два новых виртуальных датчика:

TYPE_GRAVITY – это вход акселерометра с эффектом отфильтрованного движения. TYPE_LINEAR_ACCELERATION – это акселерометр с отфильтрованным компонентом силы тяжести.

Эти два виртуальных датчика синтезируются с помощью комбинации входа акселерометра и гироскопа.

Другим заметным датчиком является TYPE_ROTATION_VECTOR, который представляет собой кватернион, синтезированный из акселерометра, магнитометра и гироскопа. Он представляет собой полную трехмерную ориентацию устройства с отфильтрованным эффектом линейного ускорения.

Тем не менее, Quaternions немного абстрактны для большинства людей, и, поскольку вы, вероятно, работаете с трехмерными преобразованиями, ваш лучший подход состоит в объединении TYPE_GRAVITY и TYPE_MAGNETIC_FIELD с помощью SensorManager.getRotationMatrix ().

Еще один момент: если вы работаете с устройством, использующим более старую версию Android, вам нужно обнаружить, что вы не принимаете события TYPE_GRAVITY и вместо этого используете TYPE_ACCELEROMETER. Теоретически, это будет место для использования вашего собственного фильтра калманов, но если ваше устройство не имеет встроенного термодатчика, он, вероятно, также не имеет гироскопов.

Во всяком случае, вот пример кода, чтобы показать, как я это делаю.

  // Requires 1.5 or above class Foo extends Activity implements SensorEventListener { SensorManager sensorManager; float[] gData = new float[3]; // Gravity or accelerometer float[] mData = new float[3]; // Magnetometer float[] orientation = new float[3]; float[] Rmat = new float[9]; float[] R2 = new float[9]; float[] Imat = new float[9]; boolean haveGrav = false; boolean haveAccel = false; boolean haveMag = false; onCreate() { // Get the sensor manager from system services sensorManager = (SensorManager)getSystemService(Context.SENSOR_SERVICE); } onResume() { super.onResume(); // Register our listeners Sensor gsensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY); Sensor asensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER); Sensor msensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD); sensorManager.registerListener(this, gsensor, SensorManager.SENSOR_DELAY_GAME); sensorManager.registerListener(this, asensor, SensorManager.SENSOR_DELAY_GAME); sensorManager.registerListener(this, msensor, SensorManager.SENSOR_DELAY_GAME); } public void onSensorChanged(SensorEvent event) { float[] data; switch( event.sensor.getType() ) { case Sensor.TYPE_GRAVITY: gData[0] = event.values[0]; gData[1] = event.values[1]; gData[2] = event.values[2]; haveGrav = true; break; case Sensor.TYPE_ACCELEROMETER: if (haveGrav) break; // don't need it, we have better gData[0] = event.values[0]; gData[1] = event.values[1]; gData[2] = event.values[2]; haveAccel = true; break; case Sensor.TYPE_MAGNETIC_FIELD: mData[0] = event.values[0]; mData[1] = event.values[1]; mData[2] = event.values[2]; haveMag = true; break; default: return; } if ((haveGrav || haveAccel) && haveMag) { SensorManager.getRotationMatrix(Rmat, Imat, gData, mData); SensorManager.remapCoordinateSystem(Rmat, SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, R2); // Orientation isn't as useful as a rotation matrix, but // we'll show it here anyway. SensorManager.getOrientation(R2, orientation); float incl = SensorManager.getInclination(Imat); Log.d(TAG, "mh: " + (int)(orientation[0]*DEG)); Log.d(TAG, "pitch: " + (int)(orientation[1]*DEG)); Log.d(TAG, "roll: " + (int)(orientation[2]*DEG)); Log.d(TAG, "yaw: " + (int)(orientation[0]*DEG)); Log.d(TAG, "inclination: " + (int)(incl*DEG)); } } } 

Хммм; Если у вас будет удобная библиотека Quaternion, возможно, проще просто получить TYPE_ROTATION_VECTOR и преобразовать ее в массив.

На вопрос, где найти полный код, вот реализация по умолчанию на железе Android: https://android.googlesource.com/platform/frameworks/base/+/jb-release/services/sensorservice/ Начните с проверки слияния. CPP / ч. Он использует измененные параметры Родригса (близкие к углам Эйлера) вместо кватернионов. В дополнение к ориентации фильтр Калмана оценивает дрейф гироскопа. Для обновлений измерений он использует магнитометр и, немного неправильно, ускорение (удельная сила).

Чтобы использовать код, вы должны быть либо мастером, либо знать основы INS и KF. Чтобы фильтр работал, необходимо настроить параметры. Как правильно сказал Эдвард, эти ребята делают это для жизни.

По крайней мере, в галактической галактике Google эта реализация по умолчанию остается неиспользованной и переопределяется запатентованной системой Invense.