改進(jìn)的強(qiáng)跟蹤平方根容積卡爾曼濾波方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種非線(xiàn)性濾波方法。
【背景技術(shù)】
[0002] 容積卡爾曼濾波（cubatureKalmanfilter,CKF)是由加拿大學(xué)者Ienkaran Arasaratnam和SimonHaykin在2009年提出的一種新的非線(xiàn)性近似濾波算法。由于CKF 求解容積點(diǎn)時(shí)，要對(duì)協(xié)方差陣開(kāi)方，隨著濾波迭代次數(shù)的增加，舍入誤差的積累，可能會(huì)導(dǎo) 致協(xié)方差陣失去非負(fù)定性甚至失去對(duì)稱(chēng)性。IenkaranArasaratnam和SimonHaykin在 CKF的基礎(chǔ)上，加入平方根算法，提出平方根容積卡爾曼濾波算法（square-rootcubature Kalmanfilter，SCKF)。該算法有效地解決了CKF的數(shù)值穩(wěn)定性問(wèn)題，并減少計(jì)算量，提供 更佳的濾波性能。
[0003] 在實(shí)際系統(tǒng)中，由于（1)數(shù)學(xué)模型不準(zhǔn)確；(2)系統(tǒng)本身在緩慢動(dòng)態(tài)變化，而建立 的數(shù)學(xué)模型難以根據(jù)這些變化動(dòng)態(tài)改變模型，導(dǎo)致模型匹配性逐漸變差；（3)受系統(tǒng)外部 變化影響等原因，系統(tǒng)模型并不能完全準(zhǔn)確，導(dǎo)致SCKF濾波效果不理想，嚴(yán)重時(shí)甚至導(dǎo)致 濾波發(fā)散。周華東等人基于正交原理建立強(qiáng)跟蹤算法（strongtrackingfilter,STF)，大 大增強(qiáng)了非線(xiàn)性濾波算法的魯棒性。結(jié)合強(qiáng)跟蹤的思想，將減消因子引入SCKF中，可建立 強(qiáng)跟蹤SCKF算法（strongtrackingSCKF，STSCKF)，克服SCKF在系統(tǒng)模型不確定時(shí)濾波 精度下降的缺點(diǎn)。STSCKF已經(jīng)被應(yīng)用在機(jī)動(dòng)跟蹤、組合導(dǎo)航等領(lǐng)域，有效提高系統(tǒng)的魯棒性 和精度。
[0004] 但STSCKF算法引入減消因子過(guò)程中，首先要獲取k+Ι時(shí)刻一步預(yù)測(cè)互相關(guān)協(xié)方差 陣，再計(jì)算減消因子，最后進(jìn)行量測(cè)更新。根據(jù)SCKF實(shí)現(xiàn)步驟，這種減消因子引入方法等價(jià) 于將量測(cè)更新中"計(jì)算容積點(diǎn)"到"計(jì)算互相關(guān)協(xié)方差陣"部分重復(fù)執(zhí)行了兩次，大幅增加 了算法的時(shí)間復(fù)雜度。

【發(fā)明內(nèi)容】

[0005] 為了克服現(xiàn)有技術(shù)的不足，本發(fā)明提供一種改進(jìn)的強(qiáng)跟蹤SCKF算法（improved strongtrackingSCKF)，通過(guò)分析減消因子提高強(qiáng)跟蹤算法魯棒性的機(jī)理和SCKF算法流 程特點(diǎn)，ISTCKF重新選擇減消因子引入位置，減少由于減消因子引入帶來(lái)額外計(jì)算量。
[0006] 本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案包括以下步驟：
[0007](1)設(shè)定初始參數(shù)設(shè)定，包括初始時(shí)刻系統(tǒng)狀態(tài)值X。、初始時(shí)刻系統(tǒng)狀態(tài)協(xié)方差平 方根S。、系統(tǒng)噪聲協(xié)方差Q、觀(guān)測(cè)噪聲協(xié)方差R和遺忘因子P;
[0008] ⑵時(shí)間更新，包括以下內(nèi)容：
[0009]首先定義S=Tria(AMXN)表示一種矩陣三角分解運(yùn)算，AT=QARA，其中QA為正交 陣，Ra為上三角矩陣，取Ra的前MXM階矩陣的轉(zhuǎn)置，即S=(Rmxm)t;
[0010] 假設(shè)已知系統(tǒng)k時(shí)刻的估計(jì)狀態(tài)％和協(xié)方差陣平方根Sk，時(shí)間更新如下：
[0011]

[0016] 其中i= 1，2, 一,111,111= 2n，n為狀態(tài)向量維數(shù)；Xlik為容積點(diǎn)集； 維單位列向量e= [1，0,…，0]τ，使用符號(hào)[1]表示對(duì)e的元素進(jìn)行全排列和改變?cè)胤?hào) 產(chǎn)生的點(diǎn)集，稱(chēng)為完整全對(duì)稱(chēng)點(diǎn)集，[1]1表示點(diǎn)集[1]中的第i個(gè)點(diǎn)；為通過(guò)狀態(tài)函 數(shù)傳遞后的容積點(diǎn)集；f( ·)為非線(xiàn)性狀態(tài)函數(shù)；4+1/?為k+Ι時(shí)刻狀態(tài)預(yù)測(cè)值；Sk+1/k為k+1 時(shí)刻預(yù)測(cè)誤差協(xié)方差陣平方根；;^+1?為k+1時(shí)刻XL+m的加權(quán)中心矩陣；為k時(shí)刻的系 統(tǒng)噪聲平方根，有
[0017] ⑶量測(cè)更新，包括以下內(nèi)容：

[0026] 其中Vk+1為實(shí)際殘差序列的協(xié)方差矩陣，估算公式如下：
[0027]
[0028] 若Ak+1> 1，表示殘差信息沒(méi)有被完全提取，要對(duì)增益矩陣Kk+1進(jìn)行修正，相關(guān)計(jì) 算如下：
[0029]
[0031] Pyy,k+l/k=Hk+ 1Pxyjk+1/k+Rk
[0032] Kk+1=Pxyik+l/k/Pyy,k+l/k
[0033] 若Ak+1彡1，表示在此時(shí)刻非線(xiàn)性系統(tǒng)是準(zhǔn)確的，不用對(duì)增益矩陣Kk+1進(jìn)行修正， 貝ljPjiy,k+1/k和Yk+1/k已求得，增益矩陣Kk+1計(jì)算如下：
[0034] Syy_k+1/k=Tria([Yk+1/kSRjJ)
[0035]
[0036] 最后計(jì)算k+1時(shí)刻狀態(tài)估計(jì)值和k+1時(shí)刻狀態(tài)誤差協(xié)方差陣平方根完成量測(cè)更 新：
[0039] 其中Xlik+1/k為容積點(diǎn)集；ylik+1/kS通過(guò)量測(cè)函數(shù)傳遞后的容積點(diǎn)集；h( ·)為非線(xiàn) 性量測(cè)函數(shù)；Ιω?為k+Ι時(shí)刻觀(guān)測(cè)預(yù)測(cè)值；Yk+1/kSk+Ι時(shí)刻ylik+1/k加權(quán)中心矩陣；Pxyik+1/ k+1時(shí)刻互相關(guān)協(xié)方差陣；xk+1/kSk+1時(shí)刻Xlik+1/k的加權(quán)中心矩陣；λk+1為k+1時(shí)刻 漸消因子；Pk+1/k為k+1時(shí)刻預(yù)測(cè)狀態(tài)誤差協(xié)方差陣；Hk+1為k+1時(shí)刻量測(cè)函數(shù)h( ·)對(duì)X的 偏導(dǎo)的雅可比矩陣；Nk+1，Mk+1，Ck+1為求解減消因子中使用的中間過(guò)程矩陣；tr( ·)為矩陣求 跡運(yùn)算；max{ ·}為求最大值運(yùn)算；殘差；JVtl，yk+1為k+1時(shí)刻量測(cè)值；P為遺忘 因子，0 <P彡1，通常取P= 0. 95 ;%+ιλ中上標(biāo)s表示未引入減消因子時(shí)的變量；Pyy,k+1/ k+1時(shí)刻量測(cè)誤差協(xié)方差陣；Kk+1為k+1時(shí)刻增益矩陣；S">1/14為k+1時(shí)刻量測(cè)誤差協(xié) 方差陣平方根；％為k+Ι時(shí)刻狀態(tài)估計(jì)值；Sk+1為k+Ι時(shí)刻狀態(tài)誤差協(xié)方差陣平方根；SRik+1 為Rk+1的平方根，有L=Wiw。
[0040] 本發(fā)明的有益效果是：
[0041] (1)減消因子引入位置調(diào)整到xk+1/k#，僅有X k+l/k、Pxy，k+1/k、Pyy，k+1/k和Kk+1等變量 的計(jì)算受到減消因子的影響，不必對(duì)容積點(diǎn)以及傳遞后的容積點(diǎn)集等變量重復(fù)求解，減少 了重復(fù)執(zhí)行步驟，降低了時(shí)間復(fù)雜度，提高了算法效率。由于計(jì)算傳遞后的容積點(diǎn)集步驟的 時(shí)間復(fù)雜度與狀態(tài)向量維數(shù)和量測(cè)函數(shù)h( ·)復(fù)雜程度密切相關(guān)，對(duì)于系統(tǒng)狀態(tài)維數(shù)越高， h( ·)越復(fù)雜的系統(tǒng)，時(shí)間復(fù)雜度的優(yōu)化效果越明顯；
[0042] (2)算法精度與改進(jìn)前相當(dāng)，并沒(méi)有因?yàn)榻档退惴〞r(shí)間復(fù)雜度而影響算法精度。
【附圖說(shuō)明】
[0043] 圖1是SCKF狀態(tài)估計(jì)與估計(jì)誤差不意圖；
[0044] 圖2是STSCKF狀態(tài)估計(jì)與估計(jì)誤差示意圖；
[0045] 圖3是ISTSCKF狀態(tài)估計(jì)與估計(jì)誤差示意圖。
【具體實(shí)施方式】
[0046] 下面結(jié)合附圖和某輪船推進(jìn)系統(tǒng)的實(shí)施例對(duì)本發(fā)明進(jìn)一步說(shuō)明，本發(fā)明包括但不 僅限于下述實(shí)施例。
[0047] 本發(fā)明的實(shí)現(xiàn)步驟如下：
[0048] 考慮如下離散時(shí)間非線(xiàn)性動(dòng)態(tài)系統(tǒng)：
[0049] xk+1=fk(xk)+wk
[0050] yk+i=hk+1 (xk+1)+vk+1
[0051] 其中xkeRn為系統(tǒng)狀態(tài)向量，yk+le為量測(cè)向量；_/；(□和/W(□分別為非線(xiàn)性 系統(tǒng)的狀態(tài)函數(shù)和量測(cè)函數(shù);wkeRn為系統(tǒng)噪聲，Vk+1eRm為量測(cè)噪聲，二者均為高斯白噪 聲，互不相關(guān)，且協(xié)方差分別為Q和R。
[0052] 基于以上非線(xiàn)性系統(tǒng)的ISTSCKF算法具體流程如下：
[0053] 1)設(shè)定初始參數(shù)
[0054] 設(shè)定初始時(shí)刻系統(tǒng)狀態(tài)值X。，初始時(shí)刻系統(tǒng)狀態(tài)協(xié)方差平方根S。，系統(tǒng)噪聲協(xié)方 差Q，觀(guān)測(cè)噪聲協(xié)方差R，遺忘因子p。
[0055] 2)時(shí)間更新
[0056] 假設(shè)已知系統(tǒng)k時(shí)刻的估計(jì)狀態(tài)％和協(xié)方差陣平方根Sk。
[0057]①選取容積點(diǎn)Xlik(i= 1，2, · · ·，m)
[0058]
[0059] 其中m=2η，η為狀態(tài)向量維數(shù)。
[0060] ②計(jì)算傳遞后容積點(diǎn)μ );
[0062] ④計(jì)算k+1時(shí)刻預(yù)測(cè)誤差協(xié)方差陣平方根Sk+1/k
[0063]
[0064]
為k時(shí)刻的 系統(tǒng)噪聲平方根，有α= 。
[0065] 3)量測(cè)更新
[0066]①計(jì)算容積點(diǎn) = G=U