卡爾曼濾波中關鍵參數的調整

06-27

卡爾曼濾波中關鍵參數的調整

寫在前面

對於卡爾曼濾波，大多數人僅限於會用，很少有人能透徹的理解，而關於卡爾曼濾波中那幾個關鍵參數的調整更是一頭霧水。自從寫完「深度解析卡爾曼濾波在IMU中的使用」一文，很多朋友諮詢卡爾曼濾波的參數到底怎麼調整，工作之餘總結一下，由於水平有限，錯誤之處在所難免，歡迎交流指正。

本篇還是以飛控與移動機器人中常用的IMU晶元MPU6050為例，詳細講解卡爾曼濾波中那幾個關鍵參數的調整。

源碼參考 TKJElectronics/KalmanFilter

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 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 secondsfloat Kalman::getAngle(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 */ 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 K[0] = P[0][0] / S; K[1] = P[1][0] / S; // 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 anglefloat 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; };

關鍵參數

使用卡爾曼濾波時需要讀取系統起始角度，就是通過讀取加速度計的值，然後根據反正切計算出的系統傾角。

卡爾曼濾波中的幾個關鍵參數包括dt、Q_angle、Q_bias、R_measure、P。

前綴Q表示預測（過程）雜訊方差;

前綴R表示測量（觀測）雜訊方差;

dt表示採樣時間,它的值要特別精確；

Q_angle表示加速度計過程雜訊協方差;

Q_bias表示陀螺儀過程雜訊協方差;

R_measure表示測量雜訊協方差;

P表示誤差協方差矩陣;

下面簡單的看一下當改變參數時系統的輸出變化。直接通過讀取加速度計的值計算出的角度，我們稱為accRaw圖中紅線表示;通過陀螺儀積分計算的角度，我們稱為gyroDt圖中白線表示，黃線和白線貼的比較緊，需要放大才能看出;卡爾曼濾波計算的角度，我們稱為kalman圖中黃線表示;

當dt過小時，黃線雖然能跟隨紅線，但是非常滯後:

dt=0.001

當dt過大時，黃線能跟隨紅線但明顯過沖:

dt=0.010

dt=0.005時黃線可以很好的跟隨紅線:

當Q_angle越大時，黃線跟隨紅線趨勢越緊密，濾波之後的雜訊也越大，下圖的雜訊明顯增大

Q_angle=1.0

縮小看一下

Q_angle=0.01黃線跟隨紅線趨勢變緩，濾波效果改善明顯

Q_bias越小白線跟隨黃線越緊密

Q_bias=0.3

Q_bias=0.003

R_measure值的改變主要是影響卡爾曼的收斂速度，由於測試設備簡陋，測試時並沒有得到較好的波形輸出效果的比較。

參數解析

Q值為過程雜訊，越小系統越容易收斂，我們對模型預測的值信任度越高；但是太小則容易發散，如果Q為零，那麼我們只相信預測值；Q值越大我們對於預測的信任度就越低，而對測量值的信任度就變高；如果Q值無窮大，那麼我們只信任測量值；R值為測量雜訊，太小太大都不一定合適。R太大，卡爾曼濾波響應會變慢，因為它對新測量的值的信任度降低；越小系統收斂越快，但過小則容易出現震蕩；測試時可以保持陀螺儀不動，記錄一段時間內陀螺儀的輸出數據，這個數據近似正態分布，按3σ原則，取正態分布的(3σ)^2作為R的初始化值。

測試時可以先將Q從小往大調整，將R從大往小調整；先固定一個值去調整另外一個值，看收斂速度與波形輸出。

系統中還有一個關鍵值P，它是誤差協方差初始值，表示我們對當前預測狀態的信任度，它越小說明我們越相信當前預測狀態；它的值決定了初始收斂速度，一般開始設一個較小的值以便於獲取較快的收斂速度。隨著卡爾曼濾波的迭代，P的值會不斷的改變，當系統進入穩態之後P值會收斂成一個最小的估計方差矩陣，這個時候的卡爾曼增益也是最優的，所以這個值只是影響初始收斂速度。

結語

在實際工程中大家可能都用過PID，演算法很簡單，但是要想調好PID，實現一定的精度和速度卻不是那麼容易。這個時候就需要你對系統有很深的理解。卡爾曼濾波的核心代碼也就那麼幾行，但參數的整定卻不是那麼容易。從反饋角度看，卡爾曼濾波也是一個線性時變反饋系統，逐漸收斂於一個線性時不變系統。最大似然估計方面、反饋系統方面、殘差方面以及動態特性方面等都是分析卡爾曼濾波參數的關鍵之處。在很多工業控制領域，比如飛船的控制、衛星的控制等都有專門的負責調參的工程師甚至是一個團隊，可見卡爾曼濾波參數整定的重要程度。從某種意義上說，調參更像是一門藝術。

針對於同一套工程模型，這些參數理論上應該可以統一，但是不同環境下系統的過程雜訊以及測量雜訊有可能不同，因此還是需要實際測試，以便於找到參數的最優值。

參考:

xiahouzuoxin/kalman_filter

TKJElectronics/KalmanFilter

[Fundamentals of Kalman Filtering A Practical Approach]

Kalman filter