Complementary Filter

    互補濾波是另一種比較簡單取得平滑的採樣值的方法，計算起來比卡爾曼濾波簡單，要將兩個感測器的值互補只要對一方做高通濾波（陀螺儀瞬時動態較靈敏所以高頻的值較有效）一方做低通濾波（加速度計長時間靜態的值較準確所以低頻的值較有效）然後相加，但兩者乘上的係數相加要等於一，這篇文章有比較詳細的說明我是參考他的，將陀螺儀的角速度乘上時間微分dt然後連續積分就可以得到該時間累積的角度，再與加速度計的角度做互補濾波即可，另外WMP與Wii Nunchuk使用延伸模式時對I2C操作跟單純使用單一個不一樣（參考Activated Wii Motion Plus in Nunchuck passthrough mode），使用這個模式兩者好像會有3ms左右的採樣延遲（MultiWii Source Code　－　IMU.ino內的computeIMU注解說的，我沒計算過驗證）。

方向

Acc只有加速度計Noise突出很多，AccGyro有用陀螺儀做互補所以較平滑。

Complementary Filter.ino

#include <Wire.h>

uint8_t buffer[6];
int xID;

double AccAngleX = 0,AccAngleY = 0;

int GyroLastYaw = 0,GyroLastPitch = 0,GyroLastRoll = 0;
double GyroAngleYaw = 0,GyroAnglePitch = 0,GyroAngleRoll = 0;

double HighPass = 0.93f,LowPass = 0.07f;
double AngleX = 0,AngleY = 0;

unsigned long last_loop_time;       //ms
unsigned long last_gyro_computing_time;  //us

void setup()
{
  Serial.begin(115200);
  Wire.begin();
  Init6DOF();
  last_loop_time = millis();
  last_gyro_computing_time = micros();
}

void loop()
{
  if(millis() > (last_loop_time + 10)){
    IICRead(buffer,0x52,6);
 
    if((buffer[5] & 0x03) == 0x00){
      ComputingNunchuk(buffer);
    }
    else if((buffer[5] & 0x03) == 0x02){
      ComputingWMP(buffer);
    }
 
    ComplementaryFilter();
 
    IICWrite(0x52,0x00);
    last_loop_time = millis();
  }
}

void Init6DOF()
{
  delay(100);

  IICWrite(0x53,0xFE,0x05);
  Serial.println("Passthrough Mode Ok!");
  delay(100);

  IICWrite(0x53,0xF0,0x55);
  Serial.println("Init WMP Ok!");
  delay(100);

  IICWrite(0x52,0xFA);
  Serial.println("Set Reading Address 0xFA Ok!");
  delay(100);

  IICRead(buffer,0x52,6);
  xID = buffer[0] + buffer[1] + buffer[2] +
  buffer[3] + buffer[4] + buffer[5];
  Serial.print("Extension Controller xID = 0x");
  Serial.println(xID,HEX);
  if(xID == 0xCB){
    Serial.println("WMP Connected But Not Avtivared!");
  }
  if(xID == 0xCE){
    Serial.println("WMP Connected And Avtivared!");
  }
  if(xID == 0x00){
    Serial.println("WMP Not Connected!");
  }
  delay(100);

  IICWrite(0x52,0x8);
  Serial.println("Set Reading Address 0x08 Ok!");
  delay(100);

  IICWrite(0x52,0x00);
}

double Map(double value,double Input_Min,double Input_Max,double Output_Min,double Output_Max)
{
  double rValue = (value - Input_Min) * (Output_Max - Output_Min) / (Input_Max - Input_Min) + Output_Min;

  double rMin,rMax;
  if(Output_Min < Output_Max){
    rMin = Output_Min;
    rMax = Output_Max;
  }
  else{
    rMin = Output_Max;
    rMax = Output_Min;
  }
  if(rValue < rMin){
    return rMin;
  }
  if(rValue > rMax){
    return rMax;
  }

  return rValue;
}

void ComputingNunchuk(uint8_t *buf)
{
  int accel_x_axis = (buf[2] << 2) + ((buf[5] >> 3) & 2);
  int accel_y_axis = (buf[3] << 2) + ((buf[5] >> 4) & 2);
  int accel_z_axis = (buf[4] << 2) + ((buf[5] >> 5) & 6);

  int mapX = Map(accel_x_axis,300.0f,700.0f,-90.0f,90.0f);
  int mapY = Map(accel_y_axis,300.0f,700.0f,-90.0f,90.0f);
  int mapZ = Map(accel_z_axis,360.0f,760.0f,-90.0f,90.0f);

  AccAngleX = atan2(mapX,mapZ) / 3.14159 * 180.0f;
  AccAngleY = atan2(mapY,mapZ) / 3.14159 * 180.0f;
  /*
  Serial.print(accel_x_axis);
  Serial.print(" ");
  Serial.print(accel_y_axis);
  Serial.print(" ");
  Serial.println(accel_z_axis);
  */
}

void ComputingWMP(uint8_t *buf)
{
  int yaw = (((buf[5] & 0xFC) << 6) + buf[0]);
  int pitch = (((buf[4] & 0xFC) << 6) + buf[1]);
  int roll = (((buf[3] & 0xFC) << 6) + buf[2]);

  double GyroDiffYaw = (yaw - GyroLastYaw) / 14.375f;
  double GyroDiffPitch = (pitch - GyroLastPitch) / 14.375f;
  double GyroDiffRoll = (roll - GyroLastRoll) / 14.375f;

  GyroAngleYaw  = GyroDiffYaw *
  (double)(micros() - last_gyro_computing_time) / 1000000.0f;
  GyroAnglePitch = GyroDiffPitch *
  (double)(micros() - last_gyro_computing_time) / 1000000.0f;
  GyroAngleRoll = GyroDiffRoll *
  (double)(micros() - last_gyro_computing_time) / 1000000.0f;

  last_gyro_computing_time = micros();

  GyroLastYaw = yaw;
  GyroLastPitch = pitch;
  GyroLastRoll = roll;
  /*
  Serial.print(yaw);
  Serial.print(" ");
  Serial.print(pitch);
  Serial.print(" ");
  Serial.println(roll);
  */
  /*
  Serial.print(GyroAngleYaw);
  Serial.print(" ");
  Serial.print(GyroAnglePitch);
  Serial.print(" ");
  Serial.println(GyroAngleRoll);
  */
}

void ComplementaryFilter()
{
  AngleX = (HighPass * (AngleX + GyroAngleYaw)) + (LowPass * AccAngleX);
  AngleY = (HighPass * (AngleY + GyroAnglePitch)) + (LowPass * AccAngleY);

  Serial.print(AngleX);
  Serial.print("  ");
  Serial.println(AngleY);

}

void IICWrite(uint8_t address,uint8_t register_address)
{
  Wire.beginTransmission(address);
  Wire.write(register_address);
  Wire.endTransmission();
}

void IICWrite(uint8_t address,uint8_t register_address,uint8_t data)
{
  Wire.beginTransmission(address);
  Wire.write(register_address);
  Wire.write(data);
  Wire.endTransmission();
}

void IICRead(uint8_t *buf,uint8_t address,uint8_t length)
{
  Wire.requestFrom(address,length);
  for(int i = 0;Wire.available();++i){
    buf[i] = Wire.read();
  }
}