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;
};