Дополнительный фильтр (Gyro + accel) с Android

Недавно я провел некоторые исследования, чтобы использовать оба акселерометра + гироскоп, чтобы использовать эти сенсоры для отслеживания смартфона без помощи GPS (см. Этот пост) внутренняя система позиционирования на основе гироскопа и акселерометра

Для этого мне понадобится моя ориентация (угол (тангаж, крен и т. д..)) Итак, вот что я сделал до сих пор:

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;
 }
}

Здесь я пытаюсь угол (просто для тестирования) от моего акселерометра (шаг), от интеграции gyroscope_x корыто время фильтрует его с помощью дополнительного фильтра

Filtered_angle[0] = (0.98 f) * (filtered_angle[0] + gyro_x * dT) + (0.02 f)* (Шаг)

С dT начинаются более или менее 0.009 секунды

Но я не знаю, почему, но мой угол зрения не совсем точен...когда устройство расположено плашмя на столе (экран обращен вверх)

Pitch (angle fromm accel) = 1.5 (average) 
Integrate gyro = 0 to growing (normal it's drifting) 
filtered gyro angle = 1.2

И когда я поднимаю телефон на 90° (смотрите, экран передо мной обращен к стене)

Pitch (angle fromm accel) = 86 (MAXIMUM) 
Integrate gyro = he is out ok its normal 
filtered gyro angle = 83 (MAXIMUM)

То есть углы никогда не достигают 90 ??? Даже если я попытаюсь поднять телефон еще немного... Почему он не идет до 90° ? Неужели мои расчеты неверны? или качество сенсора-дерьмо?

Еще одна вещь, которая меня интересует, это то, что: с Android я не "считываю" значение датчика, но я уведомлен, когда они меняются. Проблема в том, что, как вы видите в коде, Аксель и гироскоп используют один и тот же метод.... поэтому, когда я вычислю отфильтрованный угол, я возьму шаг Акселя за 0,009 секунды до этого, нет ? Это может быть, источник моей проблемы?

Спасибо !