加速度計の代わりにAndroidジャイロスコープを使用しています。ビットやピースがたくさんありますが、完全なコードはありません
センサーフュージョンビデオは見栄えは良いですが、コードはありません： 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);
}
ジャイロスコープのデータを追加する方法を理解しようとしましたが、正しく実行していません。 http://developer.Android.com/reference/Android/hardware/SensorEvent.html にあるGoogleドキュメントは、ジャイロスコープデータからデルタマトリックスを取得するためのコードを示しています。その考えは、加速度計と磁気センサーのフィルターを下げて、それらが本当に安定するようにすることです。それは長期的な方向性を追跡するでしょう。
次に、ジャイロスコープからの最新の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;
}
コードの2番目のブロックには、ジャイロスコープをミックスに追加する最初のコードブロックからの変更が含まれています。
具体的には、accelのフィルター係数を小さくします（値をより安定させます）。 MatrixHistoryクラスは、gyroscopeSensorChangedメソッドで計算された最新の100ジャイロスコープdeltaRotationMatrix値を追跡します。
このトピックに関するこのサイトの多くの質問を見てきました。彼らは私がこの時点に到達するのを助けましたが、私は次に何をすべきか理解できません。センサーフュージョンの人がどこかにコードを投稿したことを本当に望みます。彼は明らかにすべてをまとめました。
まあ、カルマンフィルターとは何かを知っているあなたに+1してください。よろしければ、この投稿を編集して、数年前に書いたコードをお伝えします。
しかし、最初に、なぜそれが必要ないのかを説明します。
上記のスタンが述べたように、Androidセンサースタックの使用Sensor Fusion）の最新の実装。これは、すべての利用可能なデータ（accel、mag、gyro）が1つのアルゴリズムで収集され、すべての出力がAndroidセンサーの形式で読み取られます。
編集：私はこの素晴らしいGoogle Tech Talkに偶然遭遇しました： Sensor Fusion on Android Devices：A Revolution in Motion Processing 。見るのに45分の価値がありますあなたがトピックに興味があるならそれ。
本質的に、Sensor Fusionはブラックボックスです。私はAndroid実装のソースコードを調べました。これはC++で記述された大きなカルマンフィルターです。かなり良いコードがいくつかあり、これまでに作成したどのフィルターよりもはるかに洗練されています。そして、おそらくあなたが書いているものよりも洗練されていることを覚えておいてください。
また、少なくとも1つのチップセットメーカーが独自のセンサーフュージョンを実装していることも知っています。次に、デバイスの製造元は、独自の基準に基づいてAndroidとベンダーの実装のいずれかを選択します。
最後に、スタンが前述したように、Invensenseはチップレベルで独自のセンサーフュージョンを実装しています。
とにかく、それがすべて要約すると、デバイスに組み込まれているセンサーフュージョンは、あなたや私が一緒にできるものより優れている可能性が高いということです。したがって、本当にしたいことは、それにアクセスすることです。
Androidには、物理センサーと仮想センサーの両方があります。仮想センサーは、利用可能な物理センサーから合成されるセンサーです。最もよく知られている例はTYPE_ORIENTATIONで、加速度計と磁力計を使用して、ロール/ピッチ/ヘッディング出力を作成します。 （ちなみに、このセンサーは使用しないでください。制限が多すぎます。）
しかし重要なことは、Androidの新しいバージョンには、次の2つの新しい仮想センサーが含まれていることです。
TYPE_GRAVITYは、モーションの影響が除外された加速度計入力です。TYPE_LINEAR_ACCELERATIONは、重力成分が除外された加速度計です。
これら2つの仮想センサーは、加速度計入力とジャイロ入力の組み合わせによって合成されます。
もう1つの注目すべきセンサーは、加速度計、磁力計、ジャイロから合成されたQuaternionであるTYPE_ROTATION_VECTORです。これは、デバイスの完全な3次元方向を表し、線形加速度の影響が除外されます。
ただし、Quaternionsはほとんどの人にとって少し抽象的であり、いずれにしても3次元変換を使用する可能性があるため、最善のアプローチは、SensorManager.getRotationMatrix（）を介してTYPE_GRAVITYとTYPE_MAGNETIC_FIELDを組み合わせることです。
もう1つのポイント：古いバージョンの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 Jelly Bean： https://Android.googlesource.com/platform/frameworks/base/+/jb -release/services/sensorservice / まず、fusion.cpp/hを確認します。クォータニオンではなく、修正ロドリゲスパラメーター（オイラー角に近い）を使用します。方向付けに加えて、カルマンフィルターはジャイロドリフトを推定します。測定値の更新では、磁力計と、少し間違って、加速度（特定の力）を使用します。
コードを使用するには、ウィザードであるか、INSとKFの基本を知っている必要があります。フィルターを機能させるには、多くのパラメーターを微調整する必要があります。エドワードが十分に述べたように、これらの人たちは生きるためにこれを行っています。
少なくともグーグルのギャラクシーネクサスでは、このデフォルトの実装は未使用のままにされ、インベンスの専用システムによってオーバーライドされます。