如何整定卡爾曼濾波的Q、R參數(shù)？

卓老師，我想問一個(gè)關(guān)于卡爾曼濾波的問題，希望您能解答一下。之前我用的互補(bǔ)濾波效果也還好，但在用卡爾曼濾波的時(shí)候出現(xiàn)了一些問題：就是如何整定卡爾曼濾波的Q、R這兩個(gè)參數(shù)，這兩個(gè)參數(shù)分別是角度數(shù)據(jù)置信度與角速度數(shù)據(jù)置信度。我看別人用的這兩個(gè)參數(shù)都非常小，比如別人Q都是零點(diǎn)零零幾，而我用的時(shí)候發(fā)現(xiàn)Q零點(diǎn)幾跟隨效果很差，我把Q調(diào)到1跟隨效果才差不多。但是Q和R不都是協(xié)方差嗎，它們可以取到1及以上的值嗎？即Q和R有沒有取值范圍，以及這兩個(gè)參數(shù)整定方法是什么？還是我理解有問題，希望卓大或者車友可以解答一下。

回復(fù)：在智能車競(jìng)賽中，Kalman濾波算法常常被用來(lái)計(jì)算直立車模傾角和轉(zhuǎn)動(dòng)角速度。算法利用陀螺儀給出的轉(zhuǎn)速度量和加速度傳感器給出的傾角觀測(cè)量完成計(jì)算。除了Kalman濾波器算法之外，還可以使用互補(bǔ)濾波器進(jìn)行幫助計(jì)算。

如果不考慮系統(tǒng)狀態(tài)的時(shí)變性和系統(tǒng)噪聲的時(shí)變性，上述兩種方法本質(zhì)上是相同的。

下圖顯示了對(duì)于被測(cè)物體位置使用Kalman濾波算法來(lái)估計(jì)物體實(shí)際位置的示意圖。

通常情況下所使用的Kalman濾波器是離散時(shí)間系統(tǒng)形式的。我們真正想得到的物理量表示成系統(tǒng)狀態(tài)中的某些分量。對(duì)于系統(tǒng)狀態(tài)的估計(jì)（濾波結(jié)果）所使用的信息來(lái)源于兩個(gè)方面，一個(gè)是對(duì)系統(tǒng)狀態(tài)演變模型的了解，包括系統(tǒng)轉(zhuǎn)移矩陣和輸入控制矩陣，輸入孔質(zhì)量等，另一方面來(lái)自于對(duì)系統(tǒng)狀態(tài)的觀測(cè)量。

但這兩方面的信息都會(huì)有某種不確定性。通常使用系統(tǒng)噪聲向量（W）和觀測(cè)噪聲向量（V）來(lái)表示。兩個(gè)噪聲大小分別使用它們各自的協(xié)方差矩陣來(lái)表示。系統(tǒng)噪聲協(xié)方差矩陣使用Q，觀測(cè)噪聲的協(xié)方差矩陣使用R。

下面是系統(tǒng)方程和觀測(cè)方程表達(dá)式。雖然在表達(dá)式中包括了系統(tǒng)噪聲w和轉(zhuǎn)測(cè)噪聲v，但在計(jì)算系統(tǒng)狀態(tài)轉(zhuǎn)移和系統(tǒng)觀測(cè)量的時(shí)候并不考慮這兩個(gè)噪聲的影響。

兩個(gè)噪聲的影響只是在卡爾曼濾波器離散迭代算法過程中使用到了兩個(gè)噪聲的協(xié)方差矩陣Q和R。分別用于計(jì)算系統(tǒng)狀態(tài)估計(jì)誤差的協(xié)方差矩陣P和卡爾曼濾波器增益K的大小。

下面是傳統(tǒng)線性卡爾曼濾波器的五個(gè)基本迭代公式。

從上面公式來(lái)看，真正所要濾波得到的結(jié)果來(lái)自于公式（4）中的系統(tǒng)狀態(tài)估計(jì)值x的某些分量，公式（4）的結(jié)果是由公式（1）所得到的狀態(tài)預(yù)測(cè)值和來(lái)自觀測(cè)量y計(jì)算得到的。其中卡爾曼濾波器增益K是在狀態(tài)預(yù)測(cè)值和觀測(cè)誤差值之間做了一個(gè)折中。

如果K很小，比如等于0，則濾波結(jié)果更加接近由系統(tǒng)狀態(tài)估計(jì)值給出的遞歸結(jié)果；如果K很大，比如等于1，則濾波結(jié)果更加接近于觀測(cè)值所反算出來(lái)的狀態(tài)變量。

K值是由公式（3）計(jì)算獲得，其中的P值是通過公式（2）（5）進(jìn)行計(jì)算，反映了Q的大小，因此K值的大小可以由如下形式示意：

因此，K值至于Q,R的比值有關(guān)系，而與Q,R的絕對(duì)值沒有關(guān)系。所以，在不同算法中，R, Q的取值根據(jù)反應(yīng)的不同量綱，可以有很大的變化，但它們的比值會(huì)決定了濾波值應(yīng)該更多來(lái)自于系統(tǒng)模型演化的信息，還是來(lái)自于觀察信號(hào)信息。

在智能車競(jìng)賽中，使用Kalman濾波器將慣性傳感器所得到的車體陀螺儀所反映的角速度和和加速度傳感器所獲得的傾斜角信息進(jìn)行融合，獲得直立車模傾角和轉(zhuǎn)動(dòng)角速度。

此時(shí)，往往將系統(tǒng)狀態(tài)x設(shè)定為車模需要觀察的角度。系統(tǒng)輸入量u為測(cè)量所得到的角速度；系統(tǒng)觀察值設(shè)定為有加速度傳感器給出的傾角。

系統(tǒng)模型噪聲w應(yīng)該反映出陀螺儀測(cè)定角速度的隨機(jī)誤差和隨著時(shí)間漂移的系統(tǒng)誤差兩部分。系統(tǒng)觀測(cè)噪聲v應(yīng)該反映了加速度計(jì)輸出量中在計(jì)算角度的近似誤差和由于車模運(yùn)動(dòng)所產(chǎn)生的干擾噪聲。

如果Q大R小，造成K增加，則濾波結(jié)果中就會(huì)存在較大的由于車模運(yùn)動(dòng)所產(chǎn)生的噪聲，俗稱跟蹤不好；如果Q小R大，造成K減小，則濾波結(jié)果會(huì)出現(xiàn)兩種問題，第一就是從處置值收斂到正確值的過程較慢，需要等一個(gè)比較長(zhǎng)的穩(wěn)定時(shí)間。另一方面就是會(huì)受到陀螺儀本身零點(diǎn)漂移，產(chǎn)生比較大的輸出零點(diǎn)誤差。

最終這兩個(gè)參數(shù)的大小可以根據(jù)所選擇的器件的實(shí)際性能（噪聲，漂移等）通過實(shí)驗(yàn)觀察的方式獲得一個(gè)比較好的相對(duì)值。

編輯：黃飛