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

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

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