MPU6050使用一阶互补和卡尔曼滤波算法平滑角度数据

最近项目上想用MPU6050来自动探测物体的转向角度，花了2天时间学习如何拿陀螺仪的姿态角度，发现蛮难的，写点笔记。
下面是哔哩哔哩的一堆废话讲解，只想看代码本体的可以直接跳到最后。

应用场景是51单片机环境，有一块MPU6060，需要知道硬件板子水平摆放时，板子摆放的姿态和旋转的角度。编译环境只能用C语言。
首先单片机通过TTL串口接到MPU6050上拿到通信数据，水平旋转角度需要另外加地磁仪通过南北极磁性拿到。很遗憾设计硬件时没注意这茬，只用了一块MPU6050。不过呢可以用旋转时的角速度求出旋转幅度（这个下篇说）。但是拿到原始数据后，发现原始数据的跳动非常厉害，需要用带滤波的积分算法平滑过滤。

开始网上翻相关的算法，看到卡尔曼滤波和互补滤波算法，但是没有能跑起来的代码。但是很多纯写理论，我这种数学白痴完全看不懂。要么直接甩代码，云里雾里不明就里，根据理论推论很多代码甚至是错误的，各种计算方式，变量和函数名字也完全理解不了。
又翻了很久，在某代码里发现一句注释，找到国外TKJ Electronics的一系列代码，使用结合卡尔曼滤波和互补滤波算法的演示。终于搞了份可以用的代码。
TKJElectronics在github上有两份代码（ https://github.com/TKJElectronics），KalmanFilter和Example-Sketch-for-IMU-including-Kalman-filter，分别是卡尔曼滤波算法，和IMU融合卡尔曼滤波的样例。

代码演示了计算Roll，Pitch角和Yaw角并用卡尔曼过滤算法。
样例里面有四种芯片的演示，我用的是MPU6050，就直接看这个目录了，里面还有MPU6050+HMC5883L磁力计的样式，可惜手头板子当初没有想过算Yaw角旋转要磁力计，也就莫法实现读取Yaw角。加HMC5883L磁力计的那个样例，是读的磁力计的数据来算Yaw轴角度。
MPU6050的重力加速度出来的z轴数据基本不咋变化，仅依靠x和y轴数据肯定不准的，所以这里通过重力加速度算出来Yaw轴的角度无意义。

继续回来说代码。
下载回来的样例代码扩展名是.ino，搞不懂啥，改成.c，一样看，c语言万岁！
首先要先拿到陀螺仪的数据，角速度和重力加速度。
如何读取MPU6050的数据我略过。网上有很多现成的样例，直接拿来用。

/* IMU Data */
float accX, accY, accZ;
float gyroX, gyroY, gyroZ;
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
//tempRaw = (i2cData[6] << 8) | i2cData[7];
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];

i2cData是MPU6050读到的14个字节的数据。
i2cData[6]和[7]是温度不用。
6个变量，样例里是double类型，我改float了，用不到那么高精度，且我是51单片机运算，计算资源极其有限。

acc打头的变量是重力加速度，gyro打头的变量是角速度。
这里角速度和重力加速度容易让人搞晕。
角速度反应的是芯片的运动加速度，静止时是个很小值，芯片运动的时候XYZ三轴值会对应速度变化。
重力加速度跟重力有关，反应芯片的摆放状态，垂直于地心引力。要获取芯片的摆放姿态，只需要重力加速度就够了。

芯片读出来的数据是16位，数据范围是2的16次方65536，10进制整数范围是-32768到32768。
这里要根据MPU6050初始化时，设置的角速度和重力加速度各自的量程换算下来，我设置的角速度是±2000deg/s，重力加速度是2g，分别使用32768除以2和2000就是各自换算单位。

TKJ Electronics样例里面double gyroXrate = gyroX / 131.0; 是用的250deg/s的量程，就是32768/250≈131
btw，2000deg/s是有多快？我感觉一般手机摇一摇这种，250deg/s应该就够吧？
我自己修改的代码中(32768/2)和(32768/2000)是使用的2000deg/s的量程计算单位

accX /= 16384.0f; // (32768 / 2) = 16384.0f
accY /= 16384.0f;
accZ /= 16384.0f;

gyroX /= 16.384f; // (32768 / 2000) = 16.384f
gyroY /= 16.384f;
gyroZ /= 16.384f;

下面是如何根据acc重力加速度计算芯片目前相对地面的两个偏转角度（roll和pitch角）。
上面说过，靠重力加速度无法计算yaw角，所以这里也略过yaw。

// 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
	float roll  = atan2(accY, accZ) * RAD_TO_DEG;
	float pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else    // Eq. 28 and 29
	float roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
	float pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

这个计算的理论公式我自己没去推算（人笨...）。
国外有个学习网站详细描述了各种计算公式可以收藏参考，我看网上很多文章也就是照搬上面的图片和翻译。
数学达人收藏推荐：
http://www.euclideanspace.com/maths/geometry/rotations/euler/index.html
喜欢刨根问底的理论基础的可以拿去学习。

回来继续说代码。

RAD_TO_DEG 是弧度到度的转换单位，定义如下，我也稍调整了下，把即时计算改成了静态值定义

//#define RAD_TO_DEG (360/PI/2) // 弧度转角度的转换率
//#define DEG_TO_RAD (2*PI/360) // 角度转弧度的转换率
#define RAD_TO_DEG 57.295779513082320876798154814105   // 弧度转角度的转换率
#define DEG_TO_RAD 0.01745329251994329576923690768489 // 角度转弧度的转换率

有个RESTRICT_PITCH宏定义，貌似用来约束atan输出的角度？不同的翻转计算方式不一样？
注释里有链接，里面是算法解释，全是矩阵方程，数学大牛可以看看。
欧拉角三维翻转有个方向死锁问题，不过一般应用，平衡小车，四轴飞行器都不会用到翻转90°，我这里应用场景也是不会发生垂直翻转场景，所以这里我是默认定义没去深究。

接下来就是卡尔曼滤波了，卡尔曼滤波一开始要先设置初始角度，原代码如下

kalmanX.setAngle(roll);
kalmanY.setAngle(pitch);

样例代码是cpp，搞了两个滤波对象对应x和y轴，分别初始化设置。
为了在单片机上跑，我给改成了c形式。

typedef struct
{
	float Q_angle;
	float Q_bias;
	float R_measure;
	float angle; // Reset the angle
	float bias; // Reset bias
	float P[2][2];
	// Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
	float rate;
} KalmanFilter_t;

typedef struct
{
	KalmanFilter_t *pKalmanX;
	KalmanFilter_t *pKalmanY;
	float gyroXangle, gyroYangle; // Angle calculate using the gyro only
	float compAngleX, compAngleY; // Calculated angle using a complementary filter
	float kalAngleX, kalAngleY;  // Calculated angle using a Kalman filter
} KalmanFilterSys_t;

// Need set starting angle
static KalmanFilterSys_t *Get_Kalman_Filter(float roll, float pitch)
{
	KalmanFilterSys_t *pSys = (KalmanFilterSys_t *)calloc(1, sizeof(KalmanFilterSys_t));
	pSys->pKalmanX = (KalmanFilter_t *)calloc(1, sizeof(KalmanFilter_t));
	pSys->pKalmanY = (KalmanFilter_t *)calloc(1, sizeof(KalmanFilter_t));

	/* We will set the variables like so, these can also be tuned by the user */
	pSys->pKalmanX->Q_angle = pSys->pKalmanY->Q_angle = 0.001f;
	pSys->pKalmanX->Q_bias = pSys->pKalmanY->Q_bias = 0.003f;
	pSys->pKalmanX->R_measure = pSys->pKalmanY->R_measure = 0.03f;

	pSys->pKalmanX->angle = roll; // Reset the angle
	pSys->pKalmanY->angle = pitch; // Reset bias

	// 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
	pSys->pKalmanX->P[0][0] = 0.0f;
	pSys->pKalmanX->P[0][1] = 0.0f;
	pSys->pKalmanX->P[1][0] = 0.0f;
	pSys->pKalmanX->P[1][1] = 0.0f;

	pSys->pKalmanY->P[0][0] = 0.0f;
	pSys->pKalmanY->P[0][1] = 0.0f;
	pSys->pKalmanY->P[1][0] = 0.0f;
	pSys->pKalmanY->P[1][1] = 0.0f;

	pSys->gyroXangle = roll;
	pSys->gyroYangle = pitch;
	pSys->compAngleX = roll;
	pSys->compAngleY = pitch;

	return pSys;
};

这里创建结构时，附带就把roll和pitch对应的angle初始化了。
结构创建好，直接设置数据计算即可，同样函数我给改成了c语言函数，同时设置roll和pitch。

float dt = 10.0 / 1000;
Kalman_Fileter_SetAngle(m_pKalmanFilter, roll, pitch, gyroX, gyroY, dt);

这里的dt直接影响一阶互补滤波的计算权重，我测试是10毫秒读一次数据，1秒作为拟合周期，dt就是10.0 / 1000毫秒，需要根据自身使用情况调整。TKJ Electronics的样例是用的1微妙，拟合周期也是1秒。
调用时dt取两次读取数据的实际间隔时间。我是用单片机中断读取数据，延迟时间很稳定，所以取固定值10ms。

注意自己的采样频率，如果是125Hz采样频率，dt间隔时间就是8ms，每1秒钟要读125次角度数据。
卡尔曼滤波的参数用的默认值，几个参数具体如何调整，数学大牛可以继续问看度娘，我也没去细看算法理论。
每次采样数据，经过Kalman_Fileter_SetAngle计算后，打印下各个数据结果看看

char text[260];
_snprintf_s(text, _TRUNCATE,
	_T("roll(%.4f %.4f %.4f %.4f) pitch(%.4f %.4f %.4f %.4f)\n"),
	roll,
	m_pKalmanFilter->gyroXangle,
	m_pKalmanFilter->compAngleX,
	m_pKalmanFilter->kalAngleX,
	pitch, 
	m_pKalmanFilter->gyroYangle,
	m_pKalmanFilter->compAngleY,
	m_pKalmanFilter->kalAngleY);
printf(text);

这里打印的结果，
roll和pitch是原始的计算角度，会看到跳变会比较厉害，时大时小。
gyroXangle和gyroYangl经过dt乘积后，会看到数据基本抑制住跳变，但数据会有一点不断的累加漂移。
compAngleX和compAngleY是角速度与重力加速度算出的偏转角，进行一阶互补过滤后的角度，这个两个数据相对就比较稳定了。
kalAngleX和kalAngleY就是卡尔曼过滤算法后的角度数据了。这两个数据就是最稳定的。

这里的一阶互补过滤默认参数0.093和0.07权重占比，卡尔曼过滤也是用的默认参数，手动翻转电路板晃来晃去，目测计算的结果很精准，感觉不需要再调这个权重比例。
网上其他很多卡尔曼滤波代码拿来测试过，但是这个权重则是各种参数，算法看代码也是各种形式五花八门，实际跑出来的结果乱飘，真心不能直接抄…
可惜以前写的一个绘制频谱的软件代码搞丢了，不然可以实时显示对比下这些不同算法的计算结果曲线，对比看那个代码是准确的且平滑效果最佳。但我目测下，感觉就TKJ Electronics网站上这个样例算法最佳。

最后附上整个我自己整合修改后的卡尔曼角度滤波代码。原版代码是C++，我照顾51单片机里运行改成了C代码。

KalmanFilter.h

#pragma once

//#define RAD_TO_DEG (360/PI/2) // 弧度转角度的转换率
//#define DEG_TO_RAD (2*PI/360) // 角度转弧度的转换率
#define RAD_TO_DEG 57.295779513082320876798154814105  // 弧度转角度的转换率
#define DEG_TO_RAD 0.01745329251994329576923690768489 // 角度转弧度的转换率

// Comment out to restrict roll to ±90deg instead -
// please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
#define RESTRICT_PITCH

typedef struct
{
	float Q_angle;
	float Q_bias;
	float R_measure;
	float angle; // Reset the angle
	float bias; // Reset bias
	float P[2][2];
	// Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
	float rate;
} KalmanFilter_t;

typedef struct
{
	KalmanFilter_t *pKalmanX;
	KalmanFilter_t *pKalmanY;
	float gyroXangle, gyroYangle; // Angle calculate using the gyro only
	float compAngleX, compAngleY; // Calculated angle using a complementary filter
	float kalAngleX, kalAngleY;  // Calculated angle using a Kalman filter
} KalmanFilterSys_t;

// Need set starting angle
static KalmanFilterSys_t *Get_Kalman_Filter(float roll, float pitch)
{
	KalmanFilterSys_t *pSys = (KalmanFilterSys_t *)calloc(1, sizeof(KalmanFilterSys_t));
	pSys->pKalmanX = (KalmanFilter_t *)calloc(1, sizeof(KalmanFilter_t));
	pSys->pKalmanY = (KalmanFilter_t *)calloc(1, sizeof(KalmanFilter_t));

	/* We will set the variables like so, these can also be tuned by the user */
	pSys->pKalmanX->Q_angle = pSys->pKalmanY->Q_angle = 0.001f;
	pSys->pKalmanX->Q_bias = pSys->pKalmanY->Q_bias = 0.003f;
	pSys->pKalmanX->R_measure = pSys->pKalmanY->R_measure = 0.03f;

	pSys->pKalmanX->angle = roll; // Reset the angle
	pSys->pKalmanY->angle = pitch; // Reset bias

	// 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
	pSys->pKalmanX->P[0][0] = 0.0f;
	pSys->pKalmanX->P[0][1] = 0.0f;
	pSys->pKalmanX->P[1][0] = 0.0f;
	pSys->pKalmanX->P[1][1] = 0.0f;

	pSys->pKalmanY->P[0][0] = 0.0f;
	pSys->pKalmanY->P[0][1] = 0.0f;
	pSys->pKalmanY->P[1][0] = 0.0f;
	pSys->pKalmanY->P[1][1] = 0.0f;

	pSys->gyroXangle = roll;
	pSys->gyroYangle = pitch;

	pSys->compAngleX = roll;
	pSys->compAngleY = pitch;

	return pSys;
}

// 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
static void Accel_To_Angle(float *p_roll, float *p_pitch, float accX, float accY, float accZ)
{
#ifdef RESTRICT_PITCH // Eq. 25 and 26
	*p_pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
	*p_roll = atan2(accY, accZ) * RAD_TO_DEG;
#else     // Eq. 28 and 29
	*p_pitch = atan2(-accX, accZ) * RAD_TO_DEG;
	*p_roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
#endif
};

// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
static float Kalman_Filter_GetAngle(KalmanFilter_t *pSys, float newAngle, float newRate, float dt)
{
	// 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 */
	pSys->rate = newRate - pSys->bias;
	pSys->angle += dt * pSys->rate;

	// Update estimation error covariance - Project the error covariance ahead
	/* Step 2 */
	pSys->P[0][0] += dt * (dt*pSys->P[1][1] - pSys->P[0][1] - pSys->P[1][0] + pSys->Q_angle);
	pSys->P[0][1] -= dt * pSys->P[1][1];
	pSys->P[1][0] -= dt * pSys->P[1][1];
	pSys->P[1][1] += pSys->Q_bias * dt;

	// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
	// Calculate Kalman gain - Compute the Kalman gain
	/* Step 4 */
	float S = pSys->P[0][0] + pSys->R_measure; // Estimate error

	/* Step 5 */
	float K[2]; // Kalman gain - This is a 2x1 vector
	K[0] = pSys->P[0][0] / S;
	K[1] = pSys->P[1][0] / S;

	// Calculate angle and bias - Update estimate with measurement zk (newAngle)
	/* Step 3 */
	float y = newAngle - pSys->angle; // Angle difference

	/* Step 6 */
	pSys->angle += K[0] * y;
	pSys->bias += K[1] * y;

	// Calculate estimation error covariance - Update the error covariance
	/* Step 7 */
	float P00_temp = pSys->P[0][0];
	float P01_temp = pSys->P[0][1];

	pSys->P[0][0] -= K[0] * P00_temp;
	pSys->P[0][1] -= K[0] * P01_temp;
	pSys->P[1][0] -= K[1] * P00_temp;
	pSys->P[1][1] -= K[1] * P01_temp;

	return pSys->angle;
};

static void Kalman_Fileter_SetAngle(KalmanFilterSys_t *pSys, float roll, float pitch,
	float gyroXrate, float gyroYrate, float dt)
{
#ifdef RESTRICT_PITCH
	// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
	if ((roll < -90 && pSys->kalAngleX > 90) || (roll > 90 && pSys->kalAngleX < -90)) {
		pSys->pKalmanX->angle = roll;
		pSys->compAngleX = roll;
		pSys->kalAngleX = roll;
		pSys->gyroXangle = roll;
	}
	else
		pSys->kalAngleX = Kalman_Filter_GetAngle(pSys->pKalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

	if (abs(pSys->kalAngleX) > 90)
		gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
	pSys->kalAngleY = Kalman_Filter_GetAngle(pSys->pKalmanY, pitch, gyroYrate, dt);
#else
	// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
	if ((pitch < -90 && pSys->kalAngleY > 90) || (pitch > 90 && pSys->kalAngleY < -90)) {
		pSys->pKalmanY->angle = pitch;
		pSys->compAngleY = pitch;
		pSys->kalAngleY = pitch;
		pSys->gyroYangle = pitch;
	}
	else
		pSys->kalAngleY = Kalman_Filter_GetAngle(pSys->pKalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

	if (abs(pSys->kalAngleY) > 90)
		gyroXrate = -(gyroXrate); // Invert rate, so it fits the restriced accelerometer reading
	pSys->kalAngleX = Kalman_Filter_GetAngle(pSys->pKalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif

	pSys->gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
	pSys->gyroYangle += gyroYrate * dt;

	//pSys->gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
	//pSys->gyroYangle += kalmanY.getRate() * dt;

	// Calculate the angle using a Complimentary filter
	pSys->compAngleX = 0.93 * (pSys->compAngleX + gyroXrate * dt) + 0.07 * roll;
	pSys->compAngleY = 0.93 * (pSys->compAngleY + gyroYrate * dt) + 0.07 * pitch;

	// Reset the gyro angle when it has drifted too much
	if (pSys->gyroXangle < -180 || pSys->gyroXangle > 180)
		pSys->gyroXangle = pSys->kalAngleX;
	if (pSys->gyroYangle < -180 || pSys->gyroYangle > 180)
		pSys->gyroYangle = pSys->kalAngleY;
};

static void Delete_Kalman_Filter(KalmanFilterSys_t **ppSys)
{
	free((*ppSys)->pKalmanX);
	free((*ppSys)->pKalmanX);
	free((*ppSys));
	*ppSys = 0;
};