基于非線性未知隨機偏差的兩階段容積卡爾曼濾波方法
【專利摘要】本發(fā)明涉及一種基于非線性未知隨機偏差的二階段容積卡爾曼濾波方法。本發(fā)明對于不含未知偏差的部分，算出一步預測值、一步預測方差、一步預測協(xié)方差。從而算出增益陣、估計值、估計誤差方差。對于含未知偏差的部分，算出一步偏差估計、一步偏差估計方差、一步偏差估計協(xié)方差。再算出偏差增益陣、偏差對于狀態(tài)變量的一步增益陣和估計增益陣。通過兩個部分算出來的值，算出含未知偏差非線性系統(tǒng)的一步預測值，一步預測誤差方差，估計值和估計誤差方差。在完成濾波的同時，避免了對計算機來說大的計算量和維度災難。
【專利說明】基于非線性未知隨機偏差的兩階段容積卡爾曼濾波方法

【技術領域】
[0001] 本發(fā)明屬于非線性系統(tǒng)的濾波領域，特別涉及一種處理含有未知隨機變量非線性 系統(tǒng)的兩階容積卡爾曼濾波方法。

【背景技術】
[0002] 非線性濾波是信號處理、目標跟蹤和控制領域方面的熱門話題之一，特別是，在卡 爾曼濾波框架下的非線性濾波研究仍然是一個很火的問題，在近年來受到了越來越多的關 注和研究。。
[0003] 卡爾曼濾波器"為線性高斯系統(tǒng)中最小均方差下的位置狀態(tài)"提供了有效的解決 方法。然而真實世界中，系統(tǒng)總是受到非線性和非高斯兩方面的困擾。想要獲得關于狀態(tài)估 計良好的辦法是很難的。擴展卡爾曼于是受到了廣泛的使用，無蹤卡爾曼是另外一種用樣 本來解決非線性分布的方法。不幸的是上述提到的非線性濾波器都有發(fā)散和維度災難的問 題。一個更為精確的容積卡爾曼應運而生。容積卡爾曼濾波的核心是球型射線容積法則。 它可以用來解決高斯非線性濾波問題，而不需要很大的計算量。
[0004] 標準的求容積卡爾曼濾波器要求精確的系統(tǒng)模型和精確的隨機指標信息，在實際 中模型通常包括未知定量和未知偏差。在處理含有位置偏差的動態(tài)系統(tǒng)的狀態(tài)估計時，將 偏差視為一部分系統(tǒng)狀態(tài)，并且像系統(tǒng)一樣估計偏差。這就引出了增廣狀態(tài)卡爾曼濾波，它 的應用卻也存在相對大的計算量的問題。


【發(fā)明內容】

[0005] 本發(fā)明涉及一種處理還有未知偏差非線性系統(tǒng)的兩階段容積卡爾曼濾波器。該方 法將含有位置偏差的非線性系統(tǒng)看作不含位置偏差估計和含有位置偏差估計兩部分。從而 在獲得良好非線性濾波的情況下，避免了增廣卡爾曼濾波所帶來的計算量大的問題。對于 不含未知偏差的部分，算出一步預測值、一步預測方差、一步預測協(xié)方差。從而算出增益陣、 估計值、估計誤差方差。對于含未知偏差的郃分，算出一步偏差估計、一步偏差估計方差、一 步偏差估計協(xié)方差。再算出偏差增益陣、偏差對于狀態(tài)變量的一步增益陣和估計增益陣。 通過兩個部分算出來的值，算出含未知偏差非線性系統(tǒng)的一步預測值，一步預測誤差方差， 估計值和估計誤差方差。在完成濾波的同時，避免了對計算機來說大的計算量和維度災難。 本發(fā)明的具體步驟如下：
[0006] 步驟1.計算不含有未知偏差部分的無偏差估計一步預測，一步預測誤差方 差Pk'k-iP:，k\k-i 一步預測誤差協(xié)方差峨-1。
[0007] 步驟2.計算不含有未知偏差部分的無偏差估計增益陣估計值毛|和估計誤差 方差陣Pk| k。
[0008] 步驟3.計算含有未知偏差部分的，未知偏差一步預測buH偏差一步預測方差 偏差預測誤差協(xié)方差雄-丨，和< 以及噴。
[0009] 步驟4.計算含有未知偏差部分的，偏差增益陣I〗和偏差估計誤差方差陣以 及偏差一步增益UkiH和偏差增益。
[0010] 步驟5.根據(jù)以上四步計算未知偏差非線性系統(tǒng)的一步預測方差和一步預測 誤差協(xié)方差/和最終目標狀態(tài)的最優(yōu)線性估計及其誤差協(xié)方差。
[0011] 本發(fā)明有益效果：使用兩階段容積卡爾曼方法不僅能有效處理還有未知偏差的非 線性系統(tǒng)，還有效避免了增廣狀態(tài)卡爾曼在處理同類問題時所產生的維度災難和計算機計 算量過大的問題。

【專利附圖】

【附圖說明】
[0012] 圖1為兩階段容積卡爾曼濾波的流程圖。

【具體實施方式】
[0013] 下面首先為跟蹤目標的運動狀態(tài)建立模型，其次給出兩階段容積強濾波器的濾波 公式，將其分為無偏差估計和有偏差估計兩個步驟，下面結合圖1詳細介紹本發(fā)明的實施 過程。
[0014] 1系統(tǒng)建模
[0015] 1. 1給出如下非線性系統(tǒng)動態(tài)模型
[0016] xk = fk-iUk-i)+^Kk-i
[0017] zk = hk (xk) +vk
[0018] 其中k彡1是時刻指數(shù)，xke RnX1表示系統(tǒng)狀態(tài)（RnX1為nXl維列向量全集）， zkeRnX1是測量值列向量，f^J·)以及hk(·)都是可微函數(shù)。初始狀態(tài)&服從元為均值， PQ為方差且獨立于Wk和vk隨機變量。wk e RnX1和vk e ΓΧ1都是均值為零的高斯白噪聲， 其中￡[uA u') ] = ￡[νΑ V) ] = ￡[uA W ] = 0，δ。是克羅內克脈沖函數(shù)。
[0019] 在容積卡爾曼中，η維隨機變量xk以毛為均值Pk為方差的隨機變量，它可以被容 積點近似為尤…=ι;?-名+ W4其中權重值Wi為W ^
[0020] 當非線性系統(tǒng)信息不完全時，離散兩階容積卡爾曼濾波的分析方程如下：
[0021]

【權利要求】
1.基于非線性未知隨機偏差的二階段容積卡爾曼濾波方法，其特征在于： 系統(tǒng)建模，給出如下非線性系統(tǒng)動態(tài)模型 Xk - fk-1 (Xk-1)+Wk，k-1zk-hk (xk) +Vk 其中k彡I是時刻指數(shù)，xkeRnxi表示系統(tǒng)狀態(tài)，Rnxi為nxI維列向量全集，ZkeRnxi 是測量值列向量，fH( ·)以及hk( ·)都是可微函數(shù)；初始狀態(tài)Xtl服從元為均值，Ptl為方 差且獨立于wk和Vk隨機變量;WkeRnxi和VkeRmxi都是均值為零的高斯白噪聲，其中 可".'，"'(]=0為可％ /] = /^;￡|>乂] = 〇，6。是克羅內克脈沖函數(shù)； 在容積卡爾曼中，η維隨機變量Xk以毛為均值Pk為方差的隨機變量，它可以被容積點 近似為叉 _C￡ ,c.其中權重值Wi為冰,.=丄(i=l,2,…"?;;》= 2?); 當非線性系統(tǒng)信息不完全時，離散兩階容積卡爾曼濾波的分析方程如下：
其中和bk為兩階容積卡爾曼的狀態(tài)向量，修正無偏差估計濾波器和偏差部分分 別如下：其中下標kIk表示通過前k時刻的觀測值所得到的狀態(tài)估計值，同理kIk-i表示的 是根據(jù)前k-Ι個時刻的觀測值得到k時刻的狀態(tài)估計值； 修正無偏差部分： 步驟1.計算不含有未知偏差部分的無偏差估計一步預測，一步預測誤差方差 Pk\k-lP:，k'k-.l一步預測誤差協(xié)方差一1 1) .計算容積點一步遞歸Xi，k|H Xi，k|k-1 - f (Xi，k-l|k-i)⑴ 2) .根據(jù)m個容積點的采樣來求得一步預測容積點近似估計值毛m
3) . -步預測誤差估計方差，其中G,代表高斯白噪Wlrt的方差
其中右上角標X表明是不含偏差部分的方差計算，包含'代表估計值； 4) .容積點一步觀測值遞歸 Zi，k|k-1一h (Xi,k|k-l)⑷ 5) .通過m個容積點近似一步預測觀測估計值
6) .得到新息Zk以后，根據(jù)上一步得到的一步預測觀測值，求其誤差 Im = zk-K\k-\= zkΣ丨" （6) m 7) .觀測值預測方差
8) . -步預測狀態(tài)與觀測協(xié)方差
步驟2.計算不含有未知偏差部分的無偏差估計增益陣--估計值元|4和估計誤差方差 陣Pklk 1) .根據(jù)上兩步得到一步預測增益值
其中W由MJ 2) .根據(jù)上一步的預測增益值可以得到狀態(tài)估計 -Wi+[X (10) 3) .根據(jù)（3)、（7)、（10)狀態(tài)估計誤差方差 P叫=PUlPUf (11) 偏差估計部分： 步驟3.計算含有未知偏差部分的，未知偏差一步預測bkh偏差一步預測方差A-IA-I偏差預測誤差協(xié)方差以及^#-1 1). 一步預測觀測值誤差
其中右上角標b表示的是含偏差部分，是由之后的耦合方程 < 求出的耦合系數(shù)； 2) . -步預測觀測值誤差協(xié)方差
mti' ' ................................... 3) . -步預測觀測誤差協(xié)方差
4) .狀態(tài)偏差一步預測 bk|k-i - bk-i |k-i (16) 5) .關于誤差偏差b的一步偏差預測誤差方差陣A|t-i Kh+QL (17) 其中ZtllM是上一步的估計誤差方差，是一步狀態(tài)偏差噪聲的方差值 6) .狀態(tài)偏差值迭代
其中為耦合方程At= f所求的值 7) .通過（16)式算出的一步偏差預測方差陣求得/Jt 其中為稱合方程M=[乃
所求的值 步驟4.計算含有未知偏差部分的，偏差增益陣夂〗和偏差估計誤差方差陣/Jt以及偏差 一步增益Ukllrf和偏差增益 1).計算關于狀態(tài)誤差增益系數(shù)陣
步驟5.根據(jù)以上四步計算未知偏差非線性系統(tǒng)的一步預測方差.和一步預測誤差 協(xié)方差/i:,和最終目標狀態(tài)的最優(yōu)線性估計及其誤差協(xié)方差7S;
將修正無偏差部分所算出來的和含偏差部分所求出來的 UklH,Klk+Uklk,bk|k，帶入到離散兩階容積卡爾曼濾波的分析方程中，便可以得到 由上一步狀態(tài)估計和狀態(tài)估計偏差在得到信息之后得到下一步的狀態(tài)估計和狀態(tài)估計偏 差； 所以當知道關于偏差估計部分的初始條件：
整個濾波過程便可以從第一步開始不斷估計出下一步，一直持續(xù)下去，整個濾波估計 過程得以實現(xiàn)。
【文檔編號】H03H17/02GK104270119SQ201410487371
【公開日】2015年1月7日 申請日期:2014年9月22日 優(yōu)先權日:2014年9月22日
【發(fā)明者】張露, 呂梅蕾, 王海倫, 姜春娣 申請人:衢州學院