Utilisation de Android gyroscope au lieu de l'accéléromètre. Je trouve beaucoup de morceaux, mais pas de code complet
La vidéo Sensor Fusion est superbe, mais il n'y a pas de code: http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s
Voici mon code qui utilise simplement l'accéléromètre et la boussole. J'utilise également un filtre de Kalman sur les 3 valeurs d'orientation, mais c'est trop de code à afficher ici. En fin de compte, cela fonctionne bien, mais le résultat est soit trop instable, soit trop lent en fonction de ce que je fais avec les résultats et de la faiblesse des facteurs de filtrage.
/** 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);
}
J'ai essayé de comprendre comment ajouter des données de gyroscope, mais je ne le fais tout simplement pas correctement. Le google doc à http://developer.Android.com/reference/Android/hardware/SensorEvent.html montre du code pour obtenir une matrice delta à partir des données du gyroscope. L'idée semble être que je réduirais les filtres de l'accéléromètre et des capteurs magnétiques pour qu'ils soient vraiment stables. Cela permettrait de suivre l'orientation à long terme.
Ensuite, je garderais un historique des matrices N delta les plus récentes du gyroscope. Chaque fois que j'en avais un nouveau, je déposais le plus ancien et je les multipliais tous ensemble pour obtenir une matrice finale que je multiplierais contre la matrice stable renvoyée par l'accéléromètre et les capteurs magnétiques.
Cela ne semble pas fonctionner. Ou, du moins, ma mise en œuvre ne fonctionne pas. Le résultat est bien plus nerveux que le simple accéléromètre. Augmenter la taille de l'historique du gyroscope augmente en fait la gigue, ce qui me fait penser que je ne calcule pas les bonnes valeurs à partir du gyroscope.
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;
}
Le deuxième bloc de code contient mes modifications du premier bloc de code qui ajoutent le gyroscope au mélange.
Plus précisément, le facteur de filtrage pour l'accélération est réduit (ce qui rend la valeur plus stable). La classe MatrixHistory garde la trace des 100 dernières valeurs deltaRotationMatrix du gyroscope qui sont calculées dans la méthode gyroscopeSensorChanged.
J'ai vu de nombreuses questions sur ce site à ce sujet. Ils m'ont aidé à arriver à ce point, mais je ne sais pas quoi faire ensuite. J'aurais vraiment aimé que le gars de Sensor Fusion vienne de publier du code quelque part. Il avait évidemment tout mis en place.
Eh bien, +1 à vous pour même savoir ce qu'est un filtre de Kalman. Si vous le souhaitez, je vais éditer ce post et vous donner le code que j'ai écrit il y a quelques années pour faire ce que vous essayez de faire.
Mais d'abord, je vais vous dire pourquoi vous n'en avez pas besoin.
Les implémentations modernes de la pile de capteurs Android utilisent Sensor Fusion ), comme Stan l'a mentionné ci-dessus. Cela signifie simplement que tous les les données disponibles - accel, mag, gyro - sont rassemblées dans un seul algorithme, puis toutes les sorties sont lues sous la forme de Android capteurs.
Edit: Je viens de trébucher sur ce superbe Google Tech Talk sur le sujet: Sensor Fusion on Android Devices: A Revolution in Motion Processing . Ça vaut bien les 45 minutes à regarder si vous êtes intéressé par le sujet.
En substance, Sensor Fusion est une boîte noire. J'ai regardé dans le code source de l'implémentation Android, et c'est un gros filtre Kalman écrit en C++. Un assez bon code là-dedans, et beaucoup plus sophistiqué que n'importe quel filtre que j'ai jamais écrit, et probablement plus sophistiqué que ce que vous écrivez. Rappelez-vous, ces types font ça pour gagner leur vie.
Je sais également qu'au moins un fabricant de chipsets a sa propre implémentation de fusion de capteurs. Le fabricant de l'appareil choisit alors entre le Android et l'implémentation du fournisseur en fonction de leurs propres critères.
Enfin, comme Stan l'a mentionné ci-dessus, Invensense a sa propre implémentation de fusion de capteurs au niveau de la puce.
Quoi qu'il en soit, tout cela revient à dire que la fusion de capteurs intégrée dans votre appareil est susceptible d'être supérieure à tout ce que vous ou moi pourrions bricoler. Donc ce que vous vraiment voulez faire est d'y accéder.
Sous Android, il existe des capteurs physiques et virtuels. Les capteurs virtuels sont ceux qui sont synthétisés à partir des capteurs physiques disponibles. L'exemple le plus connu est TYPE_ORIENTATION qui utilise un accéléromètre et un magnétomètre et crée une sortie roulis/tangage/cap. (Au fait, vous ne devriez pas utiliser ce capteur; il a trop de limitations.)
Mais l'important est que les nouvelles versions de Android contiennent ces deux nouveaux capteurs virtuels:
TYPE_GRAVITY est l'entrée de l'accéléromètre avec l'effet du mouvement filtré. TYPE_LINEAR_ACCELERATION est l'accéléromètre avec la composante de gravité filtrée.
Ces deux capteurs virtuels sont synthétisés grâce à une combinaison d'entrée d'accéléromètre et d'entrée gyroscopique.
Un autre capteur notable est TYPE_ROTATION_VECTOR qui est un Quaternion synthétisé à partir d'un accéléromètre, d'un magnétomètre et d'un gyroscope. Il représente l'orientation 3D complète de l'appareil avec les effets de l'accélération linéaire filtrés.
Cependant, les quaternions sont un peu abstraits pour la plupart des gens, et comme vous travaillez probablement avec des transformations 3D de toute façon, votre meilleure approche est de combiner TYPE_GRAVITY et TYPE_MAGNETIC_FIELD via SensorManager.getRotationMatrix ().
Un autre point: si vous travaillez avec un appareil exécutant une ancienne version d'Android, vous devez détecter que vous ne recevez pas d'événements TYPE_GRAVITY et utiliser TYPE_ACCELEROMETER à la place. Théoriquement, ce serait un endroit pour utiliser votre propre filtre Kalman, mais si votre appareil n'a pas de fusion de capteurs intégrée, il n'a probablement pas non plus de gyroscopes.
Quoi qu'il en soit, voici un exemple de code pour montrer comment je le fais.
// 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));
}
}
}
Hmmm; si vous avez une bibliothèque Quaternion à portée de main, il est probablement plus simple de recevoir TYPE_ROTATION_VECTOR et de la convertir en tableau.
À la question où trouver le code complet, voici une implémentation par défaut sur Android Jelly bean: https://Android.googlesource.com/platform/frameworks/base/+/jb -release/services/sensorervice / Commencez par vérifier le fichier fusion.cpp/h. Il utilise les paramètres Rodrigues modifiés (proches des angles d'Euler) au lieu des quaternions. En plus de l'orientation, le filtre de Kalman estime la dérive du gyroscope. utilise le magnétomètre et, un peu incorrectement, l'accélération (force spécifique).
Pour utiliser le code, vous devez être un assistant ou connaître les bases de INS et KF. De nombreux paramètres doivent être affinés pour que le filtre fonctionne. Comme Edward l'a bien dit, ces gars font ça pour vivre.
Au moins dans le Galaxy Nexus de Google, cette implémentation par défaut est laissée inutilisée et remplacée par le système propriétaire d'Invense.