未知測量噪聲方差的平方根高階容積卡爾曼濾波方法
【專利摘要】本發明屬于非線性系統的濾波領域，特別涉及一種處理含有未知測量噪聲方差的平方根高階容積卡爾曼濾波方法。該方法用于處理未知測量噪聲方差的平方根高階容積Kalman濾波，以HCKF為基礎，首先利用QR分解，Cholesky因子更新和高效最小二乘法等矩陣分解技術，提高了濾波方法的運行效率和數值穩定性。并在SHCKF基礎上，通過采用Sage-Husa估計器實時估算量測噪聲的統計特性，有效解決了測量噪聲方差位置的非線性系統濾波問題。
【專利說明】未知測量噪聲方差的平方根高階容積卡爾曼濾波方法
【技術領域】
[0001] 本發明屬于非線性系統的濾波領域，特別涉及一種處理含有未知測量噪聲方差的 平方根高階容積卡爾曼濾波方法。
【背景技術】
[0002] 在諸多領域中，系統狀態的動態估計問題一直是人們關注的焦點。線性高斯系統 的狀態估計問題，一般采用Kalman濾波方法。但是在處理實際問題時，比如目標跟蹤、導航 定位以及視頻監控等，系統的狀態方程或測量方程通常表現出較強的非線性特征。因此，研 究非線性系統的狀態估計即非線性濾波問題具有重要的理論意義和實際應用價值。擴展 Kalman濾波（EKF)是一種最直接、最簡單的非線性濾波方法。EKF方法通過對非線性函數 的泰勒轉換開始進行一階線性化截斷，將非線性問題轉化為線性問題，然后利用Kalman濾 波方法進行處理。對于一般的非線性系統而言，EKF不能保證其收斂，狀態估計的誤差較大。 無跡Kalman濾波是一類用采樣策略逼近非線性分布的方法。其核心是通過一種非線性變 換--U變化來進行非線性模型的狀態與誤差協方差的遞推和更新。與EKF不同。UKF不 是對非線性模型做近似，而是對狀態的概率密度函數做近似，因而能獲得比EKF更高的估 計精度。為了解決UKF應對高維狀態效果不理想的問題，有人提出了 3階容積Kalman濾波 (CKF)。該方法利用球形積分準則和徑向積分準則優化了 UKF中的sigma點采樣策略和權 重分配，改善了濾波性能。為了進一步估計精度，又有人提出了高階容積Kalman濾波方法， 它能獲得與粒子濾波相當的性能，但在計算開銷上小于后者。然而，標準的UKF、CKF和HCKF 算法由于數值計算舍入誤差、可觀測性弱（初值誤差較大）和觀測噪聲大等因素影響，可能 引起誤差協方差矩陣負定，而導致濾波器不穩定，甚至不能工作。為此，眾多學者通過采用 誤差協方差陣的平方根代替協方差陣參加遞推運算，提出一系列平方根UKF和平方根CK算 法，較好地解決了濾波器不穩定問題。
[0003] 然而，上述非線性濾波應用的一個先決條件是己知噪聲的統計特性，由于實際系 統噪聲統計特性往往具有不確定性，這就會導致濾波性能下降。對實際應用系統而言，量測 噪聲方差總是時變未知的，這是因為量測系統受到內外部各種因素的干擾，包括測量誤差 和環境擾動。也就是說，噪聲統計特性未知或者知道的不確切，此時需要在濾波過程中進行 估計。
【發明內容】
[0004] 針對上述的這些問題，一種以處理未知測量噪聲方差的平方根高階容積Kalman 濾波方法應運而生，該方法以HCKF為基礎，首先利用QR分解，Cholesky因子更新和高效最 小二乘法等矩陣分解技術，提高了濾波算法的運行效率和數值穩定性。接著，在SHCKF基礎 上，通過采用Sage-Husa估計器實時估算量測噪聲的統計特性，有效解決了測量噪聲方差 位置的非線性系統濾波問題。
[0005] 本發明是shckf的改進形式，包括計算預測狀態估計值.?(Αμ-ι)以及計算預測 誤差方差陣的平方根s (k I k-i)、計算測量與測量
【權利要求】
1.未知測量噪聲方差的平方根高階容積卡爾曼濾波方法，其特征在于： 設非線性動態系統的狀態空間模型為： x (k+1) = f (x (k)) +w (k) z (k) = h (x (k)) +v (k) 上式中，x(k) e Rn為目標狀態，z(k) e Rm表示測量值；f:Rn -Rn為非線性狀態演化 過程，h:Rn -Rm為相應的非線性測量映射；過程噪聲w(k) e Rn是均值為零的高斯白噪聲， 其方差為Q(k);測量噪聲v(k) eRm是均值為零的高斯白噪聲，但方差R(k)時變未知； 假設系統模型中過程噪聲與測量噪聲互不相關；系統的初始狀態x (〇)均值為X〇,方差 為P。，且獨立于w(k)和v(k); 基于上述系統模型，該方法具體步驟是： 步驟1設置濾波初始條件：
步驟2預測過程； 1)計算高階容積點 x： (/c - 1 ! /c - 1) = S(k -\ \ k - \ )c： ^ x(k - \ i /c - 1) (1) 其中，S(k-1 |k-l)為方差P(k-1 |k-l)的Cholesky分解因子，點集H J由下式確定：
其中，ei為n階單位向量，且其第i個元素為1 ;點集丨<丨和K丨由下式給出：
而相應的權值系數為：
2) 計算傳播后的容積點 X； {k | A： -1) = fix-X 'k -11 ^ - 〇) ⑵ 3) 狀態預測估計值
(3) 4) 計算預測誤差方差陣的平方根
(4) 其中，qr ( ?)表示對矩陣進行QR分解，
I示矩陣Q(k_l)的Cholesky分解因 子； 步驟3更新過程 1) 計算容積點
2) 計算傳播后的容積點 Zi(k|k-1) = h(Xi(k|k-l)) (6) 3) 測量預測 (7) 1=0
4) 計算互協方差陣與增益陣
(8) 5) 在設計狀態估計算法時需要在線估計v(k)的方差辦幻
(9)
d(k) = (i-bV(i-bk+1)，b為遺忘因子，b用來限制濾波器的記憶長度； ?(/f) = z(/f)-/辦(/f|卜1))為測量新息，H(k，k-1)為線性化后的觀測矩陣； 即：
(10) 4) 計算新息協方差陣的平方根
(11) 5) 計算增益陣K(k)
(12) 6) 估計狀態及其協方差陣的平方根
(13)。
【文檔編號】H03H21/00GK104283529SQ201410514913
【公開日】2015年1月14日 申請日期:2014年9月29日 優先權日:2014年9月29日
【發明者】管冰蕾, 黃晶, 包蕾, 駱再飛, 湯顯峰 申請人:寧波工程學院