android 陀螺仪滤波,互补滤波器（陀螺仪+加速度）与Android

Recently I have made some research to use both the accelerometer + Gyroscope to use those senser to track a smartphone without the help of the GPS (see this post)

Indoor Positioning System based on Gyroscope and Accelerometer

For that purpose I will need my orientation (angle (pitch, roll etc..)) so here what i have done so far:

public void onSensorChanged(SensorEvent arg0) {

if (arg0.sensor.getType() == Sensor.TYPE_ACCELEROMETER)

{

accel[0] = arg0.values[0];

accel[1] = arg0.values[1];

accel[2] = arg0.values[2];

pitch = Math.toDegrees(Math.atan2(accel[1], Math.sqrt(Math.pow(accel[2], 2) + Math.pow(accel[0], 2))));

tv2.setText("Pitch: " + pitch + "\n" + "Roll: " + roll);

} else if (arg0.sensor.getType() == Sensor.TYPE_GYROSCOPE )

{

if (timestamp != 0) {

final float dT = (arg0.timestamp - timestamp) * NS2S;

angle[0] += arg0.values[0] * dT;

filtered_angle[0] = (0.98f) * (filtered_angle[0] + arg0.values[0] * dT) + (0.02f)* (pitch);

}

timestamp = arg0.timestamp;

}

}

Here I'm trying to angle (just for testing) from my accelerometer (pitch), from integration the gyroscope_X trough time filtering it with a complementary filter

filtered_angle[0] = (0.98f) * (filtered_angle[0] + gyro_x * dT) + (0.02f)* (pitch)

with dT begin more or less 0.009 secondes

But I don't know why but my angle are not really accurate...when the device is position flat on the table (Screen facing up)

Pitch (angle fromm accel) = 1.5 (average)

Integrate gyro = 0 to growing (normal it's drifting)

filtered gyro angle = 1.2

and when I lift the phone of 90° (see the screen is facing the wall in front of me)

Pitch (angle fromm accel) = 86 (MAXIMUM)

Integrate gyro = he is out ok its normal

filtered gyro angle = 83 (MAXIMUM)

So the angles never reach 90 ??? Even if I try to lift the phone a bit more...

Why doesn't it going until 90° ? Are my calculation wrong? or is the quality of the sensor crap?

AN other thing that I'm wondering it is that: with Android I don't "read out" the value of the sensor but I'm notified when they change. The problem is that as you see in the code the Accel and Gyro share the same method.... so when I compute the filtered angle I will take the pitch of the accel measure 0.009 seconds before, no ? Is that maybe the source of my problem?

Thank you !

解决方案

You get position by integrating the linear acceleration twice but the error is horrible. It is useless in practice. In other words, you are trying to solve the impossible.

What you actually can do is to track just the orientation.

Roll, pitch and yaw are evil, do not use them. Check in the video I already recommended, at 38:25.

Here is an excellent tutorial on how to track orientation with gyros and accelerometers.

Similar questions that you might find helpful: