基于有限步記憶的卡爾曼濾波方法技術(shù)

本發(fā)明專利技術(shù)公開了一種基于有限步記憶的卡爾曼濾波方法，主要解決現(xiàn)有技術(shù)中目標(biāo)跟蹤準(zhǔn)確性與穩(wěn)定性低的問題。其技術(shù)方案是：由傳統(tǒng)卡爾曼濾波方法得到目標(biāo)航跡的前N步狀態(tài)及狀態(tài)協(xié)方差；按當(dāng)前狀態(tài)向后回溯N步，得到目標(biāo)航跡的參考狀態(tài)；根據(jù)參考狀態(tài)判斷目標(biāo)的機(jī)動(dòng)性，若發(fā)生機(jī)動(dòng)，則對(duì)上一幀濾波的速度作修正；再根據(jù)參考狀態(tài)判斷當(dāng)前量測(cè)的有效性，若無效，則對(duì)新息加一個(gè)小于1的權(quán)值，得到新的新息；根據(jù)上一時(shí)刻的狀態(tài)協(xié)方差得到一步預(yù)測(cè)協(xié)方差，再計(jì)算增益矩陣；根據(jù)預(yù)測(cè)狀態(tài)和增益矩陣及新的新息更新當(dāng)前的狀態(tài)；根據(jù)一步預(yù)測(cè)協(xié)方差和增益矩陣更新狀態(tài)協(xié)方差，完成目標(biāo)跟蹤。本發(fā)明專利技術(shù)提高了目標(biāo)跟蹤的準(zhǔn)確性與穩(wěn)定性，可用于雷達(dá)數(shù)據(jù)處理。

Calman filtering method based on finite step memory

The invention discloses a Calman filtering method based on a finite step memory, which mainly solves the problems of low accuracy and low stability of the target tracking in the prior art. The technical scheme is: get the target track before N step state and state covariance by traditional Calman filtering method; according to the current state of the N step back back, get the target track according to the judgment reference state; the reference state of mobility, if there is a frame of motor, filter speed correction; and to determine the effectiveness of the at present, measured according to the reference state if invalid, the new information with a weight of less than 1, get new information new; get one step prediction covariance covariance according to the last moment, then calculate the gain matrix; according to the predicted state and the gain matrix and the new information update of the current state; according to one step prediction covariance and gain matrix to update the state covariance, complete the target tracking. The invention improves the accuracy and stability of target tracking, and can be used for radar data processing.

【技術(shù)實(shí)現(xiàn)步驟摘要】
基于有限步記憶的卡爾曼濾波方法
本專利技術(shù)屬于雷達(dá)數(shù)據(jù)處理
，主要涉及慢速運(yùn)動(dòng)目標(biāo)的跟蹤濾波技術(shù)，可用于對(duì)地面與海面目標(biāo)的穩(wěn)定跟蹤。
技術(shù)介紹
雷達(dá)對(duì)運(yùn)動(dòng)目標(biāo)的跟蹤即是跟蹤濾波的過程，這是雷達(dá)跟蹤的核心內(nèi)容，它的作用是對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)作出估計(jì)和預(yù)測(cè)。跟蹤的任務(wù)是通過相關(guān)和濾波處理建立目標(biāo)的運(yùn)動(dòng)軌跡。雷達(dá)系統(tǒng)根據(jù)在建立目標(biāo)軌跡過程中對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)作出的估計(jì)和預(yù)測(cè)來評(píng)估目標(biāo)運(yùn)動(dòng)的安全態(tài)勢(shì)和機(jī)動(dòng)性的安全效果。因此，雷達(dá)跟蹤環(huán)節(jié)工作性能的優(yōu)劣直接影響到雷達(dá)系統(tǒng)的安全效能。鑒于目標(biāo)跟蹤在增進(jìn)雷達(dá)效能中的重要作用，各國(guó)在軍用和民用等領(lǐng)域中一直非常重視發(fā)展這一雷達(dá)技術(shù)。機(jī)動(dòng)目標(biāo)跟蹤理論有了很大的發(fā)展，尤其是在跟蹤算法的研究上，理論更是日趨成熟。在跟蹤算法中，主要有線性自回歸濾波，兩點(diǎn)外推濾波，維納濾波，加權(quán)最小二乘濾波，α-β濾波和卡爾曼濾波，其中卡爾曼濾波算法在目標(biāo)跟蹤理論中占據(jù)了主導(dǎo)地位。傳統(tǒng)的標(biāo)準(zhǔn)卡爾曼濾波算法是一種疊代最小均方RLS濾波技術(shù)，其是把目標(biāo)的位置，速度和加速度作為目標(biāo)狀態(tài)矢量，通過目標(biāo)的動(dòng)力學(xué)方程來描述目標(biāo)狀態(tài)的變化，利用遞推的計(jì)算方法，估計(jì)目標(biāo)的狀態(tài)，以建立目標(biāo)的航跡?？柭鼮V波算法雖具有收斂速度快、運(yùn)算量小的特點(diǎn)。但該方法普遍存在的問題就是跟蹤誤差大，跟蹤結(jié)果可能不收斂甚至有丟失目標(biāo)的狀況，即它的穩(wěn)定性不高。
技術(shù)實(shí)現(xiàn)思路
本專利技術(shù)的目的在于針對(duì)傳統(tǒng)的標(biāo)準(zhǔn)卡爾曼濾波算法的不足，提出一種基于有限步記憶的卡爾曼濾波方法，以減小跟蹤誤差，提高目標(biāo)跟蹤的準(zhǔn)確性與穩(wěn)定性。實(shí)現(xiàn)本專利技術(shù)目的的技術(shù)方案，包括如下：(1)根據(jù)已知目標(biāo)航跡的前N步濾波狀態(tài)和預(yù)測(cè)狀態(tài)以及狀態(tài)協(xié)方差P(k-1|k-1)，按照目標(biāo)航跡的當(dāng)前狀態(tài)向后回溯N步，得到目標(biāo)航跡的參考狀態(tài)其中，k＝1,2,…,N表示時(shí)刻；(2)根據(jù)目標(biāo)航跡的參考狀態(tài)判斷目標(biāo)是否發(fā)生機(jī)動(dòng)：2a)根據(jù)參考狀態(tài)到當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)的位移a和當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)到量測(cè)狀態(tài)Z(k)的位移c，計(jì)算這兩個(gè)位移之間的夾角θ：其中，<a,c>表示a與c之間的內(nèi)積。2b)根據(jù)角度θ判斷目標(biāo)是否發(fā)生機(jī)動(dòng)：若則表示量測(cè)在當(dāng)前目標(biāo)航跡方向的±45°范圍內(nèi)，目標(biāo)沒有發(fā)生機(jī)動(dòng)，直接執(zhí)行步驟(4)；若則表示目標(biāo)發(fā)生了機(jī)動(dòng)，此時(shí)對(duì)當(dāng)前量測(cè)的速度加一個(gè)權(quán)值wX和對(duì)上一時(shí)刻濾波的速度加一個(gè)權(quán)值wZ：2c)根據(jù)當(dāng)前量測(cè)的速度的權(quán)值wX和上一時(shí)刻濾波的速度的權(quán)值wZ修正上一時(shí)刻的濾波，得到更新后的濾波從而得到更新后的預(yù)測(cè)再根據(jù)當(dāng)前時(shí)刻的預(yù)測(cè)計(jì)算當(dāng)前時(shí)刻預(yù)測(cè)的量測(cè)(3)根據(jù)目標(biāo)航跡的參考狀態(tài)判斷當(dāng)前量測(cè)Z(k)的可靠性：3a)根據(jù)參考狀態(tài)到當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)的位移a和參考狀態(tài)到量測(cè)狀態(tài)Z(k)的位移b，計(jì)算這兩個(gè)位移之間的夾角α：其中，<a,b>表示a與b之間的內(nèi)積；3b)根據(jù)當(dāng)前量測(cè)Z(k)和當(dāng)前預(yù)測(cè)的量測(cè)得到當(dāng)前新息v(k)：3c)根據(jù)角度α判斷當(dāng)前量測(cè)Z(k)的可靠性：若cosα≥0，則表示當(dāng)前量測(cè)可靠；若cosα<0，則表示當(dāng)前量測(cè)不可靠，對(duì)當(dāng)前新息v(k)加一個(gè)權(quán)值w，得到新的新息v'(k)，其中，w<1；(4)根據(jù)上一時(shí)刻的狀態(tài)協(xié)方差P(k-1|k-1)得到一步預(yù)測(cè)協(xié)方差P(k|k-1)，進(jìn)而計(jì)算出當(dāng)前增益矩陣G(k)；(5)根據(jù)當(dāng)前預(yù)測(cè)狀態(tài)和增益矩陣G(k)及新的新息v'(k)，更新當(dāng)前時(shí)刻的濾波狀態(tài)(6)根據(jù)一步預(yù)測(cè)協(xié)方差P(k|k-1)和增益矩陣G(k)，更新當(dāng)前狀態(tài)協(xié)方差矩陣P(k|k)。本專利技術(shù)與現(xiàn)有的傳統(tǒng)卡爾曼濾波方法相比具有以下優(yōu)點(diǎn)：1.本專利技術(shù)在目標(biāo)發(fā)生機(jī)動(dòng)時(shí)，對(duì)上一幀濾波狀態(tài)的速度進(jìn)行加權(quán)修正，避免了跟蹤結(jié)果不收斂甚至目標(biāo)丟失的情況；2.本專利技術(shù)通過將目標(biāo)航跡向后回溯N步來判斷當(dāng)前量測(cè)的可靠性，對(duì)于不可靠的量測(cè)再通過對(duì)當(dāng)前新息加一個(gè)小于1的權(quán)值使其滿足可靠性的要求，從而大大減小了跟蹤誤差；3.本專利技術(shù)使用基于有限步記憶的卡爾曼濾波方法，使目標(biāo)跟蹤的準(zhǔn)確性與穩(wěn)定性得以大幅提高。為了更清楚地說明本專利技術(shù)實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將結(jié)合實(shí)施例和附圖對(duì)本專利技術(shù)作進(jìn)一步描述。附圖說明圖1為本專利技術(shù)的實(shí)現(xiàn)流程圖；圖2為本專利技術(shù)中量測(cè)可靠性判斷示意圖；圖3為用傳統(tǒng)卡爾曼濾波方法處理間斷航跡的仿真結(jié)果圖；圖4為用本專利技術(shù)中基于有限步記憶的卡爾曼濾波方法處理間斷航跡的仿真結(jié)果圖；圖5為用傳統(tǒng)卡爾曼濾波方法處理連續(xù)航跡的仿真結(jié)果圖；圖6為用本專利技術(shù)中基于有限步記憶的卡爾曼濾波方法處理連續(xù)航跡的仿真結(jié)果圖；具體實(shí)施方式參照?qǐng)D1，本專利技術(shù)的實(shí)現(xiàn)步驟包括如下：步驟一、獲取目標(biāo)航跡的參考狀態(tài)。由傳統(tǒng)卡爾曼濾波方法得到目標(biāo)航跡的前N步濾波狀態(tài)和預(yù)測(cè)狀態(tài)以及狀態(tài)協(xié)方差P(k-1|k-1)，其中，k＝1,2,…,N表示時(shí)刻；按照航跡的當(dāng)前狀態(tài)向后回溯N步，得到的濾波狀態(tài)稱為目標(biāo)航跡的參考狀態(tài)步驟二、根據(jù)目標(biāo)航跡的參考狀態(tài)判斷目標(biāo)是否發(fā)生機(jī)動(dòng)。參照?qǐng)D2，本步驟的具體實(shí)現(xiàn)如下：2a)根據(jù)參考狀態(tài)到當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)的位移a和當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)到量測(cè)狀態(tài)Z(k)的位移c，計(jì)算這兩個(gè)位移之間的夾角θ：其中，<a,c>表示a與c之間的內(nèi)積；2b)根據(jù)角度θ判斷目標(biāo)是否發(fā)生機(jī)動(dòng)：若則表示量測(cè)在當(dāng)前目標(biāo)航跡方向的±45°范圍內(nèi)，目標(biāo)沒有發(fā)生機(jī)動(dòng)，直接執(zhí)行步驟三；若則表示目標(biāo)發(fā)生了機(jī)動(dòng)，此時(shí)分別給定當(dāng)前量測(cè)的速度的權(quán)值wZ和上一時(shí)刻濾波的速度的權(quán)值wX：2c)根據(jù)當(dāng)前量測(cè)的速度vZ的權(quán)值wZ和上一時(shí)刻濾波的速度vX的權(quán)值wX修正上一時(shí)刻濾波的速度vX，得到修正后的速度v'X：v′X＝wZ·vZ+wX·vX，用修正后的速度v'X代替修正前的速度vX，得到修正后的濾波進(jìn)而更新當(dāng)前時(shí)刻的預(yù)測(cè)為：其中，F(xiàn)(k-1)為狀態(tài)轉(zhuǎn)移矩陣；再根據(jù)當(dāng)前時(shí)刻的預(yù)測(cè)得到當(dāng)前時(shí)刻預(yù)測(cè)的量測(cè)其中，H(k)為量測(cè)矩陣。步驟三、根據(jù)目標(biāo)航跡的參考狀態(tài)判斷當(dāng)前量測(cè)Z(k)的可靠性。3a)根據(jù)參考狀態(tài)到當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)的位移a和參考狀態(tài)到量測(cè)狀態(tài)Z(k)的位移b，計(jì)算這兩個(gè)位移之間的夾角α：其中，<a,b>表示a與b之間的內(nèi)積；3b)根據(jù)當(dāng)前量測(cè)Z(k)和當(dāng)前時(shí)刻預(yù)測(cè)的量測(cè)得到當(dāng)前新息v(k)：3c)根據(jù)角度α判斷當(dāng)前量測(cè)Z(k)的可靠性：若cosα≥0，則表示當(dāng)前量測(cè)可靠；若cosα<0，則表示當(dāng)前量測(cè)不可靠，則要對(duì)當(dāng)前新息v(k)加一個(gè)權(quán)值w，得到新的新息v'(k)，其中，w<1。步驟四、計(jì)算一步預(yù)測(cè)協(xié)方差矩陣和當(dāng)前增益矩陣。4a)根據(jù)上一時(shí)刻的狀態(tài)協(xié)方差矩陣P(k-1|k-1)得到一步預(yù)測(cè)協(xié)方差矩陣P(k|k-1)：P(k|k-1)＝F(k-1)P(k-1|k-1)FT(k-1)，其中，F(xiàn)T(k-1)為狀態(tài)轉(zhuǎn)移矩陣F(k-1)的轉(zhuǎn)置。4b)根據(jù)一步預(yù)測(cè)協(xié)方差矩陣P(k|k-1)計(jì)算出當(dāng)前增益矩陣G(k)：G(k)＝P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k))-1，其中，HT(k)為量測(cè)矩陣H(k)的轉(zhuǎn)置，(H(k)P(k|k-1)HT(k))-1為H(k)P(k|k-1)HT(k)的逆。步驟五、更新當(dāng)前時(shí)刻的濾波狀態(tài)。根據(jù)當(dāng)前預(yù)測(cè)狀態(tài)和增益矩陣G(k)及新的新息v'(k)來更新當(dāng)前時(shí)刻的濾波狀態(tài)步驟六、更新當(dāng)前時(shí)刻的狀態(tài)協(xié)方差矩陣。根據(jù)一步預(yù)測(cè)協(xié)方差矩陣P(k|k-1)和增益矩陣本文檔來自技高網(wǎng)...

【技術(shù)保護(hù)點(diǎn)】
基于有限步記憶的卡爾曼濾波方法，包括：(1)根據(jù)傳統(tǒng)卡爾曼濾波方法得到目標(biāo)航跡的前N步濾波狀態(tài)

【技術(shù)特征摘要】
1.基于有限步記憶的卡爾曼濾波方法，包括：(1)根據(jù)傳統(tǒng)卡爾曼濾波方法得到目標(biāo)航跡的前N步濾波狀態(tài)和預(yù)測(cè)狀態(tài)以及狀態(tài)協(xié)方差P(k-1|k-1)，按照目標(biāo)航跡的當(dāng)前狀態(tài)向后回溯N步，得到目標(biāo)航跡的參考狀態(tài)其中，k＝1,2,…,N表示時(shí)刻；(2)根據(jù)目標(biāo)航跡的參考狀態(tài)判斷目標(biāo)是否發(fā)生機(jī)動(dòng)：2a)根據(jù)參考狀態(tài)到當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)的位移a和當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)到量測(cè)狀態(tài)Z(k)的位移c，計(jì)算這兩個(gè)位移之間的夾角θ：其中，<a,c>表示a與c之間的內(nèi)積。2b)根據(jù)角度θ判斷目標(biāo)是否發(fā)生機(jī)動(dòng)：若則表示量測(cè)在當(dāng)前目標(biāo)航跡方向的±45°范圍內(nèi)，目標(biāo)沒有發(fā)生機(jī)動(dòng)，直接執(zhí)行步驟(4)；若則表示目標(biāo)發(fā)生了機(jī)動(dòng)，此時(shí)對(duì)當(dāng)前量測(cè)的速度加一個(gè)權(quán)值wX和對(duì)上一時(shí)刻濾波的速度加一個(gè)權(quán)值wZ：2c)根據(jù)當(dāng)前量測(cè)的速度的權(quán)值wX和上一時(shí)刻濾波的速度的權(quán)值wZ修正上一時(shí)刻的濾波，得到更新后的濾波從而得到更新后的預(yù)測(cè)再根據(jù)當(dāng)前時(shí)刻的預(yù)測(cè)計(jì)算當(dāng)前時(shí)刻預(yù)測(cè)的量測(cè)(3)根據(jù)目標(biāo)航跡的參考狀態(tài)判斷當(dāng)前量測(cè)Z(k)的可靠性：3a)根據(jù)參考狀態(tài)到當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)的位移a和參考狀態(tài)到量測(cè)狀態(tài)Z(k)的位移b，計(jì)算這兩個(gè)位移之間的夾角α：其中，<a,b>表示a與b之間的內(nèi)積；3b)根據(jù)當(dāng)前量測(cè)Z(k)和當(dāng)前預(yù)測(cè)的量測(cè)得到當(dāng)前新息v(k)：3c)根據(jù)角度α判斷當(dāng)前量測(cè)Z(k)的可靠性：若cosα≥0，則表示當(dāng)前量測(cè)可靠；若cosα<0，則表示當(dāng)前量測(cè)不可靠，對(duì)當(dāng)前新息v(k)加一個(gè)權(quán)值w，得到新的新息v'(k)，其中，w<1；(4)根據(jù)上一時(shí)刻的狀態(tài)協(xié)方差P(k-1|k-1)得到一步預(yù)測(cè)協(xié)方差P(k|k-1)，進(jìn)而計(jì)算出當(dāng)前增益矩陣G(k)；(5)根據(jù)當(dāng)前預(yù)測(cè)狀態(tài)和增益矩陣G(k)及新的新息v'(...

【專利技術(shù)屬性】
技術(shù)研發(fā)人員：楊志偉，郭永霞，辛金龍，楊桂娥，廖桂生，袁賽，
申請(qǐng)(專利權(quán))人：西安電子科技大學(xué)，
類型：發(fā)明
國(guó)別省市：陜西,61