Дополнительный фильтр Гироскоп Акселерометр

Question

Я использую комплементарный фильтр для гироскоп и акселерометр только для азимута ... Я получил его с этого сайта: http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/1/

ядро ​​фильтра:

  /* 
     * Fix for 179° <--> -179° transition problem: 
     * Check whether one of the two orientation angles (gyro or accMag) is negative while the other one is positive. 
     * If so, add 360° (2 * math.PI) to the negative value, perform the sensor fusion, and remove the 360° from the result 
     * if it is greater than 180°. This stabilizes the output in positive-to-negative-transition cases. 
     */ 

     // azimuth 
     if (gyroOrientation[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) { 
      fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[0]); 
      fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0; 
      Log.d("test","gyro Is Negative"); 
     } 
     else if (accMagOrientation[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) { 
      fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * (accMagOrientation[0] + 2.0 * Math.PI)); 
      fusedOrientation[0] -= (fusedOrientation[0] > Math.PI)? 2.0 * Math.PI : 0; 
      Log.d("test","accel Is Negative"); 

     } 
     else { 
      fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0]; 
     } 
     gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation); 
     System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3); 

Я хочу сравнить это с Real гироскоп данных, которые дрейфуют ... для этого, я использовал gyroOreintationReal ... и добавить некоторые коды, чтобы сохранить gyroOreintation, как это:

if(initState) { 
     float[] initMatrix = new float[9]; 
     initMatrix = getRotationMatrixFromOrientation(accMagOrientation); 
     float[] test = new float[3]; 
     SensorManager.getOrientation(initMatrix, test); 
     gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix); 
     gyroMatrixReal = matrixMultiplication(gyroMatrixReal, initMatrix); 

     initState = false; 
    } 

    // copy the new gyro values into the gyro array 
    // convert the raw gyro data into a rotation vector 
    float[] deltaVector = new float[4]; 
    float[] deltaVectorReal = new float[4]; 

    if(timestamp != 0) { 
     final float dT = (event.timestamp - timestamp) * NS2S; 


    System.arraycopy(event.values, 0, gyro, 0, 3); 
    System.arraycopy(event.values, 0, gyroReal, 0, 3); 


    getRotationVectorFromGyro(gyro, deltaVector, dT/2.0f); 
    getRotationVectorFromGyro(gyroReal, deltaVectorReal, dT/2.0f); 

    } 

    // measurement done, save current time for next interval 
    timestamp = event.timestamp; 

    // convert rotation vector into rotation matrix 
    float[] deltaMatrix = new float[9]; 
    float[] deltaMatrixReal = new float[9]; 

    SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector); 
    SensorManager.getRotationMatrixFromVector(deltaMatrixReal, deltaVectorReal); 


    // apply the new rotation interval on the gyroscope based rotation matrix 
    gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix); 
    gyroMatrixReal = matrixMultiplication(gyroMatrixReal, deltaMatrixReal); 


    // get the gyroscope based orientation from the rotation matrix 
    SensorManager.getOrientation(gyroMatrix, gyroOrientation); 
    SensorManager.getOrientation(gyroMatrixReal, gyroOrientationReal);  

, и я сохранил результат и привязал их к матрице ... но график показывает, что ориентация гироскопа отрицательная ... но fusedOreientation меньше +150, а ориентация ускорения немного больше +150 ...

как я могу исправить проблему ?? добавить некоторые коды к сердцевине дополнительного фильтра:

//RealGyro 
     if (gyroOrientationReal[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) { 
      gyroOrientationReal[0] = (float) (gyroOrientation[0] + 2.0 * Math.PI); 
      gyroOrientationReal[0] -= (gyroOrientationReal[0] > Math.PI) ? 2.0 * Math.PI : 0; 
     }   

это нормально, но иногда я не знаю, что я могу сделать, если у меня есть негативные данные Accel и положительные данные гироскопов?

Answer 1

когда я попытался повернуть степень телефона 360 .. Я получил этот участок от: MATLAB http://www.uppic.com/uploads/14205374821.jpg

зеленый одинокого для гироскопа, красный для плавленой ориентации и синей для разгона.

• Почему время гироскопа компенсируется ?? причина дрейфа ?? сюжет правилен?
• Как использовать 0-360 период вместо 0_180 и -180_0?

Answer 2

Ссылка на изображение, которое вы опубликовали, не работает, поэтому я не смог проверить сюжет из Matlab. Дрифт гироскопа не добавляет никакого смещения времени к данным. Возможно, смещение вызвано некоторой внутренней задержкой (недопустимой) или, скорее, задержкой в ​​алгоритме. Я не могу точно сказать, потому что я не могу видеть, насколько велика смещение времени.

Если вы хотите получить данные гироскопа, то деталь, добавленная в дополнительный фильтр, не нужна. Все данные гироскопа должны быть уже в векторе gyroOrientationReal. Никакой дальнейшей обработки не требуется. Данные реального гироскоп не должны влиять accMagOrientation (что вы делаете, изменяя gyroOrientationReal если accMagOrientation[0] > 0.0.

Если вы хотите перейти от [0, 360] периода к [0, 180, -180, 0] период нужно просто вычесть 180 из вашего результата, и вы сделали.

Я надеюсь, что я мог бы помочь вам с этим.