直立平衡車的姿態測量卡爾曼濾波算法原理與應用（附代碼及調試截圖）

鄙人最近測量調試直立平衡車的姿態角度時，用到了卡爾曼濾波算法。本着知其然還需知其所以然的學習精神，在網上閱覽了很多關於濾波原理及算法應用的文章，加上自己的調試經驗，有了一點小小的心得，現在分享給大家。疑惑不當之處，歡迎討論批評。首發於CSDN：http://blog.csdn.net/qq_32666555。轉載請註明作者及出處，謝謝！

首先介紹我的方案背景。我用了慣性測量組合元件（Inertial Measurement Uint，IMU），兩軸ENC-03MB陀螺儀及兩軸MMA7361加速度計，其中一軸陀螺儀應用在方向測量上，所以不在討論範圍內，測量零點等拓展內容也不在本次討論範圍內。而另一軸陀螺儀測得的角速度作爲卡爾曼濾波函數的一個輸入變量，兩軸加速度計測得的角度之差乘上角度量比例作爲卡爾曼濾波函數的另一輸入變量。

下面，我們以“是什麼，爲何用，怎麼用”的順序來介紹直立平衡車的姿態測量濾波算法原理與應用。因此，先介紹濾波算法的原理。在介紹卡爾曼濾波之前，我們可以先以互補濾波作爲敲門石。而介紹互補濾波，就不得不提大名鼎鼎的MIT經典PPT《The Balance Filter》。鄙人親自翻譯了這個牛文，在此附上鍊接：

《The Balance Filter》互補濾波器--MIT著名牛文翻譯（上）

《The Balance Filter》互補濾波器--MIT著名牛文翻譯（下）

簡單瀏覽這篇文章，在初步瞭解了兩種傳感器工作原理、權重分配、誤差漂移、互補濾波等後，我們理解卡爾曼濾波會簡單些。

卡爾曼濾波，於我理解，是只需要k-1時刻兩參數的協方差估算出k時刻最優解，並算出k時刻協方差進行遞歸，算出k+1，k+2…時刻的最優解。因此它是個快速而智慧的算法。知乎上有很多通俗科學的解釋，我貼出一個問題鏈接，裏面那些高贊同回答個人覺得都不錯：https://www.zhihu.com/question/23971601

那爲何用卡爾曼濾波算法呢？

首先解釋爲什麼用2種傳感器。陀螺儀具精確但有零點漂移特性，其測量誤差會隨着時間的累加而不斷的累積，從而影響測量精度。因此，短時間測量應信任陀螺儀。加速度計一直受到平衡車振動的影響，混疊額外的高頻振動量干擾，但是漂移小。因此，長時間測量應信任加速度計。所以單一的傳感器測量難以得到精確的姿態角度。需採用多傳感器信號融合的方法，來獲得準確的姿態角度量。

而卡爾曼濾波，可以設置分配信任權重，解決陀螺儀零漂、濾除加速度計高頻振動，遞歸自學習，濾波平滑、跟隨快。雖然算法較複雜，佔用較多線程時間，但是我們選用的freescale K60處理器完全可以承擔這樣的計算，所以決定使用該濾波算法。

最後，就是卡爾曼濾波算法的應用了。Show you the code.

float Kal_Gyro;
float gyro;
int16_t mmax, mmaz;
float angle, angleSpeed;
float Angle_Kalman;
int16_t Pre_Angle_Kalman;
float angleSpeedIntegral;

float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

/**
 * @brief  卡爾曼濾波, 輸入帶有噪聲的參數組, 返回可靠的參數組, 此處用於獲得變化較穩定的角度值
 * @param[in]  Accel 由加速度計得到的角度參量
 * \param[in]  Gyro 由陀螺儀得到的角速度參量
 * @retval 穩定可靠的角度值, 用於直立控制
 */

static float KalmanFilter(float Accel,float Gyro)		
{
    static float Angle = 0, Gyro_y = 0;
	Angle+=(Gyro - Q_bias) * dt; //先驗估計

	Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協方差的微分

	Pdot[1] = -PP[1][1];
	Pdot[2] = -PP[1][1];
	Pdot[3]= Q_gyro;
	
	PP[0][0] += Pdot[0] * dt;   // Pk-先驗估計誤差協方差微分的積分
	PP[0][1] += Pdot[1] * dt;   // =先驗估計誤差協方差
	PP[1][0] += Pdot[2] * dt;
	PP[1][1] += Pdot[3] * dt;
		
	Angle_err = Accel - Angle;	//zk-先驗估計
	
	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];
	
	E = R_angle + C_0 * PCt_0;
	
	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;
	
	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];

	PP[0][0] -= K_0 * t_0;		 //後驗估計誤差協方差
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;
		
	Angle	+= K_0 * Angle_err;	 //後驗估計
	Q_bias	+= K_1 * Angle_err;	 //後驗估計
	Gyro_y   = Gyro - Q_bias;	 //輸出值(後驗估計)的微分=角速度
	return Angle;
}

因爲我是按The balance filter裏的座標系修改的，所以

@param[in] angle_kal 由加速度計得到的角度參量，是兩軸加速度計測得的角度之差乘上角度量比例再乘初始所佔權重；

\param[in] angle_speed_kal 由陀螺儀得到的角速度參量，是陀螺儀測得的角速度的相反值；

@retval 穩定可靠的角度值, 用於直立控制，是送入角度調節中的當前量。

而卡爾曼濾波的5個方程

結合其餘定義式，寫成矩陣形式

對應卡爾曼濾波第一個方程
X(k|k-1)=A X(k-1|k-1)+B U(k) ………(1)//先驗估計

（2）預測方差陣的預測值，噪聲就是數據的方差值

#define Q_angle 0.001 //角度噪聲
#define Q_gyro 0.0005//漂移噪聲

Pdot[0] = Q_angle - Pk[0][1] - Pk[1][0];

Pdot[1] = -Pk[1][1];

Pdot[2] = -Pk[1][1];

Pdot[3] = Q_gyro;

Pk[0][0] += Pdot[0] * Kalman_Sample_Time;

Pk[0][1] += Pdot[1] * Kalman_Sample_Time;

Pk[1][0] += Pdot[2] * Kalman_Sample_Time;

Pk[1][1] += Pdot[3] * Kalman_Sample_Time;

對應卡爾曼濾波第二個方程
P(k|k-1)=A P(k-1|k-1) A’+Q ………(2)//協方差矩陣的預測

（3）計算卡爾曼增益K_0，K_1
#define R_angle 0.05 //角度測量噪聲值

angle_err = angle_kal - angle_kalman;

PCt_0 = C_0 * Pk[0][0];

PCt_1 = C_0 * Pk[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

對應卡爾曼濾波第三個方程

Kg(k)= P(k|k-1) H’ / (HP(k|k-1) H’ + R) ………(3)//計算卡爾曼增益

（4）對矩陣Pk進行更新
t_0 = PCt_0;

t_1 = C_0 * Pk[0][1];

Pk[0][0] -= K_0 * t_0;

Pk[0][1] -= K_0 * t_1;

Pk[1][0] -= K_1 * t_0;

Pk[1][1] -= K_1 * t_1;

對應卡爾曼濾波第五個方程

P(k|k)=（I-Kg(k) H）P(k|k-1)………(5)//更新協方差陣

（5）通過卡爾曼增益修正當前的角度、陀螺儀零點，算出當前角速度

angle_kalman += K_0 * angle_err;

Kal_Gyro_Zero += K_1 * angle_err;

Kal_Gyro = angle_speed_kal - Kal_Gyro_Zero;

對應卡爾曼濾波第四個方程

X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ………(4)//通過卡爾曼增益進行修正

最後，附上本人的測試曲線圖及實物效果圖，謝謝大家！