Arduino uno + mpu6050 陀螺儀 運用卡爾曼濾波姿态解算源代碼（編譯通過）

關于怎麼接線，請參照：

https://blog.csdn.net/ling3ye/article/details/51360568

原創的作者的代碼，不過如果用原創的代碼，不用私有庫會出現編譯錯誤，修改為了私有庫的調用就解決了，應該有很多人都遇到這個坎，誤解而放棄了使用。

這是.ino程式檔案：

//連線方法
//MPU-UNO
//VCC-VCC
//GND-GND
//SCL-A5
//SDA-A4
//INT-2 (Optional)

#include "Kalman.h"
#include <Wire.h>
#include <Math.h>

float fRad2Deg = 57.295779513f; //将弧度轉為角度的乘數
const int MPU = 0x68; //MPU-6050的I2C位址
const int nValCnt = 7; //一次讀取寄存器的數量

const int nCalibTimes = 1000; //校準時讀數的次數
int calibData[nValCnt]; //校準資料

unsigned long nLastTime = 0; //上一次讀數的時間
float fLastRoll = 0.0f; //上一次濾波得到的Roll角
float fLastPitch = 0.0f; //上一次濾波得到的Pitch角
Kalman kalmanRoll; //Roll角濾波器
Kalman kalmanPitch; //Pitch角濾波器

void setup() {
  Serial.begin(9600); //初始化序列槽，指定波特率
  Wire.begin(); //初始化Wire庫
  WriteMPUReg(0x6B, 0); //啟動MPU6050裝置

  Calibration(); //執行校準
  nLastTime = micros(); //記錄目前時間
}

void loop() {
  int readouts[nValCnt];
  ReadAccGyr(readouts); //讀出測量值
  
  float realVals[7];
  Rectify(readouts, realVals); //根據校準的偏移量進行糾正

  //計算加速度向量的模長，均以g為機關
  float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]);
  float fRoll = GetRoll(realVals, fNorm); //計算Roll角
  if (realVals[1] > 0) {
    fRoll = -fRoll;
  }
  float fPitch = GetPitch(realVals, fNorm); //計算Pitch角
  if (realVals[0] < 0) {
    fPitch = -fPitch;
  }

  //計算兩次測量的時間間隔dt，以秒為機關
  unsigned long nCurTime = micros();
  float dt = (double)(nCurTime - nLastTime) / 1000000.0;
  //對Roll角和Pitch角進行卡爾曼濾波
  float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
  float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt);
  //跟據濾波值計算角度速
  float fRollRate = (fNewRoll - fLastRoll) / dt;
  float fPitchRate = (fNewPitch - fLastPitch) / dt;
 
 //更新Roll角和Pitch角
  fLastRoll = fNewRoll;
  fLastPitch = fNewPitch;
  //更新本次測的時間
  nLastTime = nCurTime;

  //向序列槽列印輸出Roll角和Pitch角，運作時在Arduino的序列槽螢幕中檢視
  Serial.print("Roll:");
  Serial.print(fNewRoll); Serial.print('(');
  Serial.print(fRollRate); Serial.print("),\tPitch:");
  Serial.print(fNewPitch); Serial.print('(');
  Serial.print(fPitchRate); Serial.print(")\n");
  delay(10);
}

//向MPU6050寫入一個位元組的資料
//指定寄存器位址與一個位元組的值
void WriteMPUReg(int nReg, unsigned char nVal) {
  Wire.beginTransmission(MPU);
  Wire.write(nReg);
  Wire.write(nVal);
  Wire.endTransmission(true);
}

//從MPU6050讀出一個位元組的資料
//指定寄存器位址，傳回讀出的值
unsigned char ReadMPUReg(int nReg) {
  Wire.beginTransmission(MPU);
  Wire.write(nReg);
  Wire.requestFrom(MPU, 1, true);
  Wire.endTransmission(true);
  return Wire.read();
}

//從MPU6050讀出加速度計三個分量、溫度和三個角速度計
//儲存在指定的數組中
void ReadAccGyr(int *pVals) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.requestFrom(MPU, nValCnt * 2, true);
  Wire.endTransmission(true);
  for (long i = 0; i < nValCnt; ++i) {
    pVals[i] = Wire.read() << 8 | Wire.read();
  }
}

//對大量讀數進行統計，校準平均偏移量
void Calibration()
{
  float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0};
  //先求和
  for (int i = 0; i < nCalibTimes; ++i) {
    int mpuVals[nValCnt];
    ReadAccGyr(mpuVals);
    for (int j = 0; j < nValCnt; ++j) {
      valSums[j] += mpuVals[j];
    }
  }
  //再求平均
  for (int i = 0; i < nValCnt; ++i) {
    calibData[i] = int(valSums[i] / nCalibTimes);
  }
  calibData[2] += 16384; //設晶片Z軸豎直向下，設定靜态工作點。
}

//算得Roll角。算法見文檔。
float GetRoll(float *pRealVals, float fNorm) {
  float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormXZ / fNorm;
  return acos(fCos) * fRad2Deg;
}

//算得Pitch角。算法見文檔。
float GetPitch(float *pRealVals, float fNorm) {
  float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormYZ / fNorm;
  return acos(fCos) * fRad2Deg;
}

//對讀數進行糾正，消除偏移，并轉換為實體量。公式見文檔。
void Rectify(int *pReadout, float *pRealVals) {
  for (int i = 0; i < 3; ++i) {
    pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f;
  }
  pRealVals[3] = pReadout[3] / 340.0f + 36.53;
  for (int i = 4; i < 7; ++i) {
    pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f;
  }
}
           

這是Kalman.cpp檔案，把這個檔案和.h檔案一起放在.ino程式檔案同一目錄下面，編譯的時候調用私有的這個頭檔案，不要調用從庫裡面下載下傳的Kalman庫檔案，不然那編譯不通過，如果安裝了kalman，就将其删除了。

#include "Kalman.h"
 
 
Kalman::Kalman() {
    Q_angle = 0.001f;
    Q_bias = 0.003f;
    R_measure = 0.03f;
 
 
    angle = 0.0f;
    bias = 0.0f;
 
 
    P[0][0] = 0.0f;
    P[0][1] = 0.0f;
    P[1][0] = 0.0f;
    P[1][1] = 0.0f;
};
 
 
float Kalman::getAngle(float newAngle, float newRate, float dt) {
    rate = newRate - bias;
    angle += dt * rate;
    P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
    P[0][1] -= dt * P[1][1];
    P[1][0] -= dt * P[1][1];
    P[1][1] += Q_bias * dt;
 
 
    float S = P[0][0] + R_measure;
    float K[2];
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;
 
 
    float y = newAngle - angle;
    angle += K[0] * y;
    bias += K[1] * y;
 
 
    float P00_temp = P[0][0];
    float P01_temp = P[0][1];
 
 
    P[0][0] -= K[0] * P00_temp;
    P[0][1] -= K[0] * P01_temp;
    P[1][0] -= K[1] * P00_temp;
    P[1][1] -= K[1] * P01_temp;
 
 
    return angle;
};
 
 
void Kalman::setAngle(float angle) { this->angle = angle; };
float Kalman::getRate() { return this->rate; };
 
 
void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };
 
 
float Kalman::getQangle() { return this->Q_angle; };
float Kalman::getQbias() { return this->Q_bias; };
float Kalman::getRmeasure() { return this->R_measure; };
           

這是kalman.h檔案：

#ifndef _Kalman_h_
#define _Kalman_h_
 
 
class Kalman {
public:
    Kalman();
 
 
    float getAngle(float newAngle, float newRate, float dt);
 
 
    void setAngle(float angle);
    float getRate();
 
 
    void setQangle(float Q_angle);
    void setQbias(float Q_bias);
    void setRmeasure(float R_measure);
 
 
    float getQangle();
    float getQbias();
    float getRmeasure();
 
 
private:
    float Q_angle;
    float Q_bias;
    float R_measure;
 
 
    float angle;
    float bias;
    float rate;
 
 
    float P[2][2];
};
 
#endif