七階正交容積卡爾曼濾波算法

孟東，繆玲娟，邵海俊，沈軍

北京理工大學 自動化學院，北京 100081

七階正交容積卡爾曼濾波算法

孟東，繆玲娟*，邵海俊，沈軍

北京理工大學 自動化學院，北京 100081

在高斯濾波框架下，階次越高，近似精度越高。為提高濾波精度，通過提高階次，提出了七階正交容積卡爾曼濾波(CQKF)算法。在傳統CQKF算法的基礎上，該算法擴展了線性積分的近似階次，提出了七階球面積分的確定性采樣方法；進而擴展了球-半徑準則，提高了濾波估計精度。飛行器目標跟蹤的仿真實驗證明了該算法的有效性，證明了七階CQKF比五階CQKF、三階容積卡爾曼濾波器(CKF)和無跡卡爾曼濾波器(UKF)有更高的濾波精度。

高階正交濾波器；CKF；飛行器目標跟蹤；七階CQKF；UKF

卡爾曼濾波(KF)算法已經廣泛用于動態系統的濾波估計應用中[1]。例如，卡爾曼濾波算法應用在捷聯慣導系統中的初始對準過程中，能有效減小非線性模型誤差，減小失準角，提高系統初始對準的精度[2]；KF應用在組合導航系統(INS/GPS)中，降低了導航信號的系統噪聲和觀測噪聲，提高了導航精度[3]；KF應用在雷達跟蹤系統中，解決了七維狀態的雷達跟蹤難題，提高了跟蹤精度[4]。在此基礎上，發展出多種相關算法，有卡爾曼濾波(EKF)、無跡卡爾曼濾波(UKF)、容積卡爾曼濾波(CKF) 、正交容積卡爾曼濾波(CQKF) 、內嵌容積卡爾曼濾波(ICKF)等算法。EKF算法完成了對濾波對象的一階線性化，實現了KF濾波的初級應用[5-6]。UKF算法引入了確定性采樣的策略，處理了高非線性動態系統的估計問題[7-9]，能達到三階的測量精度。CKF算法采用了比UKF更好的采樣策略，增強了濾波的穩定性[10-11]。CQKF算法用采樣點近似的方法實現了對線積分的近似，提高了球-半徑準則中線積分的近似精度，進一步增強了濾波的精度[12-13]。近年來，高階濾波精度的算法不斷被提出來。五階UKF[14]、五階CKF[15]、五階嵌入式卡爾曼濾波(ECKF)算法[16]、五階ICKF[17]、高階CQKF[13]等高階算法的提出，都提高了原三階算法的濾波精度。高階算法能增加確定性采樣的采樣點個數，并在高斯近似理論中，取得高于低階算法的近似精度。但是，由于高階算法的復雜度較高，高階算法的采樣點的點集較難確定，因此，高于五階精度的高階算法較少被提及。

為進一步提高濾波精度，本文提出了七階正交容積卡爾曼濾波(7th-CQKF)算法。在傳統的高階CKF算法基礎上，本文擴展了球-半徑采樣準則，確定了7th-CQKF的采樣方法；在高斯濾波器的框架下，詳細推導出新濾波算法步驟。仿真結果證明，新方法提升了濾波精度。

1 三階容積卡爾曼濾波(3rd-CKF)

傳統的CKF濾波器建立在非線性離散動態系統上，令離散控制系統狀態方程和量測方程的非線性模型為

xk=f(xk-1,uk-1)+wk-1

(1)

zk=h(xk,uk)+vk-1

(2)

式中:xk為k時刻的狀態量；f和h為非線性函數；zk為測量值；uk為控制輸入；wk-1和vk-1是零均值、協方差分別為Qk-1和Rk-1的加性高斯白噪聲。對任意階函數f(x)，其積分可表示為[10]

If=

(3)

可被分解為兩類積分，分別為線積分和球面積分，則有

If=

(4)

(5)

三階CKF化簡為由一組2n個等權值的容積點(采樣點)來實現積分的數值逼近，共有2n個采樣點。這就是CKF算法的基本思想。

2 七階正交容積卡爾曼濾波(7th-CQKF)

CQKF算法[12]在CKF算法的基礎之上，對線積分使用切比雪夫-拉蓋爾多項式得到正交點，實現對線積分的采樣，并降低了容積準則的擴展難度。文獻[13]中的高階CQKF算法擴展了正交容積準則，將近似精度提高到五階。本文推導了七階正交容積準則，首次確定了在正交容積準則的前提下，七階準則下CQKF的采樣點及其權重，得到7th-CQKF算法。

首先，任意階次函數的積分形式式(4)包含兩類積分，第1種是線積分形式，即

(6)

(7)

文獻[15]已經證明，如果積分式(4)中要達到七階精度，那么，必須滿足兩類積分同時達到七階精度。同理，本文推導線積分和球面積分的采樣準則，使兩類積分準則均達到七階近似精度，以獲得七階精度的CKF濾波算法。

2.1 七階線積分準則

對于式(6)，根據文獻[12-13]，應用高斯-拉蓋爾正交準則(GGLQ)，線積分可用正交點近似為

(8)

式中：n′為近似階次，正交點λi′可由m階的切比雪夫-拉蓋爾等式確定[18-19],即

(9)

采樣點計算公式為

(10)

式中：α=n/2-1，當維數n確定后，解方程后可以得到n′個λ值，即n′個正交采樣點λi′。其相應的權重為

(11)

解以上方程，可得到3維下的線積分采樣點λi′；同理，在不同n下的方程，計算線積分采樣點，如表1所示。

表1 七階線積分采樣點Table 1 Seventh-degree sampling points of line integral

解不同維數n下的七次方程，易得到任意維下的線積分采樣點，采樣點的個數等于n′。當n′=7時，式(8)即為本文所擴展的七階線積分準則。

2.2 七階球面積分準則

文獻[16]在文獻[17]基礎之上，提出了基于拉格朗日插值定理的球面積分準則，該準則結論如下：

定理1球面積分U(f)能達到2m+1階精度，可表示為

(12)

式中：ωp和G(up)分別定義為

(13)

(14)

Γ((kn+1)/2)

(15)

為得到七階球面積分準則，按照降序分類-非降序排列-對稱采點等3大步驟，完成了七階算法的采點。

首先，用降序排列法對p的組合方式分類。分析式(13)和式(14)：

ωh1=ω(3,0,0,…,0)=

(16)

(17)

(18)

式中：An為n維球體的表面積。

其次，在p不按降序排列的條件下，分別討論p的不同組合的情況。

1) 與組合p=[3,0,0,0,…,0]等權重的方式有[0,3,0,0,…,0][0,0,3,0,…,0][0,0,0,…,3]，共有n個組合。

2) 與組合p=[2,1,0,0,…,0]等權重的方式有[2,0,1,0,…,0][2,0,0,1,0,…,0][2,0,0,0,…,1]，[0,2,1,0,0,…,0][0,2,0,1,0,…,0][0,2,0,0,1,0,…,0][0,2,0,0,0,…,1]…[0,…,0,0,2,0,1][0,…,0,0,0,2,1]，及數字2和1交換位置后的組合[1,0,2,0,…,0][1,0,0,2,0,…,0][0,0,…,0,1,2]，共有n(n-1)個組合。

3) 與組合p=[1,1,1,0,0,…,0]等權重的方

式有[1,1,0,1,…,0][1,1,0,0,1,…,0]…[1,1,0,0,0,…,1]，[1,0,1,1,0,…,0][1,0,1,0,1,0,…,0]…[1,0,1,0,…,0,1]…[0,0,0,0,…,1,1,1]，共有n(n-1)(n-2)/6個組合。

以上分析的每一種組合，各組均有各自相同的權重，如：

ωh1=ω(3,0,0,…,0)=ω(0,3,0,L,0)=…=

ω(0,0,0,…,3)

ωh2=ω(2,1,0,…,0)=ω(2,0,1,…,0)=…=

ω(0,…,0,1,2)

ωh3=ω(1,1,1,0,…,0)=ω(1,1,0,1,…,0)=…=

ω(0,…,0,1,1,1)

對于這3類組合，其每一類組合的總權重為

七階球面準則有3種算子的組合方式，如式(12)所示，3種算子組合及其相應權重，構成了對積分的七階近似。每種算子組合對應于一種采樣方式。當為第1種組合時，定義為h1采樣，其中一個算子為p=[3,0,0,…,0]，共有n個組合方式，每一次h1采樣的采樣點個數為2n；當為第2種組合時，定義為h2采樣，其中一個算子為p=[2,1,0,…,0]，共有n(n-1)個組合方式，每一次h2采樣的采樣點個數為4n(n-1)；當為第3種組合時，定義為h3采樣，其中一個算子為p=[1,1,1,0,…,0]，共有n(n-1)(n-2)/6個組合方式，每一次h3采樣的采樣點個數為4n(n-1)(n-2)/3。根據以上分析，每一類采樣點的權重為

(19)

最后，結合式(12)，根據以上分析和采樣點集的對稱性，當m=3時，七階球面積分的表達式為

f(m6j)+f(-m6j)+f(m7j)+f(-m7j)+f(m8j)+f(-m8j)]

(20)

式中的3種采樣方式分別是h1、h2和h3采樣。具體總結為

1)h1采樣h1：包括點集ej和-ej，與式(6)定義相同，ej為單位矩陣的第j列向量，共有2n個采樣點。

2)h2采樣h2：包括點集{m1}{m2}{m3}{m4}，分別定義為

(21)

式中：ek和eg分別為單位矩陣的第k和g列向量，每一次h2采樣共有4n(n-1)個采樣點。

3)h3采樣h3：包括點集 {m5}{m6}{m7}和{m8} ,分別定義為

(22)

式中：每一次h3采樣共有4n(n-1)(n-2)/3 個采樣點；ek、eg和et分別為單位矩陣的第k、g和t列向量。則式(20)簡化表達為

(23)

式中：G(up)代表積分近似過程中的采樣部分；f(h1)代表對函數f的一次h1采樣；f(h2)代表對函數f的一次h2采樣；f(h3)代表對函數f的一次h3采樣。

式(20)完成了七階球面積分的分解，共有采樣點2n+4n(n-1)+4n(n-1)(n-2)/3=2n(2n2+1)/3個。式(20)～式(23)就是本文推導出的七階球面積分準則。

2.3 七階正交球-半徑準則

If=

(24)

根據式(12)～式(14)，將式(24)代入球面積分準則得

If=

(25)

根據式(20)～式(23)，線積分采樣個數n′=7，式(25)中代入線積分采樣準則，七階正交容積球-半徑準則可以表示為

If=

(26)

由式(11)和式(19)可知，球面積采樣個數Ns=2n(2n2+1)/3，式(26)各系數及半徑均已知。令式(26)的各采樣部分的權重系數分別為A、B和C，If簡化為

(27)

式中：h1、h2和h3代表球面積分準則的3種采樣方式。參考式(20)和式(27)中子項表示為

(28)

(29)

(30)

(31)

(32)

(33)

式(27)就是本文推導出的七階容積卡爾曼濾波公式。令式(27)的簡化模型為

(34)

則其相應的容積點和權值為

εi=

(35)

ωi=

(36)

式中：i′=1,2,…,7；h1i代表該采樣方法的第i個采樣點；濾波維數n確定后，每種采樣方式的采樣點個數是固定的。

式(27)～式(36)就是本文提出的7th-CQKF算法的基本公式。7th-CQKF算法的采樣點總數為14n(2n2+1)/3。7th-CQKF算法提高了濾波算法的近似精度，比5th-CQKF、3rd-CKF算法的濾波精度高。

7th-CQKF算法的步驟：

1) 初始化

(37)

式中：E為期望值；Chol為喬列斯基(Cholesky)分解；S0為方差p0的均方根。

2) 計算正交點

計算線積分采樣點λi′，并求出七階正交容積權重Ai′,Bi′,Ci′。如2.1節表1所示。式(38)～式(44)為時間更新方程。

3) 假定在時刻k-1

(38)

(39)

4) 估計容積點

(40)

5) 估計傳播容積點

(41)

6) 估計預測狀態和協方差

令v=14n(2n2+1)/3，

(42)

(43)

(44)

式中：Qk-1是過程噪聲方差。式(45)～式(52)為測量更新方程。

7) 估計容積點

(45)

8) 估計傳播容積點

zi,kk-1=h(xi,kk-1,uk)

(46)

9) 估計預測測量值

(47)

10) 估計方差和協方差矩陣

(48)

zi,k-1k-1)T

(49)

11) 估計卡爾曼濾波增益

(50)

12) 估計更新狀態

(51)

13) 估計相應的方差

(52)

14)跳轉到步驟2)，更新均值與方差后，繼續下一次7th-CQKF濾波算法，直到濾波循環結束。

2.4 高階算法的對比

分析三階、五階、七階卡爾曼濾波算法的特點。

首先，本文提出的7th-CQKF濾波算法與傳統的CKF、5th-CKF的采點方法不同。

7th-CQKF濾波算法在求解半徑積分準則的過程中，采用了高斯-拉格朗日正交準則[16]；傳統的CKF、5th-CKF則采用了線積分準則[10]。高斯-拉格朗日正交準則比線積分準則有更高的近似精度。同時，高斯-拉格朗日正交準則的擴展性好，可以近似任意精度的半徑積分，具有較低的復雜度。而傳統的CKF、5th-CKF則采用了線積分準則，擴展性不好，在高階擴展過程中，有自由變量的產生，算法不唯一。以下舉例分析其不唯一性。

文獻[15]提出的5th-CKF濾波算法理論，在求解半徑準則時，需求解方程：

(53)

式(53)中有一個自由變量，該方程必須在自由變量確定后，才能求解，自由變量的確定具有不唯一性，此特性增加了5th-CKF的推導難度和復雜度，使5th-CKF的形式不唯一。更進一步，當CKF算法推廣到七階精度時，7th-CKF具有兩個自由變量，算法不唯一，進一步增加了算法的復雜度。

根據本文推導的式(8)～式(11)可發現，7th-CQKF算法的形式唯一，不存在自由變量，推導過程簡潔，算法的復雜度較低。因此，本文提出的7th-CQKF濾波算法，改進了半徑準則的求解方法，降低了原高階CKF算法的復雜度。

其次，各方法采樣方法不同，其采樣點個數也不相同。采樣點個數與算法的計算量成正比例關系。統計每個算法在一次濾波中的采樣點個數，如表2所示。

分析表2可知，濾波階次的提高會增加算法的采樣點個數。在考慮計算量的前提下，應該根據實際應用環境，按照可容忍的計算量的大小，選擇合適的濾波算法，以在計算量和精度之間達到平衡。因此，高階算法在實際應用中，需使用高性能的硬件系統處理數據，以提高高階算法的實時性。

7th-CQKF的采樣點數量反映了算法計算量的大小，且與系統狀態的維數n相關。當n≤5時，7th-CQKF算法的計算量與CKF、5th-CKF、5th-CQKF的計算量有一定的差異，但基本都在可容忍的計算量范圍之內；當n>5時，7th-CQKF算法的計算量與CKF、5th-CKF、5th-CQKF開始有明顯的差別，因計算量的增加而對原系統的影響開始顯著，此時，需使用高性能的信號處理系統，以減弱計算量帶來的影響，例如，使用高性能的FPGA、DSP等處理器，均具有較好的效果。

最后，在理論上，算法的階次越高，濾波精度越高。階次代表了在積分近似過程中，泰勒級數的余項能夠近似的最高冪的次數。因此，7th-CQKF濾波算法在理論上的濾波精度高于CKF、5th-CKF、5th-CQKF算法。

表2 采樣點個數對比Table 2 Comparison of number of sampling points

總之，利用改進的球-半徑準則方法，7th-CQKF算法能夠取得高于CKF、5th-CKF、5th-CQKF算法的濾波精度。

3 仿真結果及分析

本文通過仿真實驗，檢驗多個濾波算法對非線性系統的濾波能力，對比各算法在高斯環境下的濾波精度，以驗證7th-CQKF的優良濾波性能。這些算法分別為：UKF算法、CKF算法、5th-CQKF算法、7th-CQKF算法。

本文驗證高維狀態向量下的非線性模型系統為[13,22-24]：

xk=

wk-1

(54)

(55)

(56)

其中：x0=[1000 300 1000 0 -3×10-3]T為狀態向量的初值，時間間隔T=1 s，協方差矩陣P0=diag(100 10 100 10 100)T。在每一次的運行中，初值狀態在分布N(x0,P0)下隨機選取，4種算法在相同的初始條件下運行，仿真時間100 s，運行50次的蒙特卡羅試驗。截取10 s至100 s的仿真結果，通過對比4種算法在這種仿真環境下狀態估計誤差的均方誤差(RMSE)，判斷濾波性能的好壞。RMSE定義為

(57)

圖1 跟蹤軌跡圖
Fig.1 Track diagram

圖2 第1個狀態向量的均方誤差
Fig.2 RMSE of the first state variable

分析圖2～圖6可知，5個狀態向量的RMSE均是7th-CQKF算法最小，且7th-CQKF算法比5th-CQKF算法、CKF算法、UKF算法的濾波精度高；由RMSE的波動范圍可知，UKF算法的波動最大， 7th-CQKF算法、5th-CQKF算法、CKF算法比UKF算法的穩定性高。證明了本文提出的7th-CQKF算法，在提出的新采樣方法下，提高了濾波算法的精度。

圖3 第2個狀態向量的均方誤差
Fig.3 RMSE of the second state variable

圖4 第3個狀態向量的均方誤差
Fig.4 RMSE of the third state variable

圖5 第4個狀態向量的均方誤差
Fig.5 RMSE of the fourth state variable

圖6 第5個狀態向量的均方誤差
Fig.6 RMSE of the fifth state variable

4 結 論

本文通過提高球-半徑準則的近似階次，進行理論推導，提出了7th-CQKF算法，提高了濾波精度，得出以下結論：

1) 擴展了球-半徑準則，在五階球-半徑準則基礎上，分別提出了七階線積分準則和七階球面積分準則，進而提出七階正交球-半徑準則；在七階近似下，確定了七階正交球-半徑準則的3種確定性采樣方法及其權重函數。

2) 在高斯濾波基礎上，根據提出的七階正交球-半徑準則，提出7th-CQKF算法，詳細推導了7th-CQKF算法的濾波步驟。

3) 通過仿真對比，證明7th-CQKF算法比傳統的UKF算法、CKF算法、5th-CQKF算法的濾波精度高；7th-CQKF算法、5th-CQKF算法、CKF算法均比UKF算法的穩定性高。

[1] KALMAN R E. A new approach to linear filtering and prediction problems[J].Transaction of the ASME-Journal of Basic Engineering, 1960, 82(Series D): 35-45.

[2] CUI B B, CHEN X Y, TANG X H. Improved cubature Kalman for GNSS/INS based on transformation of posterior sigma-points error[J]. IEEE Transactions on Signal Processing, 2017, 65(11): 2975-2987.

[3] HU G G, GAO S S, ZHONG Y G. A derivative UKF for tightly coupled INS/GPS integrated navigation[J]. ISA Transactions, 2015, 56: 135-144.

[4] KULIKOV G Y, KULIKOVA M V. Accurate continuous-discrete unscented Kalman filtering for estimation of nonlinear continuous-time stochastic models in radar tracking[J]. Signal Processing, 2017, 139: 25-35.

[5] BUCY R S, SENNE K D. Digital synthesis of non-linear filters[J]. Automatica, 1971, 7(3): 287-298.

[6] SUNAHARA Y, YAMASHITA K. An approximate method of state estimation for non-linear dynamical systems with state-dependent noise[J]. Journal of Fluids Engineering, 2009, 92(2): 385.

[7] JULIER S J, UHLMANN J K. A new extension of the Kalman filter to nonlinear systems[J]. Proceedings of the Society of Photo-optical Instrumentation Engineers, 1997,3068(1): 182-193.

[8] JULIER S J, UHLMANN J K. Unscented filtering and nonlinear estimation[J].Proceedings of IEEE, 2004,92(3): 401-422.

[9] ITO K, XIONG K. Gaussian filters for nonlinear filtering problems[J]. IEEE Transactions on Automatic Control,2000, 45(5): 910-927.

[10] ARASARATNAM I, HAYKIN S. Cubature Kalman filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[11] ARASARATNAM I, HAYKIN S, HURD T R. Cubature Kalman filtering for continuous-discrete systems: Theory and simulations[J]. IEEE Transactions on Signal Processing, 2010, 58(10): 4977-4993.

[12] BHAUMIK S, SWATI. Cubature quadrature Kalman filter[J]. IET Signal Processing, 2013, 7(7): 533-541.

[13] JIA B, XIN M, CHENG Y. Higher degree cubature quadrature Kalman filter[J]. Automatica, 2013, 49(1): 510-518.

[14] WU Y, HU D, WU M. A numerical-integration perspective on Gaussian filters[J].IEEE Transactions on Signal Processing, 2006, 54(8): 2910-2921.

[15] JIA B, XIN M, CHENG Y. High-degree cubature Kalman filter[J]. Automatica, 2013, 49(2): 510-518.

[16] GENZ A. Fully symmetric interpolatory rules for multiple integrals over hyper-spherical surfaces[J]. Journal of Computational and Applied Mathematics, 2003, 157(1): 187-195.

[17] SILVESTER P. Symmetric quadrature formulae for simplexes[J]. Mathematics of Computation, 1970, 24(109): 95-100.

[18] ZHANG Y G, HUANG Y L, LI N, et al. Embedded cubature Kalman filter with adaptive setting of free parameter[J]. Signal Processing, 2015, 114(C): 112-116.

[19] ZHANG Y G, HUANG Y L, LI N, et al. Interpoltory cubature Kalman filter[J]. IET Control Theory Applications, 2015(9): 1731-1739.

[20] KRYLOV V I. Approximate calculation of integrals[M]. New York: Dover Publication, 2005: 58-66.

[21] HILDEBRAND F B. Introduction to numerical analysis[M]. 2nd ed. New York: Dover Publication, 2008: 195-198.

[22] WU H, CHEN S X. Robust range-parameterized cubature Kalman filter for bearings-only tracking[J]. Journal of Central South University, 2016, 23(6): 1399-1405.

[23] WU H, CHEN S X. Ranre-parameterized orthogonal simplex cubature Kalman filter for bearings-only measurements[J]. IET Science, Measurement Technology, 2016, 10(4): 370-374.

[24] XU B, ZHANG P, WEN H Z. Stochastic stability and performance analysis of cubature Kalman filter[J]. Neurocomputing, 2016, 186(C): 218-227.

Aseventh-degreecubaturequadratureKalmanfilter

MENGDong,MIAOLingjuan*,SHAOHaijun,SHENJun

CollegeofAutomation,BeijingInstituteofTechnology,Beijing100081,China

IntheGaussianfilterframe,thehighertheorder,thehighertheaccuracyoftheapproximation.Toimprovethefilteringaccuracy,aseventh-degreeCubatureQuadratureKalmanFilter(CQKF)algorithmisproposedbyimprovingthedegree.BasedonthetraditionalCQKF,thealgorithmextendstheapproximatedegreeoflinearintegrals,andproposesadeterministicsamplingmethodfortheseven-degreesphericalintegral.Thenthespherical-radialruleisextendedtoimprovetheaccuracyofthefilter.Thesimulationresultsoftheaircrafttargettrackingdemonstratetheeffectivenessofthealgorithm.Itisprovedthattheseventh-degreeCQKFismoreaccuratethanthefifth-CQKF,third-degreeCubatureKalmanFilter(CKF)andUnscentedKalmanFilter(UKF).

high-degreeorthogonalfilter;CKF;aircrafttargettracking;seventh-degreeCQKF;UKF

2017-05-12；

2017-05-24；

2017-08-15；Publishedonline2017-08-201717

URL：http://hkxb.buaa.edu.cn/CN/html/20171227.html

s：NationalNaturalScienceFoundationofChina(61153002,61473039)

.E-mailmiaolingjuan@bit.edu.cn

http://hkxb.buaa.edu.cnhkxb@buaa.edu.cn

10.7527/S1000-6893.2017.321410

2017-05-12；退修日期2017-05-24；錄用日期2017-08-15；網絡出版時間2017-08-201717

http://hkxb.buaa.edu.cn/CN/html/20171227.html

國家自然科學基金(61153002,61473039)

.E-mailmiaolingjuan@bit.edu.cn

孟東，繆玲娟，邵海俊，等．七階正交容積卡爾曼濾波算法J． 航空學報,2017,38(12):321410.MENGD,MIAOLJ,SHAOHJ,etal.Aseventh-degreecubaturequadratureKalmanfilterJ.ActaAeronauticaetAstronauticaSinica,2017,38(12):321410.

V419+.9

A

1000-6893(2017)12-321410-11

蘇磊，蔡斐)