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

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

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

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

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