專利名稱：：系統(tǒng)模型未知的自適應(yīng)卡爾曼濾波方法
技術(shù)領(lǐng)域：
：本發(fā)明涉及一種自適應(yīng)卡爾曼濾波方法，特別是系統(tǒng)狀態(tài)模型未知或時變的自適應(yīng)卡爾曼濾波方法。
背景技術(shù)：
：卡爾曼濾波一般在進(jìn)行濾波時，需要精確獲知系統(tǒng)的狀態(tài)模型和系統(tǒng)噪聲的統(tǒng)計特性。然而在實(shí)際情況中，系統(tǒng)狀態(tài)模型往往是未知的或者時變的，例如，飛機(jī)在空中的運(yùn)動狀態(tài)是時變且難以預(yù)測的。這種由于系統(tǒng)的狀態(tài)模型的未知或時變性會使得卡爾曼濾波精度降低甚至引起發(fā)散。解決系統(tǒng)狀態(tài)模型不確定或時變的方法是采用自適應(yīng)卡爾曼濾波算法。近年來，許多學(xué)者對自適應(yīng)卡爾曼濾波算法進(jìn)行了深入和廣泛的研究，常用的自適應(yīng)卡爾曼濾波算法是Sage-Husa自適應(yīng)算法。文獻(xiàn)“簡化的Sage-Husa自適應(yīng)濾波算法在組合導(dǎo)航中的應(yīng)用及仿真，青島大學(xué)學(xué)報，2001，16(1)”中公開了一種簡化的Sage-Husa自適應(yīng)卡爾曼濾波算法。該方法通過在線調(diào)節(jié)系統(tǒng)的狀態(tài)噪聲或觀測噪聲的協(xié)方差陣來解決系統(tǒng)狀態(tài)模型的時變性和隨機(jī)性。但是這種方法適用于系統(tǒng)狀態(tài)模型已知、狀態(tài)噪聲未知的情況，對于系統(tǒng)狀態(tài)模型未知或者隨時間變化的情況其濾波精度較差。例如，在下述不確切狀態(tài)下，X軸濾波誤差均值是0.1231；Y軸濾波誤差均值是0.3895。
發(fā)明內(nèi)容為了克服現(xiàn)有技術(shù)自適應(yīng)卡爾曼濾波方法針對系統(tǒng)狀態(tài)模型未知或者時變的情況，濾波精度下降甚至發(fā)散的不足，本發(fā)明提供一種系統(tǒng)模型未知的自適應(yīng)卡爾曼濾波方法。該方法通過對比濾波殘差與觀測噪聲之間的關(guān)系，計算出一個時變加權(quán)修正系數(shù)，從而實(shí)時調(diào)節(jié)狀態(tài)噪聲的協(xié)方差陣，可以提高濾波精度。本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案一種系統(tǒng)模型未知的自適應(yīng)卡爾曼濾波方法，其特點(diǎn)是包括下述步驟(a)建立系統(tǒng)離散化和線性化后的狀態(tài)方程x(k+l)=Ox(k)+w(k)式中，x為m維狀態(tài)向量，O為mXm維狀態(tài)轉(zhuǎn)移矩陣，w為m維系統(tǒng)狀態(tài)噪聲，建立系統(tǒng)離散化和線性化后的觀測方程z(k)=Hx(k)+v(k)式中，z為n維觀測向量；H是nXm維觀測矩陣，v為n維系統(tǒng)狀態(tài)噪聲；(b)采用卡爾曼濾波方法，對第k+1時刻進(jìn)行濾波；x(k+llk)=^{k+\,k)x{klk)P(k+\Jk)=辦(k+1,k)P(k/k)4>r(k+l,k)+4K(k+1)=P(k+l/k)Ht(k+1)[H(k+1)P(k+l/k)Ht(k+1)+Rk+1]x(k+l/k+l)=x(k+l/k)+K(k+\){z{k+1)—H(k+l)x(Jc+l/k)]P(k+1/k+1)=P(k+l/k)-P(k+l/k)Ht(k+1)[H(k+l)P(k+1/k)Ht(k+1)+Rk+1]_1H(k+l)P(k+1/k)(c)濾波殘差是，式中，ek+1是濾波殘差，zk+1是第k+1時刻的觀測值，H是系統(tǒng)觀測矩陣，是第k+1時刻狀態(tài)量的估計值；(d)修正加權(quán)系數(shù)是，式中，i=1n，Ri為觀測噪聲協(xié)方差陣R對角線上第i個數(shù)，k為修正加權(quán)系數(shù)，為m維向量；(e)修正系統(tǒng)噪聲協(xié)方差陣，式中，j=1m，4+1是估計出的第k+1時刻系統(tǒng)噪聲協(xié)方差陣，毛是修正后狀態(tài)噪聲協(xié)方差矩陣么+|中對角線上第j個分量，Qj是修正前系統(tǒng)噪聲協(xié)方差陣Qk+1對角線上第j個分量。本發(fā)明的有益效果是由于通過對比濾波殘差與觀測噪聲之間的關(guān)系，并將該殘差值引入到系統(tǒng)狀態(tài)噪聲協(xié)方差陣的估計中來，計算出加權(quán)系數(shù)，對系統(tǒng)狀態(tài)噪聲協(xié)方差陣進(jìn)行實(shí)時的修正，有效地解決了由于系統(tǒng)模型未知或隨時間變化導(dǎo)致的卡爾曼濾波精度下降甚至發(fā)散的問題。在同等條件下，X軸濾波誤差均值由現(xiàn)有技術(shù)的0.1231下降到0.0056；Y軸濾波誤差均值由現(xiàn)有技術(shù)的0.3895下降到0.0039。下面結(jié)合附圖和實(shí)施例對本發(fā)明作詳細(xì)說明。圖1是本發(fā)明系統(tǒng)模型未知的自適應(yīng)卡爾曼濾波方法的流程圖。圖2是目標(biāo)實(shí)際機(jī)動曲線圖。圖3是采用
背景技術(shù)：
簡化的Sage-Husa自適應(yīng)濾波方法估計狀態(tài)結(jié)果曲線圖。圖4是采用本發(fā)明系統(tǒng)模型未知的自適應(yīng)卡爾曼濾波方法估計狀態(tài)結(jié)果曲線圖。具體實(shí)施例方式參照圖14。以機(jī)動目標(biāo)的跟蹤為例，本發(fā)明方法的具體步驟如下步驟1建立目標(biāo)的運(yùn)動狀態(tài)方程，假設(shè)目標(biāo)的真實(shí)運(yùn)動軌跡為x方向和y方向做衰減正弦運(yùn)動，且x方向的振蕩頻率為變頻率。由于不能事先知道目標(biāo)的運(yùn)動情況，所以在建立狀態(tài)方程時，不可能精確描述其運(yùn)動軌跡，假設(shè)其按勻速直線運(yùn)動，則建立的系統(tǒng)狀態(tài)方程如下式所示其中，狀態(tài)量x，i為目標(biāo)x軸方向的位置和速度，狀態(tài)量y，j為目標(biāo)y軸方向的位置和速度，w,,w2為系統(tǒng)狀態(tài)噪聲。將狀態(tài)方程寫成向量的形式，如下所示±=Ax+w其中，x=i少為系統(tǒng)的狀態(tài)量，為4維向量，w系統(tǒng)狀態(tài)噪聲向量，為狀態(tài)矩陣。濾波時，采用的方程為離散化后的狀態(tài)方程，假設(shè)濾波周期為T，則離散化后的狀態(tài)方程為x(k+1)=(I+TA)x(k)+Tw=Ox(k)+w(k)建立系統(tǒng)的觀測方程，系統(tǒng)觀測量為雷達(dá)探測的距離和觀測角其中，P為雷達(dá)探測的距離，a為雷達(dá)的觀測角。將觀測方程寫成向量的形式，如下所示z=h(x)+v其中，Z=[pa]T為觀測向量，為2維向量，h(0為觀測方程函數(shù)，v觀測噪聲向量。上面所建立的觀測方程為非線性方程，需要對其進(jìn)行線性化，并離散化z(k)=Hx(k)+v(k)其中，H為線性化和離散化后的觀測矩陣，為2X4維矩陣。步驟2根據(jù)步驟1中所建立的狀態(tài)方程和觀測方程，采用卡爾曼進(jìn)行濾波，對第k+1時刻進(jìn)行濾波；P(k+l/k)=<P(k+1,k)P(k/k)<PT(k+\,k)+4K(k+1)=P(k+l/k)Ht(k+1)[H(k+1)P(k+l/k)Ht(k+1)+Rk+1]x(k+l/k+l)=x(k+l/k)+K(k+l)[z(k+1)-H{k+\)x(k+Hk)}P(k+l/k+l)=P(k+l/k)-P(k+l/k)HT(k+1)[H(k+l)P(k+1/k)Ht(k+1)+Rk+1]_1H(k+l)P(k+1/k)步驟3根據(jù)系統(tǒng)的狀態(tài)量、觀測量以及觀測方程求取當(dāng)前信息及其濾波殘差值(第k+1時刻)其中，ek+1是濾波殘差，為2維向量，zk+1為第k+1時刻的觀測值，H為系統(tǒng)觀測矩陣，毛+1+1為第k+1時刻狀態(tài)量的估計值。步驟4根據(jù)步驟3中所獲得的濾波殘差值計算狀態(tài)噪聲協(xié)方差陣的修正加權(quán)系數(shù)。假設(shè)系統(tǒng)的狀態(tài)噪聲和觀測噪聲均已經(jīng)為白化后的白噪聲，即狀態(tài)噪聲和觀測噪聲協(xié)方差陣均為對角線矩陣，將濾波殘差值與觀測噪聲協(xié)方差陣中對應(yīng)對角線上的值相比式中，i=12，禮，R2為系統(tǒng)觀測噪聲協(xié)方差陣R對角線上的兩個分量，設(shè)g=系統(tǒng)的觀測量為2維，則構(gòu)造如下的向量則計算的修正加權(quán)系數(shù)為k=H'(g+r)計算出的修正系數(shù)k即為系統(tǒng)噪聲協(xié)方差陣的修正系數(shù)向量，其為4維向量。步驟5:修正系統(tǒng)噪?yún)f(xié)方差陣。其中，j=14。將本發(fā)明方法與
背景技術(shù)：
中所提出的簡化Sage-Husa自適應(yīng)卡爾曼濾波方法進(jìn)行對比，對于所建立的不確切狀態(tài)模型，從圖3可以看出，采用簡化Sage-Husa自適應(yīng)卡爾曼濾波不能準(zhǔn)確估計出目標(biāo)的運(yùn)動軌跡；從圖4可以看出，采用本發(fā)明方法，對系統(tǒng)噪聲協(xié)方差陣進(jìn)行實(shí)時修正后，可以估計出系統(tǒng)狀態(tài)變量。表1給出了兩種濾波方法的估計結(jié)果，可以看出在系統(tǒng)狀態(tài)模型不確切情況下，采用本發(fā)明介紹的自適應(yīng)濾波算法的精度要優(yōu)于簡化的Sage-Husa方法。表1不同方法的濾波誤差均值對比權(quán)利要求一種系統(tǒng)模型未知的自適應(yīng)卡爾曼濾波方法，其特征在于包括下述步驟(a)建立系統(tǒng)離散化和線性化后的狀態(tài)方程x(k+1)＝Φx(k)+w(k)式中，x為m維狀態(tài)向量，Φ為m×m維狀態(tài)轉(zhuǎn)移矩陣，w為m維系統(tǒng)狀態(tài)噪聲，建立系統(tǒng)離散化和線性化后的觀測方程z(k)＝Hx(k)+v(k)式中，z為n維觀測向量；H是n×m維觀測矩陣，v為n維系統(tǒng)狀態(tài)噪聲；(b)采用卡爾曼濾波方法，對第k+1時刻進(jìn)行濾波；<mrow><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>/</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mi>Φ</mi><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>,</mo><mi>k</mi><mo>)</mo></mrow><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>/</mo><mi>k</mi><mo>)</mo></mrow></mrow><mrow><mi>P</mi><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>/</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mi>Φ</mi><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>,</mo><mi>k</mi><mo>)</mo></mrow><mi>P</mi><mrow><mo>(</mo><mi>k</mi><mo>/</mo><mi>k</mi><mo>)</mo></mrow><msup><mi>Φ</mi><mi>T</mi></msup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>,</mo><mi>k</mi><mo>)</mo></mrow><mo>+</mo><msub><mover><mi>Q</mi><mo>^</mo></mover><mi>k</mi></msub></mrow>K(k+1)＝P(k+1/k)HT(k+1)[H(k+1)P(k+1/k)HT(k+1)+Rk+1]-1<mrow><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>/</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>=</mo><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>/</mo><mi>k</mi><mo>)</mo></mrow><mo>+</mo><mi>K</mi><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>[</mo><mi>z</mi><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>-</mo><mi>H</mi><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>/</mo><mi>k</mi><mo>)</mo></mrow><mo>]</mo></mrow>P(k+1/k+1)＝P(k+1/k)-P(k+1/k)HT(k+1)·[H(k+1)P(k+1/k)HT(k+1)+Rk+1]-1H(k+1)P(k+1/k)(c)濾波殘差是，<mrow><msub><mi>ϵ</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>=</mo><msub><mi>z</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>-</mo><mi>H</mi><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn><mo>/</mo><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub></mrow>式中，εk+1是濾波殘差，zk+1是第k+1時刻的觀測值，H是系統(tǒng)觀測矩陣，是第k+1時刻狀態(tài)量的估計值；(d)修正加權(quán)系數(shù)是，<mrow><msub><mi>g</mi><mi>i</mi></msub><mo>=</mo><mfrac><msub><mi>ϵ</mi><mrow><mi>i</mi><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow></mrow></msub><msub><mi>R</mi><mi>i</mi></msub></mfrac></mrow>r＝[R1R2…Rn]Tk＝H′·(g+r)式中，i＝1～n，Ri為觀測噪聲協(xié)方差陣R對角線上第i個數(shù)，k為修正加權(quán)系數(shù)，為m維向量；(e)修正系統(tǒng)噪聲協(xié)方差陣，<mrow><msub><mover><mi>q</mi><mo>^</mo></mover><mi>j</mi></msub><mo>=</mo><msub><mi>k</mi><mi>j</mi></msub><mo>·</mo><msub><mi>q</mi><mi>j</mi></msub></mrow><mrow><msub><mover><mi>Q</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>=</mo><mi>diag</mi><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mover><mi>q</mi><mo>^</mo></mover><mn>1</mn></msub></mtd><mtd><msub><mover><mi>q</mi><mo>^</mo></mover><mn>2</mn></msub></mtd><mtd><mo>.</mo><mo>.</mo><mo>.</mo></mtd><mtd><msub><mover><mi>q</mi><mo>^</mo></mover><mi>m</mi></msub></mtd></mtr></mtable></mfenced></mrow>式中，j＝1～m，是估計出的第k+1時刻系統(tǒng)噪聲協(xié)方差陣，是修正后狀態(tài)噪聲協(xié)方差矩陣中對角線上第j個分量，qj是修正前系統(tǒng)噪聲協(xié)方差陣Qk+1對角線上第j個分量。FSA00000069855400015.tif,FSA00000069855400019.tif,FSA000000698554000110.tif,FSA000000698554000111.tif全文摘要本發(fā)明公開了一種系統(tǒng)模型未知的自適應(yīng)卡爾曼濾波方法，其目的是解決系統(tǒng)狀態(tài)模型未知或者時變的情況下，卡爾曼濾波方法濾波精度下降甚至發(fā)散的問題。針對系統(tǒng)狀態(tài)模型建立的未知或時變情況，在卡爾曼濾波的基礎(chǔ)上采用信息計算濾波殘差，根據(jù)濾波殘差和觀測噪聲協(xié)方差的比值計算出一個加權(quán)系數(shù)，通過此加權(quán)系數(shù)對系統(tǒng)狀態(tài)噪聲協(xié)方差陣進(jìn)行實(shí)時修正，有效地解決了由于系統(tǒng)模型未知或隨時間變化導(dǎo)致的卡爾曼濾波精度下降甚至發(fā)散的問題。在同等條件下，X軸濾波誤差均值由現(xiàn)有技術(shù)的0.1231下降到0.0056；Y軸濾波誤差均值由現(xiàn)有技術(shù)的0.3895下降到0.0039。文檔編號G06F17/16GK101853243SQ201010137379公開日2010年10月6日申請日期2010年4月1日優(yōu)先權(quán)日2010年4月1日發(fā)明者盧曉東,呂春紅,周軍,趙斌申請人:西北工業(yè)大學(xué)