Complementary filter (Gyro + accel) with 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 ne开发者_JS百科ed 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 !

I can only repeat myself.

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:

track small movements of iphone with no GPS
What is the real world accuracy of phone accelerometers when used for positioning?
how to calculate phone's movement in the vertical direction from rest?
iOS: Movement Precision in 3D Space
How to use Accelerometer to measure distance for Android Application Development
Distance moved by Accelerometer
How can I find distance traveled with a gyroscope and accelerometer?

I wrote a tutorial on the use of the Complementary Filter for oriëntation tracking with gyroscope and accelerometer: http://www.pieter-jan.com/node/11 maybe it can help you.

I test your code and found that probably the scale factor is not consistent. Convert the pitch to 0-pi gives better result. In my test, the filtered result is ~90 degrees.

pitch = (float) Math.toDegrees(Math.atan2(accel[1], Math.sqrt(Math.pow(accel[2], 2) + Math.pow(accel[0],   2))));
pitch = pitch*PI/180.f;

filtered_angle = weight * (filtered_angle + event.values[0] * dT) + (1.0f-weight)* (pitch);

i tried and this will give you angle 90...

filtered_angle = (filtered_angle / 83) * 90;