專利名稱：迭代平方根中心差分卡爾曼粒子濾波方法
技術領域：
本發明涉及一種卡爾曼粒子濾波方法，特別是迭代平方根中心差分卡爾曼粒子濾波方法。
背景技術：
文獻“閃爍噪聲環境下基于粒子濾波的目標跟蹤，聲學技術，2009，Vol.28(4)，p29-32”公開了一種粒子濾波方法Sigma粒子濾波SPPF。該方法是利用平方根無跡卡爾曼濾波(SRUKF)來更新粒子均值和方差
Pki，從而用高斯近似的方法作為建議分布密度函數
其中N(·)表示高斯函數。該方法雖然可以取得較快的濾波收斂性和較高的濾波精度，但是需要說明的是，在非線性、非高斯系統的目標跟蹤中，該方法存在跟蹤精度較低，均值為0.4689、方差為0.1664、效率較低、穩定性較差等問題。
發明內容
為了克服現有的卡爾曼濾波方法跟蹤精度低的不足，本發明提供一種迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF)。該方法利用迭代平方根中心差分卡爾曼濾波方法(ISRCDKF)來產生建議分布，可以使其更好的逼近后驗概率密度，達到很高的跟蹤精度，同時具有很好的穩定性。
本發明解決其技術問題所采用的技術方案一種迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF)，其特點是包括下述步驟 (a)根據非線性、非高斯系統目標跟蹤的特點，采用Levenberg-Marquardt優化方法，修正狀態協方差； 系統方程 量測方程 式中，f和h為系統狀態和量測的非線性變換，xk，yk分別是系統k時刻的狀態值和量測值，wk，vk分別是系統k時刻的狀態噪聲和測量噪聲； 對基于線性化方法的卡爾曼濾波，定義代價函數C(xk)為 (b)利用迭代的平方根CDKF和步驟(a)共同來產生建議分布，建立迭代平方根中心差分卡爾曼粒子濾波方法； 1)初始值 2)構造2L+1維Sigma點 3)時間更新 χk|k-1＝f(χk-1) (6) 4)測量更新 for i＝1m yk|k-1＝g(χk|k-1) (13) end 式中，Rw為狀態過程噪聲協方差矩陣，Rv為測量噪聲協方差矩陣；h≥1為中心差分區間長度，w1＝1/(4h2)和w2＝(h2-1)/(4h4)，L是狀態向量的維數，Levenberg-Marquardt參數μi的選為0.1； 迭代的平方根CDKF粒子濾波方法(ISPPF)， 1)當k＝0時，采樣粒子
i＝1，...，N。
2)重要性采樣 將初始粒子代入迭代的平方根CDKF中產生重要性函數分布，并采樣粒子 3)重要性權值 當i＝2，...，N時， 歸一化權值 4)重采樣 從xki中根據重要性權值重新采樣得到新的N個粒子
并重新分配權值 本發明的有益效果是由于利用迭代的平方根CDKF和引入Levenberg-Marquardt優化方法共同來產生建議分布，粒子濾波(PF)、迭代擴展卡爾曼粒子濾波(IEKFPF)、Sigma粒子濾波SPPF、迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF)粒子數均為100，迭代平方根中心差分卡爾曼粒子濾波方法可以使跟蹤精度提高至少30多倍，均值為0.0147，方差為0.00000394，還具有很好的穩定性。
下面結合附圖和實施例對本發明作詳細說明。
圖1是利用本發明方法對目標狀態估計的曲線圖。
圖2是利用本發明方法進行100次蒙特卡羅仿真的RMSE曲線圖。
具體實施例方式 以下詳細說明迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF) (a)根據非線性、非高斯系統目標跟蹤的特點，引入Levenberg-Marquardt優化方法，修正狀態協方差； 系統方程 量測方程 其中f和h為系統狀態和量測的非線性變換，xk，yk分別是系統k時刻的狀態值和量測值，wk，vk分別是系統k時刻的狀態噪聲和測量噪聲。
對基于線性化方法的卡爾曼濾波，定義代價函數C(xk)為 觀測更新的迭代過程可看作是利用Gauss-Newton方法迭代求解函數C(xk)的極小值點。然而，由于線性化等誤差的引入，狀態空間模型與觀測數據未必完全一致，因此只能求出函數C(xk)的非零極值點，這等價于將Gauss-Newton方法應用于小殘差問題的迭代優化。對于這類問題，Gauss-Newton方法往往性能不穩定，主要表現為對狀態的觀測更新不能保障估計誤差的一致減少，對協方差陣的估計值比真實值偏低，進而影響對觀測信息的有效利用。為了增強算法的穩定性，本發明利用Levenberg-Marquardt方法調整預測協方差陣，以保證算法具有全局收斂性。該方法的核心是在每次迭代過程中，使用參數μi對預測協方差陣進行修正，即調整協方差陣為
然后，以修正的協方差陣
進行迭代觀測更新。
(b)利用迭代的平方根CDKF和步驟(a)共同來產生建議分布，建立迭代平方根中心差分卡爾曼粒子濾波方法； 基于Levenberg-Marquardt優化方法的迭代SRCDKF濾波方法(ISRCDKF)，具體步驟如下 1)初始值 2)構造2L+1維Sigma點 3)時間更新 χk|k-1＝f(χk-1) (6) 4)測量更新 for i＝1m yk|k-1＝g(χk|k-1) (13) end 其中Rw為狀態過程噪聲協方差矩陣，Rv為測量噪聲協方差矩陣。h≥1為中心差分區間長度，w1＝1/(4h2)和w2＝(h2-1)/(4h4)，L是狀態向量的維數，Levenberg-Marquardt參數μi的選為0.1。
由以上推導可知，迭代的平方根CDKF能得到狀態的最大后驗概率估計，其性能要優于傳統的UKF、IEKF。所以用迭代的平方根CDKF產生建議分布，將更符合狀態變量的實際后驗概率分布。
于是迭代的平方根CDKF粒子濾波方法(ISPPF)，具體步驟如下 1)當k＝0時，采樣粒子
i＝1，...，N。
2)重要性采樣 將初始粒子代入迭代的平方根CDKF中產生重要性函數分布，并采樣粒子 3)重要性權值 當i＝2，...，N時， 歸一化權值 4)重采樣 從xki中根據重要性權值重新采樣得到新的N個粒子
并重新分配權值 (c)在非線性、非高斯模型中，運用迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF)進行實驗，給出該方法與粒子濾波(PF)、迭代擴展卡爾曼粒子濾波(IEKFPF)、Sigma粒子濾波SPPF方法的仿真結果，分析其跟蹤性能和均方根誤差。
考慮一個非線性、非高斯模型 xk＝1+sin(0.4πk)+0.5xk-1+vk-1 (25) 這里vk服從Gamma(3，2)分布，觀測噪聲nk服從高斯分布N(0，10-5)，目標的初始狀態x0＝1，經過100次蒙特卡羅仿真，每次仿真時間是60s，采樣間隔為1s。1次獨立實驗的均方根誤差定義為 其中粒子數均為100。不同濾波方法對目標狀態的估計曲線圖如圖1所示。100次蒙特卡羅仿真的RMSE曲線圖如圖2所示。
從圖1、圖2可以看出，在非線性、非高斯環境中，由于PF中沒有包含最新的測量信息，所以它受噪聲的干擾比較大，算法不穩定，跟蹤性能不好。SPPF生成的建議分布中有效地利用了測量信息，具有較高的精確度。而本發明中提出的ISPPF由迭代的平方根CDKF和步驟(a)共同來產生建議分布，它比其它的方法更能有效的利用測量信息。表1給出了重復進行100次蒙特卡羅實驗得到的ISPPF與擴展卡爾曼濾波EKF、無跡卡爾曼濾波UKF、粒子濾波PF、Sigma粒子濾波SPPF性能對比統計結果。可以看出它們對狀態估計的均方根誤差(RMSE)均值和方差的變化。顯然，粒子濾波方法(ISPPF)具有很好的穩定性，是一種效率高、性能好的跟蹤方法。
表1 100次蒙特卡羅仿真的RMSE的均值和方差
權利要求
1.一種迭代平方根中心差分卡爾曼粒子濾波方法，其特征在于包括下述步驟
(a)根據非線性、非高斯系統目標跟蹤的特點，采用Levenberg-Marquardt優化方法，修正狀態協方差；
系統方程
量測方程
式中，f和h為系統狀態和量測的非線性變換，xk，yk分別是系統k時刻的狀態值和量測值，wk，vk分別是系統k時刻的狀態噪聲和測量噪聲；
對基于線性化方法的卡爾曼濾波，定義代價函數C(xk)為
(b)利用迭代的平方根CDKF和步驟(a)共同來產生建議分布，建立迭代平方根中心差分卡爾曼粒子濾波方法；
1)初始值
2)構造2L+1維Sigma點
3)時間更新
χk|k-1＝f(χk-1)(6)
4)測量更新
for i＝1:m
uk|k-1＝g(χk|k-1) (13)
end
式中，Rw為狀態過程噪聲協方差矩陣，Rv為測量噪聲協方差矩陣；h≥1為中心差分區間長度，w1＝1/(4h2)和w2＝(h2-1)/(4h4)，L是狀態向量的維數，Levenberg-Marquardt參數μi的選為0.1；
迭代的平方根CDKF粒子濾波方法(ISPPF)，
1)當k＝0時，采樣粒子
i＝1，...，N。
2)重要性采樣
將初始粒子代入迭代的平方根CDKF中產生重要性函數分布，并采樣粒子
3)重要性權值
當i＝2，...，N時，
歸一化權值
4)重采樣
從xki中根據重要性權值重新采樣得到新的N個粒子
并重新分配權值
全文摘要
本發明公開了一種迭代平方根中心差分卡爾曼粒子濾波方法，其目的是解決現有的卡爾曼濾波方法跟蹤精度低的技術問題。技術方案是根據非線性、非高斯系統目標跟蹤的特點，引入Levenberg-Marquardt優化方法，修正狀態協方差；利用迭代的平方根CDKF和所引入Levenberg-Marquardt優化方法共同來產生建議分布，建立迭代平方根中心差分卡爾曼粒子濾波方法。由于利用迭代的平方根CDKF和引入Levenberg-Marquardt優化方法共同來產生建議分布，粒子濾波(PF)、迭代擴展卡爾曼粒子濾波(IEKFPF)、Sigma粒子濾波SPPF、迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF)粒子數均為100，迭代平方根中心差分卡爾曼粒子濾波方法可以使跟蹤精度提高至少30多倍，均值為0.0147，方差為0.00000394，還具有很好的穩定性。
文檔編號H03H21/00GK101820269SQ20101014227
公開日2010年9月1日 申請日期2010年4月8日 優先權日2010年4月8日
發明者李國輝, 李亞安, 何健, 楊宏, 崔琳 申請人:西北工業大學