一種自適應(yīng)高階容積卡爾曼濾波方法
【專利摘要】本發(fā)明涉及信號(hào)處理領(lǐng)域的一種自適應(yīng)高階容積卡爾曼濾波方法。本發(fā)明包括估計(jì)一步預(yù)測(cè)目標(biāo)狀態(tài)及其協(xié)方差陣，計(jì)算漸消因子并用其調(diào)整得到修正的協(xié)方差陣，計(jì)算目標(biāo)狀態(tài)的最優(yōu)線性估計(jì)及其誤差協(xié)方差，實(shí)時(shí)估計(jì)系統(tǒng)噪聲的方差。本發(fā)明提出的方法具有比無(wú)跡卡爾曼濾波和容積卡爾曼濾波更高的估計(jì)精度。同時(shí)，通過(guò)引入時(shí)變漸消因子，使得本發(fā)明的方法具有處理系統(tǒng)狀態(tài)突變的強(qiáng)跟蹤能力。此外，通過(guò)實(shí)時(shí)估計(jì)系統(tǒng)噪聲的方差，有效抑制了由于噪聲統(tǒng)計(jì)特性未知時(shí)變而導(dǎo)致的濾波誤差。
【專利說(shuō)明】一種自適應(yīng)高階容積卡爾曼濾波方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于信號(hào)處理【技術(shù)領(lǐng)域】，具體涉及一種自適應(yīng)高階容積卡爾曼濾波方法?！颈尘凹夹g(shù)】
[0002]在信號(hào)處理、目標(biāo)跟蹤和系統(tǒng)控制等諸多領(lǐng)域，系統(tǒng)狀態(tài)估值問(wèn)題一直是人們關(guān)注的焦點(diǎn)。對(duì)于線性高斯系統(tǒng)的而言，通常采用的是卡爾曼濾波(KF)方法。然而，許多實(shí)際應(yīng)用問(wèn)題中，系統(tǒng)狀態(tài)方程或測(cè)量方程往往具有較強(qiáng)的非線性特征，因而濾波估計(jì)問(wèn)題也表現(xiàn)出非線性。這種情況下系統(tǒng)估值最常用的方法是擴(kuò)展卡爾曼濾波(EKF)。這種方法先將非線性函數(shù)的泰勒展開(kāi)式進(jìn)行一階線性化截?cái)?，并假定線性化后的狀態(tài)仍服從高斯分布，最后對(duì)線性化后的系統(tǒng)采用KF獲得狀態(tài)估計(jì)。但是EKF的線性化處理在穩(wěn)定性、收斂精度、收斂時(shí)間上往往難以滿足要求。由于近似非線性函數(shù)的概率密度分布比近似非線性函數(shù)更容易，使用采樣方法近似非線性分布來(lái)解決非線性問(wèn)題的途徑得到了人們的廣泛關(guān)注，典型的代表是采用UT變換的無(wú)跡濾波(UKF)。UKF雖然能明顯改善EKF的性能，但是對(duì)高維狀態(tài)估計(jì)性能會(huì)急劇惡化，出現(xiàn)所謂的“維數(shù)災(zāi)難”現(xiàn)象。為了解決這一問(wèn)題，容積卡爾曼濾波器(CKF)，以及精度更高的高階容積卡爾曼濾波器(HCKF)被相繼提出來(lái)。 [0003]不足的是，這些非線性濾波應(yīng)用的一個(gè)先決條件是己知噪聲的統(tǒng)計(jì)特性，但由于實(shí)際系統(tǒng)噪聲統(tǒng)計(jì)特性往往具有不確定性，導(dǎo)致濾波方法失效。為了克服這個(gè)缺點(diǎn)，發(fā)展起來(lái)了一些自適應(yīng)濾波方法，如極大后驗(yàn)(MAP)估計(jì)、虛擬噪聲補(bǔ)償?shù)取Ａ硗?，?shí)際應(yīng)用中，系統(tǒng)狀態(tài)會(huì)出現(xiàn)突變現(xiàn)象，這也使得濾波性能大大降低。強(qiáng)跟蹤濾波器(STF)通過(guò)引入漸消因子自動(dòng)調(diào)節(jié)預(yù)測(cè)誤差協(xié)方差，具備了較好自適應(yīng)性能。

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

[0004]為了解決上述的問(wèn)題，本發(fā)明中，以高階容積卡爾曼濾波(HCKF)為基礎(chǔ)濾波器，采用Sage-Husa次優(yōu)無(wú)偏極大后驗(yàn)估計(jì)器實(shí)時(shí)估計(jì)未知噪聲，并結(jié)合強(qiáng)跟蹤濾波(STF)思想，得到一種自適應(yīng)的高階容積卡爾曼濾波(AHCKF)方法。
[0005]本發(fā)明是HCKF的改進(jìn)形式，包括估計(jì)一步預(yù)測(cè)目標(biāo)狀態(tài)蚪(μ-1)及其協(xié)方差陣P(kIk-D (本發(fā)明中k為離散時(shí)間標(biāo)記，kIk-1表示用k-1時(shí)刻的目標(biāo)信息估計(jì)第k時(shí)刻的目標(biāo)信息);計(jì)算漸消因子λ (k)并用其調(diào)整預(yù)測(cè)誤差協(xié)方差陣P(kIk-1);計(jì)算目標(biāo)狀態(tài)的最優(yōu)線性估計(jì).?(/?\k) (k|k表示該值即為第k時(shí)刻的最優(yōu)估計(jì)值)及其誤差協(xié)方差P (k I k)，
實(shí)時(shí)估計(jì)系統(tǒng)噪聲的方差?(Α)。具體內(nèi)容如下:
[0006]步驟I 設(shè)置濾波初始條件:?(ο I O) = Λ:(ο) ρ(ο|ο)=ρ(ο)，?(ο) = ρ(ο)。
[0007]步驟2時(shí)間更新，具體包括:
[0008](2.1)估計(jì)一步預(yù)測(cè)目標(biāo)狀態(tài)1)及其協(xié)方差陣p(k|k-l)；
[0009](2.2)估算漸消因子λ (k)，并利用λ (k)對(duì)(2.1)中的p(k|k-l)進(jìn)行調(diào)整，得到修正后的協(xié)方差陣Pe(k|k-1)。
[0010]步驟3量測(cè)更新，具體包括:
[0011](3.1)計(jì)算測(cè)量值的預(yù)測(cè)估計(jì)值Z(Ap-1);
[0012](3.2)計(jì)算協(xié)方差矩陣Pxz (k |k)和Pzz (k |k)(下標(biāo)XZ表示狀態(tài)和測(cè)量值的互協(xié)方差，而Zz則表示測(cè)量值的自協(xié)方差)，以及第k時(shí)刻的增益陣K (k)；
[0013](3.3)計(jì)算第k時(shí)刻的目標(biāo)狀態(tài)最優(yōu)估計(jì)及其誤差協(xié)方差μ)，p(k|k)。
[0014]步驟4遞推估計(jì)系統(tǒng)噪聲的方差么幻。
[0015]步驟5判斷濾波算法是否繼續(xù)執(zhí)行，如果是，返回步驟2 ;否則，結(jié)束算法。
[0016]本發(fā)明有益效果:首先，基礎(chǔ)濾波器HCKF具有比UKF和CKF更高的估計(jì)精度。同時(shí)，通過(guò)引入時(shí)變漸消因子，使得AHCKF具有處理系統(tǒng)狀態(tài)突變的強(qiáng)跟蹤能力。此外，通過(guò)實(shí)時(shí)估計(jì)系統(tǒng)噪聲的方差，有效抑制了由于噪聲統(tǒng)計(jì)特性未知時(shí)變而導(dǎo)致的濾波誤差。
【專利附圖】

【附圖說(shuō)明】[0017]圖1為AHCKF流程圖。
【具體實(shí)施方式】
[0018]下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明做進(jìn)一步說(shuō)明。
[0019]參照?qǐng)D1，設(shè)非線性動(dòng)態(tài)系統(tǒng)的狀態(tài)空間模型為:
[0020]x(k+l)=f (x (k))+w(k)
[0021]z (k) =h (x (k)) +V (k)
[0022]其中，X(k) e Rn表示系統(tǒng)狀態(tài)(Rn為n維列向量全集)，z (k) e Rni是測(cè)量向量，f(x(k))和h(x(k))都是可微函數(shù)，w (k) e RlPv(Ii) e Rm都是均值為零的聞斯白噪聲,其方差分別為Q(k)和R(k)，且系統(tǒng)噪聲方差Q(k)是時(shí)變未知的。
[0023]假設(shè)系統(tǒng)的初始狀態(tài)為:x(0 I O) = X(O)，P (O I O) =p (O)，且X (O)分別獨(dú)立于w (k)和
V (k)。
[0024]下面，基于系統(tǒng)模型，詳述AHCKF的具體實(shí)施步驟:
[0025]步驟I 設(shè)置濾波初始條件:i(0 I O) = x(0)，P (O I O) =p (O)，Q(O) = Q(Q)。
[0026]步驟2時(shí)間更新
[0027]I)計(jì)算高階容積點(diǎn)(i=0, I,…，2n2)
[0028]xi{k-l\k-1) 二 S[l< - ?μ -1)A + x{k-\\k-1)(I)
[0029]其中，S(k_l |k_l)為方差P (k_l | k_l)的Cholesky分解因子,點(diǎn)集{ ξ J由下式確定:
【權(quán)利要求】
1.一種自適應(yīng)高階容積卡爾曼濾波方法，其特征在于該方法包括以下步驟:
步驟 I 設(shè)置濾波初始條件:x(0 I O) = JC(O) , P(0|0) =P (O)，Q(Q) = Q(O)； 步驟2時(shí)間更新，具體包括: (2.1)估計(jì)一步預(yù)測(cè)目標(biāo)狀態(tài)對(duì)MA-1)及其誤差協(xié)方差陣P(k|k-1)； (2.2)估算漸消因子λ(k)，并利用λ (k)對(duì)誤差協(xié)方差陣P(k|k-1)進(jìn)行調(diào)整，得到修正后的協(xié)方差陣Pe(k |k-l)； 步驟3量測(cè)更新，具體包括: (3.1)計(jì)算測(cè)量值的預(yù)測(cè)估計(jì)值￡(々| Ar -1); (3.2)計(jì)算協(xié)方差矩陣Pxz(k |k)和Pzz (k |k)以及第k時(shí)刻的增益陣K (k)，其中下標(biāo)Xz表示狀態(tài)和測(cè)量值的互協(xié)方差，而ZZ則表示測(cè)量值的自協(xié)方差； (3.3)計(jì)算第k時(shí)刻的目標(biāo)狀態(tài)最優(yōu)估計(jì)対及其誤差協(xié)方差P(k|k)； 步驟4遞推估計(jì)系統(tǒng)噪聲的方差如' 步驟5判斷濾波是否繼續(xù)執(zhí)行，如果是，返回步驟2 ;否則，結(jié)束。
2.根據(jù)權(quán)利要求1所述的一種自適應(yīng)高階容積卡爾曼濾波方法，其特征在于:所述的漸消因子λ (k)表達(dá)式為:
【文檔編號(hào)】G06F19/00GK103927436SQ201410134666
【公開(kāi)日】2014年7月16日 申請(qǐng)日期:2014年4月4日 優(yōu)先權(quán)日:2014年4月4日
【發(fā)明者】史岳鵬 申請(qǐng)人:鄭州牧業(yè)工程高等專科學(xué)校