一種基于判斷機(jī)動的運(yùn)動狀態(tài)估計方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種狀態(tài)估計方法，特別是在交互多模型的目標(biāo)跟蹤領(lǐng)域，應(yīng)用于目 標(biāo)跟蹤領(lǐng)域。
【背景技術(shù)】
[0002] 隨著現(xiàn)代科技的進(jìn)步，各種高機(jī)動性能的飛行器、艦艇等高機(jī)動的運(yùn)動目標(biāo)相繼 問世。這些類型的運(yùn)動目標(biāo)的機(jī)動范圍很大，這就對跟蹤系統(tǒng)提出了很高的要求，要求跟蹤 系統(tǒng)能夠?qū)δ繕?biāo)的各種運(yùn)動狀態(tài)都有著良好的狀態(tài)估計性能。但是現(xiàn)有的目標(biāo)跟蹤交互式 多模型算法僅僅是還原出目標(biāo)的運(yùn)動軌跡，不僅具有一定的延遲性，而且對于目標(biāo)信息的 短暫缺失，沒有采取任何彌補(bǔ)措施，系統(tǒng)的抗干擾能力較差。
[0003] 傳統(tǒng)的狀態(tài)估計算法的缺點(diǎn)：首先是單模型算法，而對于傳統(tǒng)的單模型濾波器，當(dāng) 目標(biāo)的運(yùn)動模式同該模型不符合的時候只能通過增加系統(tǒng)的過程噪聲等方式加以解決，但 是當(dāng)目標(biāo)的機(jī)動超出模型的跟蹤范圍的時候就會導(dǎo)致跟蹤丟失的現(xiàn)象發(fā)生，因此應(yīng)用范圍 比較窄。其次對于傳統(tǒng)的交互式多模型的過程中，為了提高算法的濾波精度，就要覆蓋盡可 能多的運(yùn)動模型，但是這樣算法計算量的成倍的提高。而且模型集合中過多的模型又會導(dǎo) 致模型之間出現(xiàn)競爭，從而降低算法的精度，而且在復(fù)雜的環(huán)境下，由于傳感器的不穩(wěn)定因 素，容易出現(xiàn)目標(biāo)的短暫缺失，而在傳統(tǒng)的狀態(tài)估計算法中，沒有對此方面進(jìn)行處理，穩(wěn)定 性較差。

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

[0004] 本發(fā)明技術(shù)解決問題：克服現(xiàn)有技術(shù)的不足，提供一種精簡的基于判斷機(jī)動的運(yùn) 動狀態(tài)估計方法，本發(fā)明提供一種較高精度的運(yùn)動狀態(tài)估計方法，采用精簡的交互式多模 型的方式，使得本發(fā)明能夠應(yīng)對復(fù)雜多變的環(huán)境的同時，不會降低精度；并且本發(fā)明中采用 的預(yù)測驗(yàn)證機(jī)制，能夠在目標(biāo)短暫缺失后，仍然能夠得到較好的狀態(tài)估計效果，提高算法的 穩(wěn)定性。
[0005] 該方法采用的技術(shù)方案為：該技術(shù)在過程上分為以下五個步驟：輸入交互， kalman濾波，判斷機(jī)動，概率預(yù)測，數(shù)據(jù)融合。首先輸入交互是在已經(jīng)獲得模型上一時刻的 狀態(tài)估計值，協(xié)方差的估計值并獲取新的量測值z(k)之后對模型進(jìn)行重新的初始化運(yùn)算， 根據(jù)模型之間的轉(zhuǎn)移概率來獲得新的初始值。根據(jù)上一步輸入交互的結(jié)果，將X, (k-Ι|k-1) 和P, (k-11k-1)作為k時刻模型的輸入，采用kalman濾波算法得到輸出的結(jié)果，然后進(jìn)入 判斷機(jī)動階段，每一個模型用相應(yīng)的算法對未來一段時間的結(jié)果進(jìn)行預(yù)測，并且判斷目標(biāo) 是否發(fā)生了機(jī)動性較強(qiáng)的運(yùn)動，在此基礎(chǔ)上，設(shè)定了一個概率函數(shù)，對預(yù)測值進(jìn)行約束，產(chǎn) 生相應(yīng)的預(yù)測概率，根據(jù)這些預(yù)測概率將kalman濾波的輸出結(jié)果進(jìn)行加權(quán)求和。接下來是 用當(dāng)前的觀測值去驗(yàn)證過去時刻的預(yù)測值是否在誤差的允許的范圍內(nèi)；
[0006] 步驟一：輸入交互：將系統(tǒng)的觀測數(shù)據(jù)作為本算法的輸入，若本算法沒有經(jīng)過第 一輪迭代，則將此時的輸入不做任何處理，直接作為步驟二的輸入；否則，根據(jù)上一輪的步 驟四產(chǎn)生的預(yù)測概率Pl和步驟二中的經(jīng)過kalman濾波器后目標(biāo)狀態(tài)，經(jīng)過概率和狀態(tài)信 息的加權(quán)求和后，得到模型j在k時刻的輸入。
[0007] 步驟二：根據(jù)步驟一的k時刻的輸入，利用這些輸入信息，啟動kalman濾波，對系 統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法。由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu) 估計也可看作是濾波過程。利用kalman濾波修正觀測數(shù)據(jù)中含有誤差的值。
[0008] 步驟三：判斷機(jī)動：經(jīng)過步驟二后，模型的kalman濾波器已經(jīng)啟動，利用卡爾曼 濾波的預(yù)測性，預(yù)測出目標(biāo)坐標(biāo)的下一個位置坐標(biāo)，用下一時刻的目標(biāo)坐標(biāo)的觀測值與預(yù) 測的下一個位置坐標(biāo)進(jìn)行對比；判斷目標(biāo)觀測值與預(yù)測的距離是否達(dá)到最大容忍的誤差范 圍；并修改概率公式的參數(shù)。
[0009] 步驟四：概率預(yù)測：結(jié)合當(dāng)前濾波值，預(yù)測接下來N個時刻的狀態(tài)信息；根據(jù)步驟 二的目標(biāo)狀態(tài)估計信息，利用概率計算公式，用計算出來預(yù)測概率P]為每一個預(yù)測值進(jìn)行 約束，其中概率公式如下：
[0010]
(4)
[0011] 其中：
[0012] time:犯規(guī)次數(shù)即觀測點(diǎn)落在容許的范圍之外的次數(shù)；ET:最大的被容許犯規(guī)次 數(shù)；t:預(yù)測的相對時間；p]:預(yù)測概率；e:自然底數(shù)。
[0013] 概率函數(shù)的范圍是（0,1)，概率的結(jié)果是與預(yù)測時間長度成反比，與發(fā)生機(jī)動的次 數(shù)time成反比。在這種獎懲方式下，使得該概率函數(shù)能夠較好的代表預(yù)測值的準(zhǔn)確程度。
[0014] 步驟五：輸出交互：經(jīng)過步驟四后，預(yù)測概率已經(jīng)得到更新。根據(jù)步驟四得出的預(yù) 測概率，對步驟二的目標(biāo)狀態(tài)信息進(jìn)行概率加權(quán)，并將概率加權(quán)的結(jié)果進(jìn)行多步預(yù)測驗(yàn)證， 使得最終結(jié)果科學(xué)而可靠。
[0015] 經(jīng)過以上步驟，基于判斷機(jī)動的運(yùn)動狀態(tài)估計方法就完成了一次迭代。
[0016] 本發(fā)明更詳細(xì)的步驟為：
[0017] 輸入：每隔一定的時間，輸入一個二維坐標(biāo)值；
[0018] 輸出：輸出該時刻的狀態(tài)信息，并給出該狀態(tài)的概率值。
[0019] 步驟一：輸入交互：若未產(chǎn)生各匹配濾波器的混合初始狀態(tài)，則設(shè)定一個初始的 混合概率和對應(yīng)的各匹配濾波器的混合初始狀態(tài)及協(xié)方差矩陣；否則根據(jù)產(chǎn)生的混合概率 Pl和對應(yīng)的各匹配濾波器的混合初始狀態(tài)X(k-i|k-Ι)及協(xié)方差矩陣P, (k-Ι|k_l)，經(jīng)過交 互后可得模型在k時刻的輸入如下式所示：
[0022] [Xx (k-1 |k-l)-Xj(k-l|k-l)]T}
[0023] 其中：
[0024]
(7)
[0025] 步驟二：kalman濾波，對于其中的模型，進(jìn)行kalman濾波，狀態(tài)矢量預(yù)測：
[0026] Xf(k|k-1) =FfXf(k-lIk-1) (8)
[0027]
(9)
[0028] kalman增益為：
[0029]
(i〇)
[0030] k時刻的濾波值為：
[0031] Xj(kIk) =Xj(k|k-1)+Kj(k) [Zk-HjXj(kIk-1) ] (11)
[0032] 濾波協(xié)方差為：
[0033] Pj(k|k) = [I-KjGOHjPjklk-l) (12)
[0034] 其中：
[0035] …
. (13)
[0036] 公式（5)-(13)中：K:增益矩陣；X:狀態(tài)向量；P:協(xié)方差矩陣；I:單位矩陣；Η:轉(zhuǎn) 移函數(shù)；F:變換函數(shù)；μ^:轉(zhuǎn)移概率；r:結(jié)果個數(shù)；j:輸出結(jié)果編號；k:時刻編號；T:矩陣 的轉(zhuǎn)置；P_]:預(yù)測概率；S:中間變量；R:誤差協(xié)方差；Z:觀測向量；Q:噪聲方差。
[0037] 步驟三：判斷機(jī)動，經(jīng)過步驟二后，kalman濾波器已經(jīng)啟動，利用卡爾曼濾波的預(yù) 測性，預(yù)測出目標(biāo)坐標(biāo)的下一個位置坐標(biāo)，用下一時刻的目標(biāo)坐標(biāo)的觀測值與預(yù)測的下一 個位置坐標(biāo)進(jìn)行對比；判斷目標(biāo)觀測值與預(yù)測的距離是否達(dá)到最大容忍的誤差范圍；如果 誤差大于最大容忍的誤差范圍，則將公式（14)中的time值加2 ;如果沒有達(dá)到最大容忍的 誤差范圍，則將公式（14)中的time值減1 ;
[0038] 步驟四：概率預(yù)測，結(jié)合當(dāng)前濾波值，預(yù)測接下來N個時刻的狀態(tài)信息；并利用概 率計算公式，用計算出來預(yù)測概率P]為每一個預(yù)測值進(jìn)行約束，其中概率公式如下：
[0039]
(14)
[0040] 其中：
[0041] time:犯規(guī)次數(shù)即觀測點(diǎn)落在容許的范圍之外的次數(shù)；ET:最大的被容許犯規(guī)次 數(shù)；t:預(yù)測的相對時間；p]:預(yù)測概率；e:自然底數(shù)。
[0042]概率函數(shù)的范圍是（0,1)，概率的結(jié)果是與預(yù)測時間長度成反比，與發(fā)生機(jī)動的次 數(shù)time成反比。在這種獎懲方式下，使得該概率函數(shù)能夠較好的代表預(yù)測值的準(zhǔn)確程度。
[0043] 步驟五：輸出交互，經(jīng)過步驟四后，預(yù)測概率已經(jīng)得到更新，根據(jù)步驟四得出的預(yù) 測概率，對步驟二的目標(biāo)狀態(tài)信息進(jìn)行概率加權(quán)，并將概率加權(quán)的結(jié)果進(jìn)行多步預(yù)測驗(yàn)證， 使得最終結(jié)果科學(xué)而可靠。
[0044]
(15)
[0045]
[0046] 其中：
[0047] X:狀態(tài)向量；P:協(xié)方差矩陣；r:輸出結(jié)果個數(shù)；Pj:預(yù)測概率；j:輸出結(jié)果編號； k:時刻編號；T:矩陣的轉(zhuǎn)置。
[0048] 本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于：
[0049] (1)本發(fā)明提出的基于判斷機(jī)動的運(yùn)動狀態(tài)估計方法具有高精度的狀態(tài)估計效 果。
[0050] (2)本發(fā)明提出的基于判斷機(jī)動的運(yùn)動狀態(tài)估計方法對于數(shù)據(jù)的短暫缺失，能夠 從容應(yīng)對，穩(wěn)定性很高。
[0051] (3)本發(fā)明提出的基于判斷機(jī)動的運(yùn)動狀態(tài)估計方法擁有多點(diǎn)預(yù)測的能力。
[0052] 現(xiàn)有的目標(biāo)跟蹤技術(shù)不能根據(jù)外界環(huán)境進(jìn)行自我調(diào)整，只是應(yīng)對于單一的環(huán)境， 應(yīng)用范圍比