mpu6050 感測器 傾斜角度 低通濾波器(Low-Pass Filter)

上篇介紹如何使用 mpu6050 感測器 加速度 感測器取得傾斜角度，這篇介紹如何使用低通滤波器(Low-Pass Filter) 來改善 傾斜角度數值。

低通濾波器的想法是去除高頻率的干擾，當mpu6050左右搖晃時，因為加速度數值改變，造成傾斜角度數值變化，但實質上傾斜角度並沒有變化，因此要使用低通滤波器(Low-Pass Filter)去除劇烈的數值改變。

pitchN=0.9*pitchOld+0.1*pitchM; (讀取的Pitch 角度＝0.9 舊角度+0.1新角度)

rollN=0.9*rollOld+0.1*rollM;(讀取的Roll 角度＝0.9 舊角度+0.1新角度)

程式寫法是讀取的新角度是舊角度加上量測到新角度的數值，給予舊角度0.9的權重，新角度0.1的權重，比較相信原本的數值，意味者當數值在短時間劇烈改變時，因為量測到的角度佔的權重小，因此不會造成量測角度太大的變化。

#include <basicMPU6050.h>
#include <math.h> 

// Create instance
basicMPU6050<> imu;


float pitchM,rollM,pitchN,rollN;
float pitchOld=0;
float rollOld=0;

void setup() {
  // Set registers - Always required
  imu.setup();

  // Initial calibration of gyro
  imu.setBias();

  // Start console
  Serial.begin(38400);
}


void loop() { 
  // Update gyro calibration 
  imu.updateBias();
  // Angle
  pitchM=atan2(imu.ay(),imu.az())*180/3.14159;
  rollM=atan2(imu.ax(),imu.az())*180/3.14159;
  
  //low pass filter
  pitchN=0.9*pitchOld+0.1*pitchM;
  rollN=0.9*rollOld+0.1*rollM;
   
  Serial.print( imu.ax() );
  Serial.print( "  " );
  Serial.print( imu.ay() );
  Serial.print( "  " );
  Serial.print( imu.az() );
  Serial.print( "       " );
    
  //Angle
  Serial.print(pitchM);
  Serial.print("  ");
  Serial.print(rollM);
  Serial.print("  ");
  
  Serial.print(pitchN);
  Serial.print("  ");
  Serial.print(rollN);
  Serial.println("     ");
  //assign now value to old value
  pitchOld=pitchN;
  rollOld=rollN;
  
  delay(100);
  
  }

讀取數值如下

左右搖晃時感測器讀取的數據

但是還是有些缺點，使用low pass filter 過濾的數值無法立即顯示目前傾斜角度，數值會漸進的爬升至22度。

low pass filter的權重會影響數值的爬升速度，原本的權重分別為0.9 和0.1

pitchN=0.9*pitchOld+0.1*pitchM; (讀取的Pitch 角度＝0.9 舊角度+0.1新角度)

將權重改成0.5和0.5時

pitchN=0.5*pitchOld+0.5*pitchM;(讀取的Pitch 角度＝0.5 舊角度+0.5新角度)

會使讀取的數值爬升速度快(反應較快)，但搖晃時傾斜角度的數值較易被影響。

權重改成0.5和0.5時 y軸傾斜時

權重0.5和0.5時 左右搖晃時

若希望傾斜角度可以立即反應到數值上，就給予新角度較高的權重，若希望能降低搖晃的干擾，就給予舊角度較高的權重，使用Low-Pass Filter會面臨到反應速度和降低干擾的取捨。

這是簡單的Low-Pass Filter 應用在mpu6050感測器介紹。