关于MPU6050的数据获取、分析与处理

转载： Arduino教程：MPU6050的数据获取、分析与处理，

参考资料及样例：

1. 【自己翻译】-wire.h：https://blog.csdn.net/acktomas/article/details/88069640
2. Wire库的官方文档（http://www.arduino.cc/en/Reference/Wire）
3. MPU-60X0datasheet中文版和英文版V4.0： https://download.csdn.net/download/acktomas/11096370
4. 【译】使用Arduino+MPU6050传感器DIY倾角仪 https://www.yiboard.com/thread-805-1-1.html
5. 卡尔曼滤波学习笔记: https://www.geek-workshop.com/thread-9065-1-1.html
6. https://forum.arduino.cc/index.php?topic=72442.0
7. 实践 基于 Arduino I2C读取 MPU6050 三轴陀螺仪数据
   https://blog.csdn.net/pengdali/article/details/79056588
8. 【翻译】关于ADXL345连接指南： https://blog.csdn.net/acktomas/article/details/88870325
9. 关于加速度计收藏的几篇文章：https://blog.csdn.net/acktomas/article/details/88880075

摘要

MPU6050是一种非常流行的空间运动传感器芯片，可以获取器件当前的三个加速度分量和三个旋转角速度。由于其体积小巧，功能强大，精度较高，不仅被广泛应用于工业，同时也是航模爱好者的神器，被安装在各类飞行器上驰骋蓝天。

随着Arduino开发板的普及，许多朋友希望能够自己制作基于MPU6050的控制系统，但由于缺乏专业知识而难以上手。此外，MPU6050的数据是有较大噪音的，若不进行滤波会对整个控制系统的精准确带来严重影响。

MPU6050芯片内自带了一个数据处理子模块DMP，已经内置了滤波算法，在许多应用中使用DMP输出的数据已经能够很好的满足要求。关于如何获取DMP的输出数据，我将在以后的文章中介绍。本文将直接面对原始测量数据，从连线、芯片通信开始一步一步教你如何利用Arduino获取MPU6050的数据并进行卡尔曼滤波，最终获得稳定的系统运动状态。

1. Arduino与MPU-6050的通信

为避免纠缠于电路细节，我们直接使用集成的MPU6050模块。MPU6050的数据接口用的是I2C总线协议，因此我们需要Wire程序库的帮助来实现Arduino与MPU6050之间的通信。请先确认你的Arduino编程环境中已安装Wire库。

Wire库的官方文档（http://www.arduino.cc/en/Reference/Wire）中指出：在UNO板子上，SDA接口对应的是A4引脚，SCL对应的是A5引脚。MPU6050需要5V的电源，可由UNO板直接供电。按照下图连线。

（紫色线是中断线，这里用不到，可以不接）

MPU6050的数据写入和读出均通过其芯片内部的寄存器实现，这些寄存器的地址都是1个字节，也就是8位的寻址空间，其寄存器的详细列表说明书请点击下载：https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf
，也可以从本地下载：MPU-60X0寄存器表中文版和英文版V4.0

1.1 将数据写入MPU-6050

在每次向器件写入数据前要先打开Wire的传输模式，并指定器件的总线地址，MPU6050的总线地址是0x68（AD0引脚为高电平时地址为0x69）。然后写入一个字节的寄存器起始地址，再写入任意长度的数据。这些数据将被连续地写入到指定的起始地址中，超过当前寄存器长度的将写入到后面地址的寄存器中。写入完成后关闭Wire的传输模式。下面的示例代码是向MPU6050的0x6B寄存器写入一个字节0。

Wire.beginTransmission(0x68); //开启MPU6050的传输
Wire.write(0x6B); //指定寄存器地址
Wire.write(0); //写入一个字节的数据
Wire.endTransmission(true); //结束传输，true表示释放总线

1.2 从MPU-6050读出数据

读出和写入一样，要先打开Wire的传输模式，然后写一个字节的寄存器起始地址。接下来将指定地址的数据读到Wire库的缓存中，并关闭传输模式。最后从缓存中读取数据。下面的示例代码是从MPU6050的0x3B寄存器开始读取2个字节的数据：

Wire.beginTransmission(0x68); //开启MPU6050的传输
Wire.write(0x3B); //指定寄存器地址
Wire.requestFrom(0x68, 2, true); //将输据读出到缓存
Wire.endTransmission(true); //关闭传输模式
int val = Wire.read() << 8 | Wire.read(); //两个字节组成一个16位整数

1.3 具体实现

通常应当在setup函数中对Wire库进行初始化：

Wire.begin();
在对MPU6050进行各项操作前，必须启动该器件，向它的0x6B写入一个字节0即可启动。通常也是在setup函数完成，代码见1.1节。

2. MPU6050的数据格式

我们感兴趣的数据位于0x3B到0x48这14个字节的寄存器中。这些数据会被动态更新，更新频率最高可达1000HZ。下面列出相关寄存器的地址，数据的名称。注意，每个数据都是2个字节。

• 0x3B，加速度计的X轴分量ACC_X
• 0x3D，加速度计的Y轴分量ACC_Y
• 0x3F，加速度计的Z轴分量ACC_Z
• 0x41，当前温度TEMP
• 0x43，绕X轴旋转的角速度GYR_X
• 0x45，绕Y轴旋转的角速度GYR_Y
• 0x47，绕Z轴旋转的角速度GYR_Z

MPU6050芯片的坐标系是这样定义的：令芯片表面朝向自己，将其表面文字转至正确角度，此时，以芯片内部中心为原点，水平向右的为X轴，竖直向上的为Y轴，指向自己的为Z轴。见下图：

我们只关心加速度计和角速度计数据的含义，下面分别介绍。

2.1 加速度计

加速度计的三轴分量ACC_X、ACC_Y和ACC_Z均为16位有符号整数，分别表示器件在三个轴向上的加速度，取负值时加速度沿坐标轴负向，取正值时沿正向。

三个加速度分量均以重力加速度g的倍数为单位，能够表示的加速度范围，即倍率可以统一设定，有4个可选倍率：2g、4g、8g、16g。以ACC_X为例，若倍率设定为2g（默认），则意味着ACC_X取最小值-32768时，当前加速度为沿X轴正方向2倍的重力加速度；若设定为4g，取-32768时表示沿X轴正方向4倍的重力加速度，以此类推。显然，倍率越低精度越好，倍率越高表示的范围越大（指的是加速度值越大，比如碰撞），这要根据具体的应用来设定。

加速度计设置寄存器（地址：0X1C）

我们用f表示倍率，f=0为2g，f=3为16g，设定加速度倍率的代码如下：

Wire.beginTransmission(0x68); //开启MPU-6050的传输
Wire.write(0x1C); //加速度倍率寄存器的地址
Wire.requestFrom(0x68, 1, true); //先读出原配置
unsigned char acc_conf = Wire.read();
//(acc_conf & 0xE7) 在保证其它位不受影响的情况下，设置AFS_SEL为0
acc_conf = ((acc_conf & 0xE7) | (f << 3));   
Wire.write(acc_conf);
Wire.endTransmission(true); //结束传输，true表示释放总线

再以ACC_X为例，若当前设定的加速度倍率为4g，那么将ACC_X读数换算为加速度的公式为：$$a_x=\frac{4g\times \text{ACC\_X}}{32768}$$ ，g可取当地重力加速度。

2.2 角速度计

绕X、Y和Z三个座标轴旋转的角速度分量GYR_X、GYR_Y和GYR_Z均为16位有符号整数。从原点向旋转轴方向看去，取正值时为顺时针旋转，取负值时为逆时针旋转。

三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。以GYR_X为例，若倍率设定为250度/秒，则意味着GYR取正最大值32768时，当前角速度为顺时针250度/秒；若设定为500度/秒，取32768时表示当前角速度为顺时针500度/秒。显然，倍率越低精度越好，倍率越高表示的范围越大。

陀螺仪寄存器

我们用f表示倍率，f=0为250度/秒，f=3为2000度/秒，除角速度倍率寄存器的地址为0x1B之外，设定加速度倍率的代码与2.1节代码一致。

以GYR_X为例，若当前设定的角速度倍率为1000度/秒，那么将GRY_X读数换算为角速度（顺时针）的公式为：$$g_x=1000\times \frac{\text{GYR\_X}}{32768}$$

3. 运动数据

在读取加速度计和角速度计的数据并换算为物理值后，根据不同的应用，数据有不同的解译方式。本章将以飞行器运动模型为例，根据加速度和角速度来算出当前的飞行姿态。

3.1 加速度计模型

我们可以把加速度计想象为一个正立方体盒子里放着一个球，这个球被弹簧固定在立方体的中心。当盒子运动时，根据假想球的位置即可算出当前加速度的值。想象如果在太空中，盒子没有任何受力时，假想球将处于正中心的位置，三个轴的加速度均为0。见下图：

如果我们给盒子施加一个水平向左的力，那么显然盒子就会有一个向左的加速度，此时盒内的假想球会因为惯性作用贴向盒内的右侧面。如下图所示：

为了保证数据的物理意义，MPU6050的加速度计是以假想球在三轴上座标值的相反数作为三个轴的加速度值。当假想球的位置偏向一个轴的正向时，该轴的加速度读数为负值，当假想球的位置偏向一个轴的负向时，该轴的加速度读数为正值。
根据以上分析，当我们把MPU6050芯片水平放于地方，芯片表面朝向天空，此时由于受到地球重力的作用， 假想球的位置偏向Z轴的负向，因此Z轴的加速度读数应为正，且在理想情况下应为g。注意，此加速度的物理意义并不是重力加速度，而是自身运动的加速度，可以这样理解：正因为其自身运动的加速度与重力加速度大小相等方向相反，芯片才能保持静止。

3.2 Roll-pitch-yaw模型与姿态计算

表示飞行器当前飞行姿态的一个通用模型就是建立下图所示坐标系，并用Roll表示绕X轴的旋转，Pitch表示绕Y轴的旋转，Yaw表示绕Z轴的旋转。

由于MPU6050可以获取三个轴向的加速度，而地球重力则是长期存在且永远竖直向下，因此我们可以根据重力加速度相对于芯片的指向为参考算得当前姿态。
为方便起见，我们让芯片正面朝下固定在上图飞机上，且座标系与飞机的坐标系完全重合，以三个轴向的加速度为分量，可构成加速度向量a(x,y,z)。假设当前芯片处于匀速直线运动状态，那么a应垂直于地面向上，即指向Z轴负方向，模长为$|a|=g=\sqrt{x^2+y^2+z^2}（与重力加速度大小相等，方向相反，见3.1节）$。若芯片（座标系）发生旋转，由于加速度向量a仍然竖直向上，所以Z轴负方向将不再与a重合。见下图。

为了方便表示，上图坐标系的Z轴正方向（机腹以及芯片正面）向下，X轴正方向（飞机前进方向）向右。此时芯片的Roll角$\varphi$（黄色）为加速度向量与其在XZ平面上投影(x,0,z)的夹角，Pitch角$\omega$（绿色）与其在YZ平面上投影(0,y,z)的夹角。求两个向量的夹角可用点乘公式：$$a\cdot b=|a|\cdot |b|\cdot \cos \theta$$，简单推导可得：

$$\varphi ={\cos }^{-1}(\sqrt{x^2+z^2}/g)，以及\omega ={\cos }^{-1}(\sqrt{y^2+z^2}/g)$$

注意，因为arccos函数只能返回正值角度，因此还需要根据不同情况来取角度的正负值。当y值为正时，Roll角要取负值，当x轴为负时，Pitch角要取负值。

3.4 Yaw角的问题

因为没有参考量，所以无法求出当前的Yaw角的绝对角度，只能得到Yaw的变化量，也就是角速度GYR_Z。当然，我们可以通过对GYR_Z积分的方法来推算当前Yaw角（以初始值为准），但由于测量精度的问题，推算值会发生漂移，一段时间后就完全失去意义了。然而在大多数应用中，比如无人机，只需要获得GRY_Z就可以了。

如果必须要获得绝对的Yaw角，那么应当选用MPU9250这款九轴运动跟踪芯片，它可以提供额外的三轴罗盘数据，这样我们就可以根据地球磁场方向来计算Yaw角了，具体方法此处不再赘述。

4. 数据处理与实现

MPU6050芯片提供的数据夹杂有较严重的噪音，在芯片处理静止状态时数据摆动都可能超过2%。除了噪音，各项数据还会有偏移的现象，也就是说数据并不是围绕静止工作点摆动，因此要先对数据偏移进行校准 ，再通过滤波算法消除噪音。

4.1 校准

校准是比较简单的工作，我们只需要找出摆动的数据围绕的中心点即可。我们以GRY_X为例，在芯片处理静止状态时，这个读数理论上讲应当为0，但它往往会存在偏移量，比如我们以10ms的间隔读取了10个值如下：

-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5
这10个值的均值，也就是这个读数的偏移量为-158.25。在获取偏移量后，每次的读数都减去偏移量就可以得到校准后的读数了。当然这个偏移量只是估计值，比较准确的偏移量要对大量的数据进行统计才能获知，数据量越大越准，但统计的时间也就越慢。一般校准可以在每次启动系统时进行，那么你应当在准确度和启动时间之间做一个权衡。

三个角速度读数GYR_X、GYR_Y和GYR_Z均可通过统计求平均的方法来获得，但三个加速度分量就不能这样简单的完成了，因为芯片静止时的加速度并不为0。

加速度值的偏移来自两个方面，一是由于芯片的测量精度，导至它测得的加速度向量并不垂直于大地；二是芯片在整个系统（如无人机）上安装的精度是有限的，系统与芯片的座标系很难达到完美重合。前者我们称为读数偏移，后者我们称为角度偏移。因为读数和角度之间是非线性关系，所以要想以高精度进行校准必须先单独校准读数偏移，再把芯片固定在系统中后校准角度偏移。然而，由于校准角度偏移需要专业设备，且对于一般应用来说，两步校准带来的精度提升并不大，因此通常只进行读数校准即可。下面介绍读数校准的方法。我们还以3.2节的飞机为例，分以下几个步骤：

首先要确定飞机的坐标系，对于多轴飞行器来说这非常重要。如果坐标系原点的位置或坐标轴的方向存在较大偏差，将会给后面的飞控造成不良影响。
在确定了飞机的坐标系后，为了尽量避免读数偏移带来的影响，首先将MPU6050牢牢地固定在飞机上，并使二者座标系尽可能的重合。当然把Z轴反过来装也是可以的，就是需要重新推算一套角度换算公式。
将飞机置于水平、坚固的平面上，并充分预热。对于多轴无人机而言，空中悬停时的XY平面应当平行于校准时的XY平面。此时，我们认为芯片的加速度方向应当与Z轴负方向重合，且加速度向量的模长为g，因此ACC_X和ACC_Y的理论值应为0，ACC_Z的理论值应为-16384（假设我们设定2g的倍率，1g的加速度的读数应为最大值-32768的一半）。
由于ACC_X和ACC_Y的理论值应为0，与角速度量的校准类似，这两个读数偏移量可用统计均值的方式校准。ACC_Z则需要多一步处理，即在统计偏移量的过程中，每次读数都要加上16384，再进行统计均值校准。

4.2 卡尔曼滤波

对于夹杂了大量噪音的数据，卡尔曼滤波器的效果无疑是最好的。如果不想考虑算法细节，可以直接使用Arduino的Klaman Filter库完成。在我们的模型中，一个卡尔曼滤波器接受一个轴上的角度值、角速度值以及时间增量，估计出一个消除噪音的角度值。跟据当前的角度值和上一轮估计的角度值，以及这两轮估计的间隔时间，我们还可以反推出消除噪音的角速度。

实现代码见4.3节。下面介绍卡尔曼滤波算法细节，不感兴趣的可跳过。

（想看的人多了再写）

4.2.1 热门卡尔曼库

1. TKJElectronics/KalmanFilter：https://github.com/TKJElectronics/KalmanFilter
2. rfetick/Kalman：https://github.com/rfetick/Kalman
3. 另外：https://github.com/jarzebski给出了MPU6050库和kalmanFiler库

4.3 实现代码

以下代码在Arduino软件1.65版本中编译、烧写以及测试通过。下列代码中用到的库应该是： https://github.com/TKJElectronics/KalmanFilter

// 本代码版权归Devymex所有，以GNU GENERAL PUBLIC LICENSE V3.0发布
// http://www.gnu.org/licenses/gpl-3.0.en.html
// 相关文档参见作者于知乎专栏发表的原创文章：
// http://zhuanlan.zhihu.com/devymex/20082486

//连线方法
//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;
  }
}

5.Kalman库源码翻译

5.1 Kalman.h

#ifndef _Kalman_h_
#define _Kalman_h_

class Kalman {
public:
    Kalman();

    // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
    //角度应为度，速率应为度/秒，增量时间应为秒
    float getAngle(float newAngle, float newRate, float dt);

    //用于设置角度，应设置为起始角度
    void setAngle(float angle); // Used to set angle, this should be set as the starting angle
    float getRate(); // Return the unbiased rate//返回无偏转的速率

    /* These are used to tune the Kalman filter */
    void setQangle(float Q_angle);
    /**
     * setQbias(float Q_bias)
     * Default value (0.003f) is in Kalman.cpp. 
     * Raise this to follow input more closely,提高这一点以更密切地关注输入，
     * lower this to smooth result of kalman filter.降低此值可使卡尔曼滤波器的结果平滑。
     */
    void setQbias(float Q_bias);
    void setRmeasure(float R_measure);

    float getQangle();
    float getQbias();
    float getRmeasure();

private:
    /* Kalman filter variables卡尔曼滤波器变量 */
    //加速度计的过程噪声方差
    float Q_angle; // Process noise variance for the accelerometer
    //陀螺仪偏置的过程噪声方差
    float Q_bias; // Process noise variance for the gyro bias
    //测量噪声方差-这实际上是测量噪声的方差
    float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
    //卡尔曼滤波器计算出的角度-2x1状态向量的一部分
    float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
    //由卡尔曼滤波器计算出的陀螺仪偏置-2x1状态向量的一部分
    float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
    //根据比率和计算出的偏差计算出的无偏差比率-您必须调用getAngle来更新比率
    float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
    //误差协方差矩阵-这是一个2x2的矩阵
    float P[2][2]; // Error covariance matrix - This is a 2x2 matrix
};

#endif

5.2 Kalman.cpp

#include "Kalman.h"

Kalman::Kalman() {
    /* We will set the variables like so, these can also be tuned by the user */
    / *我们将像这样设置变量，这些也可以由用户调整* /
    Q_angle = 0.001f;
    Q_bias = 0.003f;
    R_measure = 0.03f;

    angle = 0.0f; // Reset the angle重置角度
    bias = 0.0f; // Reset bias重置偏差

    由于我们假设偏差为0，并且知道起始角度（使用setAngle），因此误差协方差矩阵的设置如下-请参见：http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
    P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
    P[0][1] = 0.0f;
    P[1][0] = 0.0f;
    P[1][1] = 0.0f;
};

//角度应为度，速率应为度/秒，增量时间应为秒
// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
float Kalman::getAngle(float newAngle, float newRate, float dt) {
    // KasBot V2-卡尔曼过滤器模块-http://www.x-firm.com/?page_id=145
    //由Kristian Lauszus修改
    //有关更多信息，请参见我的博客文章：http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

    //离散卡尔曼滤波器的时间更新方程式-时间更新（“预测”）
    //更新xhat-预测未来的状态
    // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    // Modified by Kristian Lauszus
    // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

    // Discrete Kalman filter time update equations - Time Update ("Predict")
    // Update xhat - Project the state ahead
    /* Step 1 */
    rate = newRate - bias;
    angle += dt * rate;

    //更新估算误差协方差-提前预测误差协方差
    // Update estimation error covariance - Project the error covariance ahead
    /* Step 2 */
    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;

    //离散卡尔曼滤波器测量更新方程-测量更新（“正确”）
    //计算卡尔曼增益-计算卡尔曼增益
    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    // Calculate Kalman gain - Compute the Kalman gain
    /* Step 4 */
    float S = P[0][0] + R_measure; // Estimate error//估算误差
    /* Step 5 */
    float K[2]; // Kalman gain增益 - This is a 2x1 vector这是2x1向量
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;

    //计算角度和偏差-使用测量值zk（newAngle）更新估算值
    // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    /* Step 3 */
    float y = newAngle - angle; // Angle difference//角度差
    /* Step 6 */
    angle += K[0] * y;
    bias += K[1] * y;

     //计算估计误差协方差-更新误差协方差
    // Calculate estimation error covariance - Update the error covariance
    /* Step 7 */
    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; }; // Used to set angle, this should be set as the starting angle//用于设置角度，应设置为起始角度
float Kalman::getRate() { return this->rate; }; // Return the unbiased rate//返回无偏转速率

/ *这些用于调整卡尔曼滤波器* /
/* These are used to tune the Kalman filter */
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; };

5.3 Kalman库示例

以下程序是TKJElectronics/KalmanFilter库中示例https://github.com/TKJElectronics/KalmanFilter

#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
//注释掉以将roll限制为±90度
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
double accX, accY, accZ; //加速度
double gyroX, gyroY, gyroZ; //陀螺仪
int16_t tempRaw;

double gyroXangle, gyroYangle; // Angle calculate using the gyro only//仅使用陀螺仪计算角度
double compAngleX, compAngleY; // Calculated angle using a complementary filter（使用互补滤波器（互补滤波器）计算角度）
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data// I2C数据的缓冲区

// TODO: Make calibration routine（校准程序）

void setup() {
  Serial.begin(115200);
  Wire.begin();
#if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

  i2cData[0] = 7; // Set the sample rate to 1000Hz（将采样率设置为1000Hz），即：8kHz/(7+1) = 1000Hz
  //禁用FSYNC并设置260 Hz Acc滤波，256 Hz陀螺滤波，8 KHz采样（FSYNC并设置260 Hz加速滤波，256 Hz陀螺滤波，8 KHz采样）
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling（禁用FSYNC并设置260 Hz加速滤波，256 Hz陀螺滤波，8 KHz采样）
  //将陀螺仪满量程范围设置为±250deg / s
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  //将加速度计满量程设置为±2g
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  //一次写入所有四个寄存器
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  //具有X轴陀螺仪参考并禁用睡眠模式的PLL
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode（带有X轴陀螺仪参考的PLL并禁用睡眠模式）

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register（读取“ WHO_AM_I”寄存器）
    Serial.print(F("Error reading sensor"));  （“读取传感器错误”）
    while (1);
  }
  //等待传感器稳定
  delay(100); // Wait for sensor to stabilize
  /*设置卡尔曼和陀螺仪起始角度*/
  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees然后将其从弧度转换为度
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  kalmanX.setAngle(roll); // Set starting angle//设置起始角度
  kalmanY.setAngle(pitch);
  gyroXangle = roll;
  gyroYangle = pitch;
  compAngleX = roll;
  compAngleY = pitch;

  timer = micros();
}

void loop() {
  /*更新所有值*/
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
  gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
  gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
  gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;

  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time//计算增量时间
  timer = micros();

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
  //这样可以解决加速度计角度在-180至180度之间跳动时的过渡问题
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    compAngleX = roll;
    kalAngleX = roll;
    gyroXangle = roll;
  } else
  //使用卡尔曼滤波器计算角度
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX) > 90)
    //反转速率，因此适合于限制的加速度计读数
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
  //这可解决加速度计角度在-180至180度之间跳跃时的过渡问题
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
    compAngleY = pitch;
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
  
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
  //不使用任何滤波器即可计算陀螺仪角度
  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * dt;
  //使用无偏率计算陀螺角
  //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate() * dt;
  //使用互补过滤器计算角度
  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
  //陀螺仪角度漂移太大时重置
  // Reset the gyro angle when it has drifted too much
  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;

  /* Print Data */
#if 0 // Set to 1 to activate//设置为1以激活
  Serial.print(accX); Serial.print("\t");
  Serial.print(accY); Serial.print("\t");
  Serial.print(accZ); Serial.print("\t");

  Serial.print(gyroX); Serial.print("\t");
  Serial.print(gyroY); Serial.print("\t");
  Serial.print(gyroZ); Serial.print("\t");

  Serial.print("\t");
#endif

  Serial.print(roll); Serial.print("\t");
  Serial.print(gyroXangle); Serial.print("\t");
  Serial.print(compAngleX); Serial.print("\t");
  Serial.print(kalAngleX); Serial.print("\t");

  Serial.print("\t");

  Serial.print(pitch); Serial.print("\t");
  Serial.print(gyroYangle); Serial.print("\t");
  Serial.print(compAngleY); Serial.print("\t");
  Serial.print(kalAngleY); Serial.print("\t");

#if 0 // Set to 1 to print the temperature//设置为1以打印温度
  Serial.print("\t");

  double temperature = (double)tempRaw / 340.0 + 36.53;
  Serial.print(temperature); Serial.print("\t");
#endif

  Serial.print("\r\n");
  delay(2);
}