預測-五階容積卡爾曼濾波方法

趙祥丹，王彪，王志勝，楊忠

南京航空航天大學 自動化學院，南京 210016

自從卡爾曼將“狀態(tài)空間法”引入到高斯濾波器中提出卡爾曼濾波器（KF）后，以遞推形式呈現(xiàn)的高斯框架下的濾波方法相繼而出。為適用于非線性系統(tǒng)，有學者提出擴展卡爾曼濾波器（EKF）將卡爾曼濾波器中線性系統(tǒng)條件轉化為更一般的非線性系統(tǒng)，但EKF 只適用于弱非線性系統(tǒng)[1］。對于強非線性系統(tǒng)，常常采用確定性采樣逼近高斯概率密度的Sigma 點卡爾曼濾波系列方法，其典型代表為無跡卡爾曼濾波（UKF）。2009 年，加 拿 大 學 者Arasaratnam 和Haykin 提 出了一種容積卡爾曼濾波器（CKF）[2］，該濾波方法以Sigma 點濾波為基礎，采用球面-徑向積分準則對高斯積分進行數值逼近并進行遞推狀態(tài)估計。相比于無跡卡爾曼濾波，容積卡爾曼濾波具有嚴格的數學推導，更加嚴謹且濾波精度更高。文獻[3］在CKF 基礎上，以任意階全對稱球面插值準則處理球面積分；以矩匹配法處理徑向積分，提出了高階容積卡爾曼濾波（HCKF）方法。文獻[4］在文獻[3］基礎上，采用球面單形準則處理球面積分，提出了五階容積卡爾曼濾波（5thCKF）方法。但上述方法均采用矩匹配法對徑向積分進行數值計算，會導致徑向積分的代數精度有所丟失。文獻[5-8］對七階容積卡爾曼濾波（7thCKF）進行了深入研究，其相較于五階卡爾曼濾波精度有所提升，但所需采樣點數過多，計算及其復雜，龐大的計算量使得算法很難應用于實際工程中。

高斯條件下的濾波器研究已經相對成熟，但實際系統(tǒng)仍然會存在有非高斯條件的情況，比如系統(tǒng)建模產生的模型誤差等。文獻[9］針對系統(tǒng)模型誤差，提出一種預測濾波（PF）方法，該方法基于實際觀測值和預測觀測值求解模型誤差得到新模型，進而求解狀態(tài)方程組得到狀態(tài)估計值，實際應用于非高斯過程噪聲系統(tǒng)，但該方法求解狀態(tài)方程組的過程過于復雜，在高維系統(tǒng)中難以實現(xiàn)。

為使濾波方法能夠同時應用于強非線性系統(tǒng)和非高斯過程噪聲系統(tǒng)，本文將高階容積卡爾曼濾波和預測濾波方法相結合，提出一種預測-五階容積卡爾曼濾波（P5thCKF）方法。采用球面單形準則處理球面積分，廣義高斯-拉蓋爾求積準則替代矩匹配法處理徑向積分，以卡爾曼濾波遞推求解替代預測濾波方程組求解，進一步提高濾波精度，且避免了精度損失和解方程組困難等問題。

1 高斯濾波框架

容積卡爾曼濾波是高斯濾波框架下的一種濾波方法，而高斯濾波又通過貝葉斯濾波發(fā)展而來。貝葉斯濾波的核心思想是通過先驗信息和觀測信息得到狀態(tài)后驗概率密度函數，進而通過后驗概率分布求其期望、方差得到狀態(tài)估計量。

假設系統(tǒng)方程為

式中：f (·)為狀態(tài)轉移函數；G 為過程噪聲系數矩陣；h(·)為觀測函數；x 為狀態(tài)量；ω 為過程噪聲；z為觀測量；v 為觀測噪聲。由文獻[10］可得貝葉斯濾波框架為

假設系統(tǒng)（1）各隨機變量服從高斯分布，結合式（2）可得高斯濾波框架[10］。

1） 預測階段

式中：xk|k-1、xk|k代表狀態(tài)預測值和狀態(tài)估計值；Pk|k-1、Pk|k代表狀態(tài)預測方差陣和狀態(tài)估計方差陣；zk|k代表預測觀測值；zk代表實際觀測值。從式（4）～式（9）可知，高斯濾波的關鍵問題是如何求解高斯積分：

相關證明可見文獻[2］。

2 五階容積卡爾曼濾波

容積卡爾曼濾波的核心思想是以高斯濾波框架為主體，對高斯積分（12）進行球坐標系變換得到球面-徑向積分，進而用不同方法分別對球面積分和徑向積分進行數值計算。為推導方便，式（12）簡記為

對式（13）進行球坐標系變換得球面-徑向積分，令x=ry （yTy=1，r ∈(0，∞)），式（13）等價為

式中：Un表示n 維超球體表面；dσ(y)表示面積微元，dr 表示半徑微元。將式（14）拆分為球面積分和徑向積分。

球面積分：

徑向積分：

下文將分別對球面積分和徑向積分進行數值積分處理。

2.1 球面積分處理

對于球面積分，采用正則單形變換群法，利用正則單形頂點和各頂點中點在球面上的投影作為容積點，以加權和形式對球面積分進行數值計算。其中正則單形頂點正容積點為[4］

取各正頂點中點并投影到球面上，得正容積中點[11］

式中：i ＜l (l=1，2，…，n+1)為正頂點數；n 為狀態(tài)變量維數；k=n(n+1)/2 為正中點數。取正容積點對應負值作為負容積點正負容積 點 合 集 記 為ai、bk，此 時i=2(n+1)，k=n(n+1)。則式（15）的五階球面單形積分準則為[11］

2.2 徑向積分處理

對于徑向積分，采用廣義高斯-拉蓋爾積分準則對其進行數值計算，首先對式（16）作積分變量替換r= t 使其滿足廣義高斯-拉蓋爾積分形式，則式（16）等價為

采用廣義拉蓋爾正交多項式對高斯函數進行擬合，羅德里格形式的廣義拉蓋爾正交多項式表示為

依據拉格朗日插值多項式和帶權函數[12］可推得廣義高斯-拉蓋爾求積權重：

定義1[13］若求積公式

定理1[14］若求積公式

式中：ωj、xj均為未知數，恰當選擇其值可使代數精度達2m-1 次。

性 質1[14］插 值 公 式（32）具 有2m-1 次 的充分必要條件是高斯求積節(jié)點xj為區(qū)間[a，b]上帶權函數ρ(x)的m 次正交多項式的零點。

由定理1 可知，五次代數精度需高斯點數為3。由性質1 可知，高斯求積節(jié)點為正交多項式的根。由上述條件結合式（30）可得五階徑向積分準則為

式中：tj為式（27）的根。

上文中已得到五階球面單形積分準則和五階徑向積分準則，結合式（12）、式（19）、式（33）可得處理式（14）的五階球面單形-徑向積分準則：

1）基于PLC的給排水控制系統(tǒng)數據采集，可通過對模擬量數據和數字量數據的科學采集分析，對該系統(tǒng)運行中的控制效果進行科學評估。在此期間，模擬量采集的數據主要有：水倉水位、電機工作電流、水泵軸溫度、電機溫度和排水管流量。數字量采集的數據主要有：水泵高壓啟動柜真空斷路器和電抗器柜真空接觸器的狀態(tài)、電動閥的工作狀態(tài)與啟閉位置、真空泵工作狀態(tài)、電磁閥狀態(tài)、水泵吸水管真空度及水泵出水口壓力。通過對這些不同數據的采集，在檢測方式的配合作用下，有利于保持給排水控制系統(tǒng)運行中良好的控制效果。

采用準則（19）和準則（34）的濾波算法即為五階容積卡爾曼濾波。在濾波系統(tǒng)的過程噪聲與觀測噪聲均服從高斯分布的環(huán)境下，該算法擁有著高濾波精度，但大多數濾波系統(tǒng)其系統(tǒng)模型均含有如模型誤差等非高斯隨機變量參與過程噪聲影響著系統(tǒng)，所以若想使五階容積卡爾曼濾波高效應用，則必須要處理這些非高斯過程噪聲，下文要講述的預測濾波就是一種很好的處理方法。

3 預測濾波

預測濾波是基于現(xiàn)有模型與新觀測值，利用“協(xié)方差約束”條件與代價函數極小值原理對現(xiàn)有模型的模型誤差進行矯正得到新的模型，對新模型進行狀態(tài)方程求解得到狀態(tài)估計值。

3.1 “協(xié)方差約束”條件和代價函數

考慮系統(tǒng)數學模型如下：

式中：x(t)為狀態(tài)變量；d(t)為模型誤差，即為過程噪聲；G 為過程噪聲系統(tǒng)矩陣；z(t)為觀測量；v(t)為高斯觀測噪聲，假設其均值為0、方差為R。設z?(t) 為 預 測 觀 測 量，則z(t)-z?(t) 為 觀 測殘差。

定義2[15］若系統(tǒng)觀測殘差方差陣與觀測誤差方差陣幾乎一致，那么認為系統(tǒng)滿足“協(xié)方差約束”條件，即

從式（36）～式（39）可知，若系統(tǒng)滿足“協(xié)方差約束”條件，則狀態(tài)預測值與狀態(tài)真值會幾乎一致。但若系統(tǒng)觀測殘差方差陣與觀測誤差方差陣不一致，說明系統(tǒng)出現(xiàn)模型誤差等過程噪聲，需要對其進行調整使得系統(tǒng)滿足“協(xié)方差約束”條件，此為預測濾波的核心要素。模型誤差調整量需構造一種代價函數求得。設代價函數為[16］

代價函數呈現(xiàn)為2 個懲罰項之和，其中第1 項為測量殘差項，第2 項為模型誤差校正項，代價函數跟隨測量殘差、模型誤差和加權矩陣的變化而變化。當代價函數處于極小值時，模型誤差調整量有最優(yōu)解[15］，下文將給出相關推導過程。

3.2 權值矩陣K 討論

代價函數受d(t)、R、K 和測量殘差的影響，而測量殘差受d(t)的影響，d(t)又隨K 的變化而變化。假設系統(tǒng)最初模型誤差相對較大，即最初模型不滿足“協(xié)方差約束”條件。當K 增大時，代價函數中模型誤差校正項較大，此時應減小d(t)。當d(t)減小時，系統(tǒng)趨于原始模型，由于模型誤差的原因使得測量殘差相對較大，系統(tǒng)不滿足“協(xié)方差約束”條件。當K 減小時，測量殘差項相對較大，觀測殘差較大，此時應增大d(t)以減小觀測殘差。但若K 減得很小，即d(t)增得很大，會導致測量殘差趨向于0，即預測觀測值近似等于實際觀測值，此時系統(tǒng)仍不滿足“協(xié)方差約束”條件。所以K 過大或過小都會對濾波系統(tǒng)不利，合理調節(jié)好K 的取值是預測濾波的關鍵。

3.3 預測濾波原理推導

求取代價函數（40）的極小值，即梯度向量為0

對式（43）截取出現(xiàn)一次d(t)為止：

結合式（42）、式（44）可得代價函數J[d(t)]對d(t)導數：

結合式（41）、式（46）可得模型誤差d(t)：

將式（48）所得模型誤差代入系統(tǒng)（35）中形成新的系統(tǒng)模型，對新模型的狀態(tài)方程進行求解即可得到狀態(tài)估計值。

預測濾波雖然在處理非高斯過程噪聲系統(tǒng)中有著很好的表現(xiàn)，但其最終需求解狀態(tài)方程組來得到狀態(tài)估計值，這導致其在計算上有一定的復雜度，甚至在高維系統(tǒng)中難以求解，若采用五階容積卡爾曼濾波方法替代上述求解過程，將2 種濾波算法融合，生成的新算法將以遞推形式進行狀態(tài)估計，從而解決求解問題。

4 預測-五階容積卡爾曼濾波

預測-五階容積卡爾曼濾波其核心思想為通過預測濾波方法更新模型誤差（過程噪聲）建立新的模型，將新模型代入五階容積卡爾曼濾波器中進行遞推狀態(tài)估計，其算法具體步驟如下：

1） 初始化階段

初始化參數：x0，K，d0，R，P0。

2） 模型更新階段

該階段主要處理非高斯過程噪聲對系統(tǒng)的影響，通過讀取實際觀測數據計算出模型誤差調整量，對系統(tǒng)過程噪聲及其方差陣D 進行調整，進而得到新的系統(tǒng)模型，新模型將滿足“協(xié)方差約束”條件。

舊模型：

① 讀取實際觀測值zk，并計算預測觀測值zk|k-1。

② 求解模型誤差調整量（48）和模型誤差方差陣Dk|k-1，即

注1為相鄰n 個時刻中誤差調整量d 的均值。

③ 建立新模型：

以下階段主要處理強非線性濾波問題，采用五階容積卡爾曼濾波，引入滿足“協(xié)方差約束”條件的新模型進行狀態(tài)遞推估計。

3） 預測更新階段

① 對Pk-1|k-1進行Cholesky 分解得Sk-1|k-1

③ 通過狀態(tài)方程傳播狀態(tài)向量容積點：

④ 一步預測狀態(tài)值和狀態(tài)預測誤差方差陣：

4） 測量更新階段

① 對Pk|k-1進行Cholesky 分解成Sk|k-1

注2文獻[17］提出了一種矩陣對角化的改進方法，采用該方法可使得方差陣不需要滿足Cholesky 分解方法的正定矩陣條件，通過矩陣特征值、特征向量即可構造出滿足式（52）和式（57）的矩陣S，此方法很巧妙地避免了系統(tǒng)濾波過程中因矩陣非正定而導致的濾波中斷問題。

② 設定觀測容積點：

③ 通過觀測方程傳播觀測向量容積點：

④ 一步預測觀測值：

以上為預測-五階容積卡爾曼濾波算法（P5thCKF）的全部步驟，實際程序流程如圖1所示。

圖1 預測-五階容積卡爾曼濾波程序流程圖Fig.1 P5thCKF flow chart

5 算法仿真驗證

本文通過2 個仿真實驗對算法進行驗證。實驗1 采用能代表一般性質的非線性系統(tǒng)驗證P5thCKF 算法在強非線性、非高斯過程噪聲（非線性模型誤差）系統(tǒng)中的可行性；實驗2 采用姿態(tài)角濾波對帶有噪聲的飛行器姿態(tài)進行濾波，以驗證P5thCKF 算法應用于工程實踐的可能性。

5.1 實驗1

實驗1 主要驗證P5thCKF 算法應用在強非線性、非高斯過程噪聲系統(tǒng)中的可行性，并與CKF、5thCKF 這2 種濾波算法進行對比分析。參考文獻[18-19］，設計采用強非線性系統(tǒng)，方程表示如下：

下面列舉P5thCKF 算法中，該系統(tǒng)狀態(tài)空間表示為

式中：d 表示誤差調整量；vk表示高斯測量噪聲。

式（66）代表一個一般性的非線性系統(tǒng)，實驗將以此系統(tǒng)作為實際參照系統(tǒng)，通過3 種濾波方法，對系統(tǒng)4 個狀態(tài)進行估計。因為系統(tǒng)（66）作為實際系統(tǒng)，所以3 種濾波方法需要對系統(tǒng)（66）進行狀態(tài)空間表示，然后根據此狀態(tài)空間表達式進行遞推濾波，3 種濾波方法對于系統(tǒng)（66）的狀態(tài)空間表示相同，但相對于式（66）有著一些結構性的變化，該變化可視為系統(tǒng)（67）因建模原因而造成的非線性模型誤差，此誤差用以替代系統(tǒng)（66）的非高斯過程噪聲。

在仿真模型中分別加入CKF、5thCKF、P5thCKF 這3 種濾波器，對系統(tǒng)（66）進行狀態(tài)估計。仿真實驗中，狀態(tài)向量的初始值設為

估計誤差方差陣初始值為

P5thCKF 的加權矩陣：

模型誤差初始值：

測量噪聲方差陣設為R=[0.5]。設定仿真步長為0.000 5 s，仿真步數為80，仿真時長為0.04 s，隨機選取可行域內不同狀態(tài)初始值，運行20 次蒙特卡洛實驗，最終通過均方根誤差（RMSE）[7］對3 種方法進行濾波精度分析。仿真結果如圖2、表1 和表2 所示。

表2 各狀態(tài)RMSE 方差Table 2 RMSE variance for each state

圖2 狀態(tài)1～4 的RMSEFig.2 RMSE for State 1-State 4

對比系統(tǒng)（66）和系統(tǒng)（67）可知，狀態(tài)3 與狀態(tài)4 因為三角函數的原因，其全過程輸出值應＜1，表1 可以看出CKF、5thCKF 這2 種方法因為模型誤差的原因其均方根誤差均值處于0.3、0.17左右，而P5thCKF 則接近于0。狀態(tài)1 因為反正切函數的原因，其本身輸出值很大，所以狀態(tài)3 與狀態(tài)4 帶來的模型誤差影響對于狀態(tài)1 微乎其微，所以3 種方法的均方根誤差相差不大。狀態(tài)2因為缺少了狀態(tài)1 和乘積因子對其的抑制，使得其估計值會比實際值大1 倍之多，表1 可看出CKF 與5thCKF 對于狀態(tài)2 的均方根誤差均值極大，而P5thCKF 卻僅僅為0.48。根據以上分析可以看出，P5thCKF 在強非線性、非高斯過程噪聲系統(tǒng)下具有可行性。在4 個狀態(tài)的估計中，P5thCKF 的估計效果比CKF、5thCKF 的效果要好，而CKF 與5thCKF 之間差距不大，這是因為系統(tǒng)模型不精確而產生的模型誤差誤導了CKF與5thCKF 這2 種方法估計狀態(tài)的最終走向，而P5thCKF 因自身帶有模型誤差校正環(huán)節(jié)，可根據實際觀測量將系統(tǒng)模型誤差以調整量的形式回饋給系統(tǒng)本身，所以能使其自調整、更新系統(tǒng)模型以彌補模型誤差帶來的精度損失。

表1 各狀態(tài)RMSE 均值Table 1 Mean RMSE for each state

5.2 實驗2

實驗2 采用姿態(tài)角濾波以驗證本文算法對于工程實踐應用的可能性。實驗以四元數和陀螺儀角速率為狀態(tài)變量作狀態(tài)方程，以加速度計、磁力計輸出為觀測變量作觀測方程，四元數輸出值最終轉化為歐拉角形式進行比對分析。采用一階龍格庫塔法對連續(xù)系統(tǒng)方程進行離散化，系統(tǒng)方程表示如下[20-23］

狀態(tài)方程：

狀態(tài)變量x中，q表示四元 數，ω表示陀螺儀角速率；δ表示陀螺儀漂移噪聲[24-25］和模型誤差等非高斯因素；z表示加速度計、磁力計測得的姿態(tài)角四元數；vk表示加速度計、磁力計的測量噪聲，本實驗假設其為高斯分布。

在飛行器仿真模型中分別加入CKF、5thCKF、P5thCKF 這3 種濾波器，并在陀螺儀角速率輸入通道加入漂移噪聲和非高斯噪聲模塊，在加速度計、磁力計輸出過程中加入高斯噪聲模塊，對其進行姿態(tài)角估計。仿真實驗中，狀態(tài)向量的初始值為

估計誤差方差陣初始值為

P5thCKF 的加權矩陣：

模型誤差初始值：

測量噪聲方差陣設為

設定仿真步長為0.5，仿真步數為80，仿真時長為40 s，對飛行器模型隨機選取輸入值以得到飛行器動態(tài)變化的姿態(tài)角，運行20 次蒙特卡洛實驗，每一次實驗中，步長20～35 為姿態(tài)角動態(tài)變化區(qū)域，最終通過均方根誤差（RMSE）對3 種方法進行濾波精度分析。仿真結果如圖3、表3和表4所示。

圖3 姿態(tài)角的RMSEFig.3 RMSE of attitude angle

表3 各姿態(tài)角RMSE 均值Table 3 Mean RMSE for each attitude angle

表4 各姿態(tài)角RMSE 方差Table 4 RMSE variance for each attitude angle

從工程應用的角度來看，在濾波精度上，由表3 數據可算出，P5thCKF 比CKF 的濾波效果要高約80%，比5thCKF 的濾波效果要高約45%。

在計算量或耗時上，每一次蒙特卡洛實驗的算法部分仿真實際時間：CKF 耗時約0.099 2 s，5thCKF 耗 時 約0.117 4 s，P5thCKF 耗 時 約0.120 7 s。在這一次實驗中，連續(xù)迭代循環(huán)調用算法80 次，即3 種算法每一次被調用平均耗時約為0.001 24 s、0.001 46 s 和0.001 5 s，可 知P5thCKF 相較于CKF 和5thCKF 在耗時上增加了約21% 和2.8%。綜上，對于CKF 來說，P5thCKF 犧牲了21%的耗時從而得到80%濾波效果的提升，而對于5thCKF 來說，P5thCKF 僅僅增加了2.8%的耗時但換取了45%的提升效果。以上數據也一定程度上說明了本文方法應用于實際工程的可能性。本實驗忽略了加速度計、磁力計非高斯測量噪聲因素的影響，而是直接假設其服從于高斯分布，使得實驗環(huán)境趨于理想化，這也是該方法要想應用于實際工程需要重點關注的要點之一，但本文方法因為預測濾波的特性，省去了實際工程應用中需要特別處理的過程噪聲或模型誤差的過程，僅僅只需考慮測量噪聲的處理，這在實際工程量上有了大大的縮減，且一般的工程系統(tǒng)對于過程噪聲的考慮與處理要相比于測量噪聲要更加復雜，這也體現(xiàn)了本文方法在實際工程應用中的優(yōu)勢。

6 結 論

本文提出了一種預測-五階容積卡爾曼濾波算法，以應用于強非線性、非高斯過程噪聲系統(tǒng)，得出以下結論：

1）P5thCKF 的實質是在五階容積卡爾曼濾波的框架下用預測濾波的方法對系統(tǒng)過程噪聲或模型誤差和其方差陣進行實時調整，進而實時遞推濾波。其優(yōu)點是不需要考慮系統(tǒng)中過程噪聲和模型誤差對濾波造成的影響，而只需考慮測量噪聲的性質及其方差即可，大大簡化了前提條件。

2）P5thCKF 不僅保證了算法每一次迭代因噪聲干擾的系統(tǒng)模型的準確性，而且采用高精度的數值積分準則提高了三階球面-徑向容積積分的代數精度，使得方法更加靈活，精度更高。

3）2 個仿真驗證表明，P5thCKF 不管在具有非線性模型誤差的系統(tǒng)中還是在非高斯過程噪聲干擾下的系統(tǒng)中，其濾波效果都比CKF、5thCKF 更好，且進一步說明了本文方法在強非線性、非高斯過程噪聲系統(tǒng)下具有可行性，在應用于實際工程中具有可能性。

4） 本文所涉及到的內容僅僅只是對P5thCKF 方法的提出，而其還有很多內容可進行拓展研究，比如對加權矩陣的模糊動態(tài)調整，對七階卡爾曼濾波進行簡化處理，降低計算復雜度以替代五階容積卡爾曼濾波，將本方法應用于實際工程以考慮各種高斯、非高斯測量噪聲的處理問題等。