線性離散系統(tǒng)的最優(yōu)濾波kalman濾波

1、For personal use only in study and research; not for commercial use第二章 線性系統(tǒng)的最優(yōu)濾波和預(yù)測2.1線性離散系統(tǒng)的最優(yōu)濾波假設(shè)系統(tǒng)分別由下列線性離散方程所描述:(2.1)x(k 1)-：(k 1,k)x(k)丨(k 1,k)w(k)z(k 1) = H (k 1)x(k 1) v(k 1)式中，x(k 1) Rn，w(k) Rp，z(k 1) Rn，v(k 1) Rn，且系統(tǒng)噪聲 w(k)和觀測 噪聲v(k 1)為零均值白噪聲序列，即對所有k, j =0,1,2，有Ew(k) =0Ev(k) =0Ew(k)wT(j)=Q(

2、k)6j(2.2)Ev(k)vT(j) =R(k)%Ew(k)vT(j)=0假定初始狀態(tài)有下列統(tǒng)計特性：Ex(0)H mx(0)，Ex(0) - mx(0)x(0) - mx(0)T二 Px(0)(2.3)x(0)與w(k)、v(k)都不相關(guān)，即E x(0)wT (k)H 0，Ex(0)vT (k) = 0(2.4)現(xiàn)在用正交投影法來推導(dǎo)線性離散系統(tǒng)的最優(yōu)估計值的計算式。系統(tǒng)狀態(tài)x(j)基于前k次觀測z(1), z(2)/ ,z(k)的線性最小方差估計應(yīng)為x( j)在zk二zT(1),zT (k)T上的正交 投影，即x(j |k) = Ex(j)|zk(2.5)亦即 使得估值?(j |k)與x

3、(j)之間的誤差(j |k)= x(j)-?(j |k)的方差為最小，即ExT(j |k)xT(j |k)=min離散系統(tǒng)Kalman濾波問題可以分成三類：(1) j k稱為預(yù)測(或外推)問題；(2) j =k稱為濾波(或估計)問題；(3) j : k稱為平滑(或內(nèi)插)問題。我們在下面各節(jié)依次討論離散線性系統(tǒng)和連續(xù)的最優(yōu)濾波和最優(yōu)預(yù)測方法。當j rk時，x(k|k)為最優(yōu)濾波估計対k|k) = Ex(j)|zk由于k廠z =z(kL式中zkJ表示z(1), ,z(k -1)全體，于是?(k|k) = Ex(k)|zk,z(k)由于z(k)與Zk不相互獨立，根據(jù)條件期望公式有?(k|k) = E

4、x(k)|zkE(k)|(k|k-1)二 Ex(k)|zk PPz；(k|k-1)其中P? =E(k|k1)T(k|k1)：Pz 二 E(k|k-1)T(k|k-1)下面推導(dǎo)(2.9)式中各項的表達式：Ex(k) |zk = E：(k,k -1)x(k -1)丨(k,k -1)w(k -1) |zk_l= E(k,k -1)x(k -1) |zkJ - E- (k, k -1)w(k -1) | zkJ-:：J(k,k -1)Ex(k -1) |zk】(k,k -1)Ew(k -1) | zkJ由于w(k -1)與zk互不相關(guān)，而且E w(k -1) =0，故x(k |k -1)=：(k,k

5、-1)5?(k -1|k -1)再來看(2.9)式等號右邊第二項PzPzz1(k|1)，由于(k|k -1) =x(k) -Ex(k) |zk4= x(k) -1)x(k -11 k -1)(2.6)(2.7)(2.8)(2.9)(2.10)(2.11)(k|k-1) =z(k)-Ez(k)|zk_(L 1 一 L 1 眩(L |空)6(上)H QNMW + (L -7 l)y(L FeM(卜 Lz) 1 一 L I)8(Ll)e(l) H gz 廠工)Dr+ )X(L 1 一idQ) Hx q)1h(l 一 l)d+ (L 1 一 L I)8(L leH (上一壬CX er(6cxl)|)山

6、 + (ihh Hl i - 1)ixqmlij十m)h(l i-i)4u(i)h + (i)1h(li-i)1x(l 1 一ixuQ) ht 寸宀q)+(l 一 蘭乞壬 H =(!+ (L 1 一iXQ) hut 彳 w i 一iTNo i 一 wmuT md eMCL.CXI)圧(9LCXI)T(LI - l)dH 儀dfl(寸 Lz) (Ll-l)dHWl-l)4(Ll-l)xUJG OH 宀(l(H(Ll-l)xtLr柑耳wbhl上匸浜WQ)F曲宀H(L 1 - I)XLLJ+- H 宀(L 1 _ 1)rx(L 1 _ 土)注山 HCS+(L 1 _ Ex) H =(L 1 _ 壬匕

7、)山 H cs + (L 1 - L 1 艮(L 1 Yegx) H =(L 1 - 1)2 D 山 H 宀 J(L 1 - L 土冬 1 ZGSH s+gx) h =(l1 - D 山 H 宀 J(L 1 - L I$1 -l) 6(l) H (l)z =(l1 - 1)4 D 山 H 似de=woL.cxl)l-Ll)3(Lrl)GgHZH(Ll-l)m一(L 1- L I$I)XLLJ(I) H Hrz -gztlr宦o宀q)lljM柑耳rzFfe眉(!)rz-(!)山十rz-gxtlrgHH rz -(!)山+rz -gxgHLLJH rz 二+sxg工 d山H(L土 - IEHrz-

8、SZLLJ(2.17)式表示在k時刻獲得觀測值z(k)后，x(k)的最優(yōu)估計值的計算式，它是用k時刻的觀測值來修正遞推值 x(k | k -1)后得到的，因而是-Kalman濾波方程。令-種遞推估計，通常稱(2.17)式為P(k |k -1)HT(k)H(k)P(k|k -1)HT(k)R(k)1 二 K(k)(2.18)則K(k)稱為Kalman增益陣，P(k|k1)為預(yù)測誤差協(xié)方差，它可以由下式求得P(k |k -1 E(k | k -1)xT (k | k -1)=E (k,k -1)(k -11 k -1)丨(k,k-1)w(k 1)(k,k -1)(k -1|k 一1)1 (k,k

9、- 1)w(k - 1)T= ：(k,k -1)E(k -11 k -1)T(k -1|k -1) ：(k,k -1) G(k,k -1)E(k -1| k - 1)wT (k-1)丨 T(k,k -1) 丨(k,k -1)Ew(k -1)T(k -1|k -1)：(k,k -1)丨(k,k -1)Ew(k - 1)wT (k - 1丨 T(k,k -1)= ：(k,k -1)P(k -1|k 1)：(k,k -1): (k,k -1)Q(k 1)】 T(k,k -1)現(xiàn)在求濾波誤差協(xié)方差陣的表達式。由于(k | k) = x(k) - X(k | k)?(k |k) =?(k |k 1) +

10、K(k)(k |k 1)故(k|k) =x(k) -X(k|k)二 x(k) - 対k|k-1)-K (k)z(k) - ？(k | k -1)= (k|k-1) -K(k)H(k)x(k) v(k)-H(k)X(k|k-1)(2.20)= (k|k -1) -K(k)H(k)(k|k -1) v(k)= (k|k -1) -K(k)H(k)(k|k -1) K(k)v(k)P(k|k)二 E(k|k)T(k|k)-E(k |k -1) -K(k)H(k)(k|k -1) -K(k)v(k)(k |k -1) -K(k)H(k)(k|k -1) - K(k)v(k)T=E(k|k -1)T(k

11、|k -1) -E(k| k -1)T(k | k -1)HT (k)KT (k)- E (k | k - 1)vT (k)KT (k) - K(k)H (k)E(k | k - 1)T (k | k -1)K(k)H (k)E(k |k -1)T(k |k -1) HT (k)KT(k) K(k)H(k)E(k | k -1)vT (k)KT(k)-K(k)Ev(k)T (k | k _ 1) _ K(k) Ev(k)T (k | k - 1)HT (k)KT (k) K(k)Ev(k)vT (k) KT (k)二 P(k|k -1) -P(k|k -1)HT(k)KT(k) -K(k)H(

12、k)P(k|k -1)K(k)H (k)P(k|k -1) H T(k)KT (k) K(k)R(k)KT(k)將K(k) =P(k|k-1)HT(k)H(k)P(k|k-1)HT(k)R(k)_1代入上式P(k|k) =P(k|k _1) _P(k|k _1)H T(k)K T(k) _ K(k) H (k)P(k |k _1)-K(k)H(k)P(k|k -1)H T(k)KT(k) K (k)R(k)KT (k)= P(k | k -1) -K(k)H(k)P(k|k -1) -P(k|k -1)H T (k)KT (k)K(k)H (k)P(k| k -1) H T (k) R(k)K

13、T (k)= P(k | k -1) -K(k)H(k)P(k|k -1) -P(k|k -1)H T (k)KT (k)P(k|k -1)HT(k)H(k)P(k|k -1)H T (k)R(k) - H (k)P(k | 1) H T (k) R(k)KT(k)二P(k|k -1) -K(k)H(k)P(k|k -1) -P(k|k -1)H T (k)KT (k) P(k| k -1)H T(k)KT(k) 二I -K(k)H(k)P(k |k -1)即P(k|k)珂I -K(k)H(k)P(k |k -1)(2.21)綜上所述，最優(yōu)濾波估計?(k|k)由以下遞推公式給出：(1) X(k

14、 | k) “(k, k -1)X(k -11 k 1) + K(k) z(k) H (k)6(k,k 1)5?(k 1|k 1)TTS(2) K(k) =P(k | k -1)H 1 (k)H(k)P(k | k -1)H 1 (k) + R(k)(3) P(k | k 一1)=(k, k -1)P(k -11 k 一1)T (k,k 1) +F(k, k -1)Q(k -1)FT(k,k -1)丨(2.22)(4) P(k|k)=l K(k)H(k)P(k |k 1)k =0,12我們稱(2.22)式所表達的遞推算法為Kalman濾波器(KF)實際應(yīng)用中，我們是在已知k -1時刻狀態(tài)x(k

15、 -1)的估計值？(k -11 k -1)和相應(yīng)的估計誤 差協(xié)方差陣P(k -1|k-1)，Kalman濾波器的濾波過程為TT(3) P(k | k 1)=(k,k 1)P(k 1| k 1)(k,k -1)+F(k,k-1)Q(k-1)F (k,k1)TT(2) K(k) =P(k |k1)H (k)H(k)P(k | k1)H (k) + R(k)(1) X(k | k) =6(k, k 1)X(k 11 k 1) + K(k)z(k) H (k)O(k,k 1)?(k 11 k -1) 卜(4) P(k|k)=l -K(k)H(k)P(k | k -1)k =0,12算法的具體步驟:(1

16、) 已知X(k-1|k-1)和P(k1|k1)，根據(jù)狀態(tài)方程對k時刻的狀態(tài)x(k)進行 預(yù)測，得預(yù)測估計值X(klk -1)=：(k,k -1)5?(k -1|k -1)和預(yù)測誤差協(xié)方差陣P(k |k -1)=：(k,k -1)P(k -1|k -1)/(k,k -1)丨(k,k-1)Q(k -1 T(k,k -1)(2) 對k時刻的測量值進行預(yù)測岔k |k -1) =H (k)X；k |k -1)(3) 計算增益陣K(k)TTA.K(k) =P(k|k -1)H (k)H (k)P(k |k -1)H (k)R(k)(4) 當k時刻觀測值到來時，對預(yù)測值進行修正Xk | k) = 5?(k | k -1) K(k)z(k) ?(k |k -1)(4)計算估計誤差P(k |k)二I -K(k)H(k)P(k | k -1)k =0,12僅供個人用于學(xué)習、研究；不得用于商業(yè)用途For personal use only in study and research; not for commercial use.Nur f u r den pers?n