， ，

(1.西北工業(yè)大學(xué) 電子信息學(xué)院，西安 710129; 2.西北工業(yè)大學(xué) 航空學(xué)院，西安 710072)

0 引言

Arasaratnam提出的平方根容積卡爾曼濾波[1](square-root cubature Kalman filter，SCKF)具有設(shè)計(jì)方法簡(jiǎn)潔，調(diào)節(jié)參數(shù)少，數(shù)學(xué)推理嚴(yán)謹(jǐn)，收斂性較好等特點(diǎn)，有效克服了無(wú)跡卡爾曼濾波(unscented kalman filter，UKF)基于直覺，高維非線性模型收斂性差的不足，受到了廣泛的關(guān)注和應(yīng)用[2]。和卡爾曼濾波一樣，SCKF也是基于高斯過(guò)程的最優(yōu)估計(jì)，在模型準(zhǔn)確條件下，具有估計(jì)精度高的特點(diǎn)。當(dāng)模型存在誤差時(shí)，由于SCKF的“記憶性”，“陳舊”的量測(cè)數(shù)據(jù)降低當(dāng)前時(shí)刻的狀態(tài)估計(jì)精度，甚至濾波發(fā)散。

為了提高卡爾曼濾波應(yīng)對(duì)模型不準(zhǔn)確性和狀態(tài)突變的能力，文獻(xiàn)[3]基于漸消濾波思想提出了強(qiáng)跟蹤濾波(strong tracking filter，STF)。該濾波器基于正交性原理，迫使不同時(shí)刻的殘差序列處處保持互相正交，從而提取殘差中的有效信息，確定漸消因子，強(qiáng)迫強(qiáng)跟蹤濾波保持對(duì)實(shí)際系統(tǒng)狀態(tài)的跟蹤。但STF是基于擴(kuò)展卡爾曼濾波(extended kalman filter，EKF)，僅利用Taylor展開對(duì)非線性模型進(jìn)行一階近似，當(dāng)系統(tǒng)是高階強(qiáng)非線性時(shí)，STF濾波精度會(huì)嚴(yán)重下降甚至發(fā)散。

SCKF是確定性采樣濾波方法，通過(guò)一定量的采樣點(diǎn)，傳遞后加權(quán)求和來(lái)擬合后驗(yàn)均值和協(xié)方差。與EKF相比，SCKF無(wú)需求解狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)的雅可比矩陣，精度高，收斂性好等特點(diǎn)。將強(qiáng)跟蹤思想引入SCKF，建立強(qiáng)跟蹤SCKF，兼具了兩者的優(yōu)點(diǎn)：魯棒性強(qiáng)，濾波精度高，數(shù)值穩(wěn)定性好，計(jì)算速度快，容易實(shí)現(xiàn)且應(yīng)用范圍廣[4-8]，有效地克服EKF的不足。目前的研究主要集中在基于STF理論架構(gòu)，建立單漸消因子平方根容積卡爾曼濾波(single strong tracking SCKF，STSCKF)，并與其他方法結(jié)合，互補(bǔ)提高濾波器整體性能。然而，STSCKF每次迭代只計(jì)算一個(gè)漸消因子，對(duì)整個(gè)系統(tǒng)進(jìn)行平均性能的改善，不能保證濾波器的最佳性，當(dāng)處理多維系統(tǒng)時(shí)，會(huì)導(dǎo)致部分系統(tǒng)狀態(tài)估計(jì)結(jié)果變差甚至發(fā)散。文獻(xiàn)[9]基于多重漸消因子擴(kuò)展卡爾曼濾波(multiple fading factors extended kalman filter，MFEKF)的思想，基于先驗(yàn)知識(shí)，預(yù)先設(shè)定多個(gè)漸消因子間的比例系數(shù)，將STSCKF擴(kuò)展成多漸消因子SCKF(multiple fading factors strong tracking SCKF，MSTSCKF)，以不同的速率對(duì)多維狀態(tài)進(jìn)行漸消修正，具有更高的估計(jì)精度，但比例系數(shù)的確定嚴(yán)重依賴先驗(yàn)知識(shí)。

為此，本文提出一種新的MSTSCKF，依據(jù)正交原理在SCKF的充分條件，建立多漸消因子的數(shù)值求解算法，迭代中直接計(jì)算確定多個(gè)漸消因子，對(duì)各個(gè)狀態(tài)進(jìn)行單獨(dú)漸消修正，以保證濾波器的最佳性；采用假設(shè)檢驗(yàn)來(lái)判斷是否存在模型不確定或者未知干擾，避免STF中以漸消因子是否大于1作為系統(tǒng)異常判斷標(biāo)準(zhǔn)的不足，降低誤判概率，提高濾波穩(wěn)定性。仿真實(shí)例驗(yàn)證了本文方法的有效性。

1 多漸消因子平方根容積卡爾曼濾波

考慮以下非線性離散系統(tǒng)：

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

(1)

yk=h(xk)+vk

(2)

其中:xk∈Rn為系統(tǒng)狀態(tài)向量，yk∈Rm為量測(cè)向量，n和m為對(duì)應(yīng)維數(shù)；f(·)和h(·)分別為非線性系統(tǒng)的狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)；wk-1∈Rn為系統(tǒng)噪聲，vk∈Rm為量測(cè)噪聲，二者均為高斯白噪聲，且互不相關(guān)，協(xié)方差矩陣為Qk和Rk。

SCKF的核心是三階球面-相徑容積規(guī)則，經(jīng)過(guò)嚴(yán)謹(jǐn)?shù)臄?shù)學(xué)推導(dǎo)，使用2n個(gè)容積點(diǎn)對(duì)非線性高斯濾波中的積分進(jìn)行擬合。與UKF相比，SCKF采樣點(diǎn)是對(duì)稱的，所有點(diǎn)權(quán)值相同且為正數(shù)，沒有中心點(diǎn)，具有更好的穩(wěn)定性。SCKF采用了平方根算法，濾波過(guò)程中傳遞的是預(yù)測(cè)誤差協(xié)方差和后驗(yàn)誤差協(xié)方差的平方根，避免了矩陣求根操作，保證了協(xié)方差矩陣的對(duì)稱性和正定性，進(jìn)一步提高了數(shù)值穩(wěn)定性。

然而，當(dāng)模型中存在較強(qiáng)的不確定時(shí)，SCKF的狀態(tài)估計(jì)精度將會(huì)下降，甚至發(fā)散。而且，SCKF在系統(tǒng)達(dá)到平穩(wěn)狀態(tài)時(shí)，增益矩陣趨于極小值。若系統(tǒng)狀態(tài)發(fā)生突變，殘差將變大，但增益矩陣仍保持極小值，使得SCKF不能充分提取殘差中的信息，喪失對(duì)突變狀態(tài)的跟蹤能力。

STF則是通過(guò)在狀態(tài)預(yù)測(cè)協(xié)方差中引入漸消因子，在線選擇合適的時(shí)變?cè)鲆婢仃?，使得濾波器滿足正交性原理，即:

(3)

(4)

其中:ek為k時(shí)刻的殘差。式(3)是濾波器性能指標(biāo)，式(4)要求濾波器中不同時(shí)刻的殘差序列保持處處正交，也是正交性原理這一名稱的由來(lái)。

將STF思想引入SCKF建立MSTSCKF，能兼得兩者的優(yōu)點(diǎn)，具有魯棒性強(qiáng)，濾波精度高，數(shù)值穩(wěn)定性好，計(jì)算速度快的特點(diǎn)。模型的不確定性可能受多個(gè)狀態(tài)變量影響，采用多個(gè)漸消因子分別對(duì)每個(gè)狀態(tài)變量進(jìn)行調(diào)整，能提高濾波器的性能，保證濾波器的最優(yōu)性。

1.1 MSTSCKF中的漸消因子求解算法

STF是基于EKF建立，在求解多漸消因子時(shí)，依賴先驗(yàn)知識(shí)確定多漸消因子之間的比例，再以類似單漸消因子的方法求解。在求解過(guò)程中，需要使用狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)的雅可比矩陣。SCKF是確定性采樣濾波方法，計(jì)算中并不需要使用雅可比矩陣。實(shí)際中，先驗(yàn)知識(shí)和非線性系統(tǒng)的雅可比矩陣都不易獲得，為了降低對(duì)它們的依賴，建立SCKF中的多漸消因子直接求解算法。

為了與平方根算法相適應(yīng)，保證預(yù)測(cè)誤差協(xié)方差陣的對(duì)稱性和正定性，MSTSCKF中引入多漸消因子形式如下：

(5)

(6)

類似強(qiáng)跟蹤UKF殘差正交化充分條件的數(shù)學(xué)推導(dǎo)[10]，式在SCKF中的充分條件為:

(7)

其中:Vk計(jì)算如下：

(8)

其中:ρ是遺忘因子，0<ρ≤1，通常取ρ=0.95。將相關(guān)變量帶入并簡(jiǎn)化得，

(9)

(10)

令式(9)左右兩邊對(duì)角線元素相等，有:

(11)

從而求得:

(12)

(13)

漸消因子小于1時(shí)，表示不存在模型不準(zhǔn)確性或者狀態(tài)突變，故限定漸消因子應(yīng)大于等于1。整理得MSTSCKF中多漸消因子求解公式為：

(14)

1.2 系統(tǒng)異常檢測(cè)

在STF中，系統(tǒng)是否存在異常的判斷是基于求解得的漸消因子值。若漸消因子大于1，則系統(tǒng)存在異常，通過(guò)漸消因子進(jìn)行修正；反之，系統(tǒng)正常，將漸消因子賦值為1，表示不進(jìn)行修正，容易出現(xiàn)誤判。本文采用假設(shè)檢驗(yàn)的方法，定義一個(gè)服從自由度為m的χ2統(tǒng)計(jì)量，對(duì)系統(tǒng)異常進(jìn)行檢測(cè)。定義兩個(gè)假設(shè)如下：

零假設(shè)γ0：系統(tǒng)正常工作；

備選假設(shè)γ1：系統(tǒng)存在異常。

定義χ2統(tǒng)計(jì)量βk如下：

(15)

選定顯著性水平α，則有:

(16)

(17)

1.3 MSTSCKF算法流程

MSTSCKF具體算法步驟如下：

(18)

(19)

(20)

(21)

(22)

(23)

(24)

(25)

(26)

2 仿真分析

采用某一強(qiáng)耦合非線性隨機(jī)系統(tǒng)[9]來(lái)驗(yàn)證MSTSCKF的有效性，并對(duì)量測(cè)方程做了相應(yīng)修改，具體如下：

為驗(yàn)證本文提出的MSTSCKF處理模型不準(zhǔn)確性和狀態(tài)突變的能力，仿真中采用3種方法進(jìn)行對(duì)比：SCKF、文獻(xiàn)[7]提出的STSCKF和本文的MSTSCKF。

2.1 狀態(tài)突變

假設(shè)在k=70和k=140時(shí)，由于外部干擾，xk,1分別有+4和-3的突變。SCKF、STSCKF和MSTSCKF對(duì)4個(gè)狀態(tài)的估計(jì)如圖1～圖4所示。

圖1 三種方法對(duì)xk,1的狀態(tài)估計(jì)

圖2 三種方法對(duì)xk,2的狀態(tài)估計(jì)

圖3 三種方法對(duì)xk,3的狀態(tài)估計(jì)

圖4 三種方法對(duì)xk,4的狀態(tài)估計(jì)

從圖1中可知，當(dāng)突變發(fā)生后，SCKF不能及時(shí)跟蹤突變狀態(tài)，對(duì)突變響應(yīng)比較遲緩，估計(jì)精度有所下降，需要很長(zhǎng)一段時(shí)間才能對(duì)回到正常誤差區(qū)間，應(yīng)對(duì)干擾能力不足，魯棒性較差。引入強(qiáng)跟蹤思想后，STSCKF和MSTSCKF均能有效的從殘差中提取信息，計(jì)算漸消因子，實(shí)現(xiàn)對(duì)突變狀態(tài)的跟蹤，響應(yīng)速度快，魯棒性好，且在突變點(diǎn)附近，MSTSCKF穩(wěn)定性優(yōu)于STSCKF，波動(dòng)小，估計(jì)精度更好。

從圖2可知，突變發(fā)生時(shí)，SCKF的估計(jì)誤差并沒有明顯的變化，而是在十多秒后突然誤差變大，偏離真實(shí)值，很長(zhǎng)時(shí)間沒有收斂回正常誤差范圍。STSCKF在突變發(fā)生時(shí)有比較明顯的波動(dòng)，估計(jì)誤差變大，這是由于單漸消因子的局限性，只能對(duì)濾波器實(shí)現(xiàn)平均性能的改善，犧牲了其他狀態(tài)的估計(jì)精度。雖然MSTSCKF在突變處也有一定的波動(dòng)，但并不明顯。采用假設(shè)檢驗(yàn)對(duì)系統(tǒng)異常進(jìn)行檢測(cè)后，MSTSCKF的濾波穩(wěn)定性優(yōu)于STSCKF，在保持魯棒性的情況下，減少了誤判導(dǎo)致的濾波結(jié)果波動(dòng)，在三種算法中估計(jì)精度最好。

圖3和圖4中表現(xiàn)地更為明顯，SCKF和STSCKF對(duì)xk,3和xk,4的估計(jì)在第一次突變后就急速下降，并趨于發(fā)散，STSCKF估計(jì)精度甚至比SCKF更差。MSTSCKF對(duì)xk,3和xk,4的估計(jì)精度不如前面兩個(gè)變量那么好，有比較明顯的波動(dòng)，然而明顯優(yōu)于STSCKF和SCKF，隨著時(shí)間逐漸趨于正常的誤差范圍，并沒有趨于發(fā)散。

可見，SCKF并沒有處理狀態(tài)突變的能力，抗干擾能力差，對(duì)4個(gè)狀態(tài)的估計(jì)精度均不理想。STSCKF引入單漸消因子，對(duì)xk,1和xk,2實(shí)現(xiàn)了較好的估計(jì)，但局限于單漸消因子只能對(duì)濾波器進(jìn)行平均性能改善的不足，對(duì)xk,3和xk,4的估計(jì)非常不理想，無(wú)法保證每個(gè)狀態(tài)的最優(yōu)估計(jì)。MSTSCKF采用4個(gè)漸消因子，分別對(duì)4個(gè)狀態(tài)進(jìn)行修正，有效地保證了濾波器的最優(yōu)性，不但能有效跟蹤狀態(tài)突變，對(duì)其他狀態(tài)變量也保持了良好的濾波精度。

2.2 模型不確定性

假設(shè)從k=70起，由于外界因素，系統(tǒng)噪聲方差變?yōu)镼=diag[0.01,0.01,0,0]，而在濾波器中，仍然認(rèn)為Q=0。進(jìn)行100次蒙特卡洛仿真，所得到的4個(gè)狀態(tài)變量平均絕對(duì)誤差如圖5所示。

在系統(tǒng)噪聲發(fā)生變化之前，SCKF、STSCKF和MSTSCKF均能較好的實(shí)現(xiàn)狀態(tài)估計(jì)，估計(jì)精度沒有明顯的區(qū)別。系統(tǒng)噪聲發(fā)生變化后，SCKF估計(jì)誤差明顯變大，呈發(fā)散趨勢(shì)，都沒有再次收斂回到正常誤差范圍。STSCKF對(duì)xk,1和xk,2的估計(jì)結(jié)果明顯優(yōu)于SCKF，但由于一個(gè)漸消因子不能保證對(duì)各個(gè)狀態(tài)分量修正都是合理的，STSCKF對(duì)xk,3和xk,4的估計(jì)結(jié)果不是很理想，且在系統(tǒng)噪聲發(fā)生變化時(shí)，有比較大的波動(dòng)，需要一定時(shí)間才能回到正常誤差區(qū)間。MSTSCKF對(duì)各個(gè)變量分別進(jìn)行修正，各變量的估計(jì)誤差均小于SCKF和STSCKF，保證了每個(gè)變量的濾波最優(yōu)性，克服了STSCKF的不足，假設(shè)檢驗(yàn)對(duì)系統(tǒng)異常進(jìn)行檢測(cè)后，濾波過(guò)程穩(wěn)定，波動(dòng)少。

圖5 系統(tǒng)噪聲變化時(shí)4個(gè)狀態(tài)的平均估計(jì)誤差

3 結(jié)束語(yǔ)

為了解決SCKF在系統(tǒng)模型不準(zhǔn)確和狀態(tài)突變情況下魯棒性差，濾波精度明顯下降的問(wèn)題，本文結(jié)合強(qiáng)跟蹤濾波器思想，將多漸消因子引入SCKF，建立MSTSCKF。完善多漸消因子引入方法，保持預(yù)測(cè)誤差協(xié)方差的對(duì)稱性；基于正交原理，推導(dǎo)多漸消因子在SCKF中的數(shù)值求解方法，克服原多漸消因子求解依賴先驗(yàn)知識(shí)的不足。MSTSCKF基于多漸消因子，各個(gè)變量分別進(jìn)行修正，互不影響，保證了濾波器的最優(yōu)性，克服單漸消因子的缺點(diǎn)。采用假設(shè)檢驗(yàn)代替漸消因子作為系統(tǒng)是否異常的標(biāo)準(zhǔn)，減少誤判，提高穩(wěn)定性。仿真結(jié)果表明，在模型存在不確定性和狀態(tài)突變的情況下，MSTSCKF對(duì)每個(gè)狀態(tài)都保持了良好的估計(jì)精度和響應(yīng)速度，突變點(diǎn)波動(dòng)小，各方面均優(yōu)于SCKF和STSCKF，有效的彌補(bǔ)了SCKF的不足。

參考文獻(xiàn)：

[1] Arasaratnam I, Haykin S. Cubature kalman filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[2] Zhang Y, Huang Y, Li N, et al. Embedded cubature Kalman filter with adaptive setting of free parameter[J]. Signal Processing, 2015, 114: 112-116.

[3] 周東華, 席?？? 非線性系統(tǒng)帶次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波[J]. 控制與決策, 1990, 5(5): 1-6.

[4] Zhao L, Wang J, Yu T, et al. Design of adaptive robust square-root cubature Kalman filter with noise statistic estimator[J]. Applied Mathematics and Computation, 2015, 256: 352-367.

[5] Hu G, Gao S, Zhong Y, et al. Modified strong tracking unscented Kalman filter for nonlinear state estimation with process model uncertainty[J]. International Journal of Adaptive Control and Signal Processing, 2015, 29(12): 1561-1577.

[6] Wu S, Wang B, Zhou Z, et al. The Application of the Fuzzy Strong Tracking UKF in the SINS Swing Base Initial Alignment[J]. 2014: 313-318.

[7] 杜占龍, 李小民, 鄭宗貴, 等. 強(qiáng)跟蹤平方根容積卡爾曼濾波和自回歸模型融合的故障預(yù)測(cè)[J]. 控制理論與應(yīng)用, 2014, (8): 1047-1052.

[8] 杜占龍, 李小民. 多重漸消因子強(qiáng)跟蹤 SCKF 及其在故障參數(shù)估計(jì)中的應(yīng)用[J]. 系統(tǒng)工程與電子技術(shù), 2014, (4): 643-649.

[9] 周東華, 席裕庚, 張鐘俊. 一種帶多重次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器[J]. 自動(dòng)化學(xué)報(bào), 1991, 6: 689-695.

[10] 郭 澤, 繆玲娟, 趙洪松. 一種改進(jìn)的強(qiáng)跟蹤 UKF 算法及其在 SINS 大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用[J]. 航空學(xué)報(bào), 2014, 35(1): 203-214.