模糊高斯和粒子濾波方法、裝置及目標(biāo)跟蹤方法、裝置的制造方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及非線性濾波領(lǐng)域，特別是涉及一種模糊高斯和粒子濾波方法、裝置及 目標(biāo)跟蹤方法、裝置。
【背景技術(shù)】
[0002] 在飛機、航空飛行器、車輛等目標(biāo)的運動過程中，常常需要對目標(biāo)的實時狀態(tài)進(jìn)行 估計以實現(xiàn)對目標(biāo)的跟蹤，飛機等目標(biāo)的運動系統(tǒng)模型一般屬于非線性隨機系統(tǒng)。非線性 濾波技術(shù)為非線性隨機系統(tǒng)中進(jìn)行狀態(tài)估計的常用手段。
[0003] 根據(jù)應(yīng)用背景的不同，現(xiàn)有技術(shù)非線性濾波技術(shù)主要分為兩類：第一類是針對非 線性高斯環(huán)境下的狀態(tài)估計問題，如擴展卡爾曼濾波器（EKF)、無跡卡爾曼濾波器（UKF)、 積分卡爾曼濾波（QKF)、截斷無跡卡爾曼濾波（IUKF)，這類方法主要是利用泰勒級數(shù)展開 或數(shù)值計算等線性近似技術(shù)對非線性的系統(tǒng)模型進(jìn)行近似，忽略近似高階項對濾波性能的 影響。第二類是針對非線性非高斯環(huán)境下的狀態(tài)估計問題，如高斯和濾波器（GSF)、高斯和 積分卡爾曼濾波器（GS-QKF)，這類高斯和方法主要是利用多個混合高斯將狀態(tài)的后驗概率 密度函數(shù)近似成單個高斯函數(shù)，然而，與上述EKF等方法類似，這類高斯和方法都必須進(jìn)行 線性化，對于強非線性非高斯系統(tǒng)，此類濾波器的濾波精度并不高，且濾波器的高斯混合項 的數(shù)量隨著時間快速增長。
[0004] 此外，為了對非線性非高斯噪聲進(jìn)行處理，現(xiàn)有技術(shù)還采用另一種非線性濾波方 法：高斯和粒子濾波方法，對于大規(guī)模被動傳感器系統(tǒng)中觀測信息有限、數(shù)據(jù)丟失率高、具 有非周期、非線性、非高斯特征的觀測數(shù)據(jù)，現(xiàn)有技術(shù)所采用的一類高斯和粒子濾波方法由 于在時間更新時只是簡單地采用狀態(tài)轉(zhuǎn)移函數(shù)進(jìn)行粒子采樣，粒子的多樣性以及準(zhǔn)確性較 差，粒子并不能有效表示目標(biāo)的后驗概率分布，從而降低粒子濾波的性能。

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

[0005] 本發(fā)明主要解決的技術(shù)問題是提供一種模糊高斯和粒子濾波方法、裝置及目標(biāo)跟 蹤方法、裝置，能夠增強粒子的多樣性和準(zhǔn)確性，有效提高濾波精度以及目標(biāo)狀態(tài)的估計性 能。
[0006] 為解決上述技術(shù)問題，本發(fā)明的第一方面是：提供一種模糊高斯和粒子濾波方法， 包括：利用高斯和構(gòu)建上一目標(biāo)觀測時刻的狀態(tài)后驗概率密度函數(shù)、觀測噪聲概率密度函 數(shù)以及狀態(tài)噪聲概率密度函數(shù)；根據(jù)所述上一目標(biāo)觀測時刻的狀態(tài)后驗概率密度函數(shù)和所 述狀態(tài)噪聲概率密度函數(shù)，利用高斯-厄米特積分和蒙特卡羅原理獲取當(dāng)前目標(biāo)觀測時刻 的目標(biāo)狀態(tài)預(yù)測概率密度函數(shù)；根據(jù)所述當(dāng)前目標(biāo)觀測時刻的目標(biāo)狀態(tài)預(yù)測概率密度函數(shù) 和所述觀測噪聲概率密度函數(shù)獲取狀態(tài)粒子集；利用模糊聚類原理獲取當(dāng)前目標(biāo)觀測時刻 的粒子權(quán)值和積分點權(quán)值，并計算各高斯項的權(quán)值；根據(jù)所述狀態(tài)粒子集和所述粒子權(quán)值、 所述積分點權(quán)值計算每個高斯分布的均值和協(xié)方差；根據(jù)所述高斯項的權(quán)值對高斯項進(jìn)行 重采樣，獲取G個權(quán)值較大的高斯項，G為正整數(shù)；以及根據(jù)獲取的G個高斯項，利用高斯和 原理得到當(dāng)前目標(biāo)觀測時刻的狀態(tài)后驗概率密度函數(shù)，完成粒子濾波。
[0007] 其中，所述根據(jù)上一目標(biāo)時刻的狀態(tài)后驗概率密度函數(shù)和狀態(tài)噪聲概率密度函 數(shù)，利用高斯-厄米特積分規(guī)則和蒙特卡羅原理獲取當(dāng)前時刻目標(biāo)狀態(tài)的預(yù)測概率密度函 數(shù)的步驟包括：利用高斯-厄米特積分構(gòu)建上一目標(biāo)觀測時刻的多個積分點概率密度函 數(shù)；根據(jù)所述多個積分點概率密度函數(shù)獲取所述積分點的近似粒子集；其中，所述積分點 X:物，的近似粒子集為物尤丨,~iV(欠，術(shù)^ 所述近似粒子集中每個近似粒子的預(yù)測粒子集；以及根據(jù)所述預(yù)測粒子集獲取當(dāng)前目標(biāo)觀 測時刻的目標(biāo)狀態(tài)預(yù)測概率密度函數(shù)。
[0008] 其中，所述利用高斯-厄米特積分構(gòu)建上一目標(biāo)觀測時刻的多個積分點概率密度 函數(shù)的步驟具體包括：獲取n時刻每個高斯分布的高斯-厄米特積分點的后驗概率密度函 數(shù)，具體如下式所示：
[0009](5) 占一J
[0010] 其中，n時刻表示上一目標(biāo)觀測時刻，'w為第g個高斯分布函數(shù)的均值，PWn)g 為第g個高斯分布函數(shù)的協(xié)方差；
[0011] 估計n時刻的目標(biāo)狀態(tài)后驗概率密度函數(shù)對應(yīng)的積分點，具體如下式所 示：
[0012] g ?
'^\n\n)g' (.6)..
[0013] 其中，g' =g+(k-l)K;g= 1，2，......，G;k=l，2，......，K;1 = 1，2，......，m; Ii為高斯-厄米特積分點；
[0014] 以n時刻的目標(biāo)狀態(tài)后驗概率密度函數(shù)對應(yīng)的積分點為均值，P(n|n)g為協(xié)方 差，構(gòu)建n時刻的多個積分點概率密度函數(shù)?，仏|")g);
[0015] 其中，公式（5)、（6)中的非高斯過程噪聲和量測噪聲的概率密度函數(shù)利用高斯和 方法近似得到如下式所示：
[0016] (7)
[0017] (S)
[0018] 其中，a^卩0j均為非負(fù)常數(shù)，且滿j
和玄&分別表 示第k個高斯概率密度函數(shù)的均值和協(xié)方差。
[0019]其中，所述根據(jù)預(yù)測粒子集獲取當(dāng)前目標(biāo)觀測時刻的目標(biāo)狀態(tài)預(yù)測概率密度函數(shù) 的步驟具體包括：根據(jù)所述預(yù)測粒子集獲取n時刻的目標(biāo)狀態(tài)后驗概率密度函 數(shù)對應(yīng)的積分點的均值焉，和協(xié)方差#，具體如下式所示：
[0020]
[0021]
[0022] 根據(jù)所述積分點 的均值. 和協(xié)方差巧L_.g，獲取n+i時刻的目標(biāo)狀態(tài)第 g'個高斯概率密度函數(shù)#(X?;l+1|,d心）的均值美:和協(xié)方差P(n+1|n)g，，具體如下式 所示：
[0023] (11)：
[0024] U-)
[0025] 計算每個高斯概率密度函數(shù)#(心 >的權(quán)值，具體如下式所示：
[0026]
(13)
[0027] 將G'個高斯概率密度函數(shù)的加權(quán)和近似為目標(biāo)狀態(tài)的預(yù)測概率密度函數(shù)，具體 如下式所示：
[0028]
(14)。
[0029] 其中，利用模糊聚類原理獲取當(dāng)前目標(biāo)觀測時刻的粒子權(quán)值和積分點權(quán)值，并 計算各高斯項的權(quán)值的步驟包括：根據(jù)所述積分點構(gòu)建n+1時刻的多個積分 點的重要性函數(shù)4w);根據(jù)所述n+1時刻的多個積分點的重要性函數(shù) 研私+丨1?心,義+iiB)g,)獲取積分點.的近似粒子集te,;」
[0030]
(21 )
[0031]其中，1 = 1，2，……，m;g，=l，2，……，G'；j=l，2，……L;i=l，2，……， N；
[0032] 計算所述近似粒子集中每個粒子的權(quán)值S^+1)gy，具體如下式所示：
[0033](22):
[0034]
(23)
[0035] 其中，為根據(jù)拉格朗日乘子法最小化目標(biāo)函數(shù)而得到的yn+1與粒子#+Wgy 的模糊隸屬度，yn+1為當(dāng)前目標(biāo)觀測時刻n+1時刻的目標(biāo)觀測。
[0036] 其中，根據(jù)所述狀態(tài)粒子集和所述粒子權(quán)值、所述積分點權(quán)值計算每個高斯分布 的均值和協(xié)方差的步驟包括：根據(jù)所述近似粒子集和權(quán)值獲取積分點 的均值 丫(??+%+1)宕'y和協(xié)方差，具體如下式所示：
[0037]
[0039] 根據(jù)所述積分點的權(quán)值獲取高斯混合項的概率密度函數(shù) N(x"+l )的均值i 和協(xié)方差ifB+1|8+WJ，以得到相應(yīng)的權(quán)值，具體 如下式所示：
[0040]
[0042] 其中，根據(jù)所述高斯項的權(quán)值對高斯項進(jìn)行重采樣，獲取G個權(quán)值較大的高斯項 的步驟包括：將計算得到的G*K*L個高斯項的權(quán)值?^uu按照降序排列以保留前G個高斯項叫\(zhòng)+1，、+1|_5,4傘如）和權(quán)值故_^其中，8=1，一，6;當(dāng)呵》 +%<1^池，樹 A/?+lln+l)if~ ^(H+l|n+l) / ' -^n+l[H+l)y 其中，j為每次循環(huán)時隨機提取，且jG{1，…，G}， 提取到」的概率正比于標(biāo)準(zhǔn)化權(quán)值％"+1)/,/1^ =1巧"+1),，8=1，一，6,6為循環(huán)次數(shù)且 兩=1'''G ;以及獲取G個權(quán)值較大的高斯項。
[0043] 其中，根據(jù)獲取的G個高斯項，利用高斯和原理得到當(dāng)前目標(biāo)觀測時刻的狀態(tài)后 驗概率密度函數(shù)，完成粒子濾波的步驟具體為：利用高斯和原理，根據(jù)獲取的G個高斯項， 當(dāng)前目標(biāo)觀測時刻的目標(biāo)狀態(tài)后驗概率密度函數(shù)P(xn+11ymn+1)可以近似成均值為元+%+1、 協(xié)方差為Pn+1|n+1的高斯分布，具體如下式所示：
[0044]
[0045]
[0046] 為解決上述技術(shù)問題，本發(fā)明的第二方面是：提供一種模糊高斯和粒子濾波裝置， 包括：積分點概率密度函數(shù)構(gòu)建模塊，用于利用高斯和構(gòu)建上一目標(biāo)觀測時刻的狀態(tài)后驗 概率密度函數(shù)、觀測噪聲概率密度函數(shù)以及狀態(tài)噪聲概率密度函數(shù)；目標(biāo)狀態(tài)預(yù)測概率密 度函數(shù)獲取模塊，用于根據(jù)所述上一目標(biāo)觀測時刻的狀態(tài)后驗概率密度函數(shù)和所述狀態(tài)噪 聲概率密度函數(shù)，利用高斯-厄米特積分和蒙特卡羅原理獲取當(dāng)前目標(biāo)觀測時刻的目標(biāo)狀 態(tài)預(yù)測概率密度函數(shù)；狀態(tài)粒子集獲取模塊，用于根據(jù)所述當(dāng)前目標(biāo)觀測時刻的目標(biāo)狀態(tài) 預(yù)測概率密度函數(shù)和所述觀測噪聲概率密度函數(shù)獲取狀態(tài)粒子集；高斯項權(quán)值計算模塊， 用于利用模糊聚類原理獲取當(dāng)前目標(biāo)觀測時刻的粒子權(quán)值和積分點權(quán)值，并計算各高斯項 的權(quán)值；高斯分布計算模塊，用于根據(jù)所述狀態(tài)粒子集和所述粒子權(quán)值、所述積分點權(quán)值計 算每個高斯分布的均值和協(xié)方差；重采樣模塊，用于根據(jù)所述高斯項的權(quán)值對高斯項進(jìn)行 重采樣，獲取G個權(quán)值較大的高斯項，G為正整數(shù)；以及概率密度函數(shù)獲取模塊，用于根據(jù)獲 取的G個高斯項，利用高斯和原理得到當(dāng)前目標(biāo)觀測時刻的狀態(tài)后驗概率密度函數(shù)，完成 粒子濾波。
[0047] 為解決上述技術(shù)問題，本發(fā)明的第三方面是：提供一種目標(biāo)跟蹤方法，包括：接收 當(dāng)前目標(biāo)觀測時刻以及當(dāng)前目標(biāo)觀測時刻之前所觀測的目標(biāo)狀態(tài)；利用高斯和構(gòu)建上一 目標(biāo)觀測時刻的狀態(tài)后驗概率密度函數(shù)、觀測噪聲概率密度函數(shù)以及狀態(tài)噪聲概率密度函 數(shù)；根據(jù)所述上一目標(biāo)觀測時刻的狀態(tài)后驗概率密度函數(shù)和所述狀態(tài)噪聲概率密度函數(shù)， 利用高斯-厄米特積分和蒙特卡羅原理獲取當(dāng)前目標(biāo)觀測時刻的目標(biāo)狀態(tài)預(yù)測概率密度 函數(shù)；根據(jù)所述當(dāng)前目標(biāo)觀測時刻的目標(biāo)狀態(tài)預(yù)測概率密度函數(shù)和所述觀測噪聲概率密度 函數(shù)獲取狀態(tài)粒子集；利用模糊聚類原理獲取當(dāng)前目標(biāo)觀測時刻的粒子權(quán)值和積分點權(quán) 值，并計算各高斯項的權(quán)值；根據(jù)所述狀態(tài)粒子集和所述粒子權(quán)值、所