1、考慮隨機(jī)非線性離散廣義系統(tǒng)Mx(k 1尸 f (x(k),k) g(x(k),k)w(k)(0-1)y(k 1) = h(x(k 1),k 1) v(k 1)(0-2)其中：k為離散時(shí)間，狀態(tài)變量x(k)w汲n,觀測(cè)變量y(k戶小m,輸入噪聲w(k), 觀測(cè)噪聲v(k), f(), g()和h()對(duì)x(k)是可微的，M w由pM是奇異常數(shù)矩陣， rank M = r < min p, n。假設(shè)1 w(k)和v(k)是零均值的不相關(guān)白噪聲EJh(kawT(j) vT(j)L,器 S(k)k二v(k)J ：S (k) R(k)J其中E為均值符號(hào)，T為轉(zhuǎn)置符號(hào)，縱k=1, 6kj=0(k

2、65;j)。假設(shè)2非線性廣義系統(tǒng)(0-1卜(0-2促強(qiáng)可控的。如果在k時(shí)刻，狀態(tài)變量x(k)的最小方差線性估值x(k|k)已知，則可以將非線性系統(tǒng)(0-1心x(k) =x(k | k) , (0-2)在x(k) = x(k +1| k)附近進(jìn)行Taylor級(jí)數(shù)展開， 只保留一次項(xiàng)，近似可得f(x(k),k)八Mx(k 1) = f(?(k|k),k) T :康)次k|k)(x(k)-x(k|k) g(x(k),k)w(k)(0-3) 二x (k)y(k 1) = hx(x(k 1|k),k)由(x(k 1?k)康體)項(xiàng)k 1|k) (x(k) - ？(k | k) v(k 1)(0-4) 二x

3、 (k 1| k)其中：離散狀態(tài)一步預(yù)測(cè)為？( k 1 |k ) ?f (x (k |,k )卷認(rèn)為g( x( k)亍k )? g (x (k祖懸商散)系統(tǒng)的模型就可以表示為Mx(k 1)= ：,(k 1| k)x(k) f (x(k|k) -中(k 1|k)x(k|k)l ： (k)w(k) (0-5)y(k 1) = H (k 1)x(k 1) h(x(k 1| k),k 1) - H(k 1)x(k 1| k) 1 v(k 1)(0-6)其中k+1|k)yx(k)W(k|k),(0-7)二x (k)fh(x(k 1),k 1),H(k +1)x(k +1)=- t院利望生那)，(0-8)

4、ex (k 1) (k) =g(x(k|k),k)(0-9)因?yàn)閤(k+1|k) =f(?(k|k),k),即為k+1|k)與？(k|k)在k時(shí)刻的取值相關(guān),所以在k+1時(shí)刻，&k+1|k)為已知的常數(shù)，從而h(X(k+1|k),k+1)在k+1時(shí)刻也是已知的常數(shù)，同時(shí)在k+1時(shí)刻H(k+1)也是已知的。由于rank M =r ,根據(jù)矩陣奇異值分解,存在正交矩1)陣U WpMp.VWBnM,使得工UMV 二,001 0(0-10)式中E =diag(O1,4,，。r),5(i =1,2，r)矩陣M的奇異值。令x(k) =V(xT (k),xT (k)T。用U左乘式(0-5)的兩端并把式

5、(0-10)代入(0-5)應(yīng)用U4(k)V =A11(k) A12(k) %(k) &%),，U (k)=(k)、I2(k) J，H(k)V= H1(k) H2(k)推導(dǎo)系統(tǒng)得,一 0 :1 x1(k1)Ju。(k)V 0 0 x2(k 1) -U (k)V僅1%)1+U 1f 住1% 2) 人、J U If x2(k k)；x2(k)rk/x1(k k)十(k)U(k k)力U- (k)w(k)(0-11)三 0x1(k1). A1(k)A12(k)x1(k). g1(k).0 0x2(k1)一 .4(k)A22(k)x2(k) g2(k)- 1(k) - 2(k) w(k)(0-1

6、2)分解得到三 x1(k 1) = A1(k)K(k) A12(k)x2(k) - 1(k)w(k) g(k)(0-13)0 = 41(k)x1(k) A22(k)x2(k) - 2(k)w(k) gz(k)(0-14)y(k)=H1(k)x4k) H2(k)x2(k) v(k)(0-15)其中x1(k) 5r , x2(k) An” , (0-13)(0-15)為非方廣義系統(tǒng)的奇異值標(biāo)準(zhǔn)形式。由式(0-14)可以得到-A22(k)x2(k) =A21(k)x1(k) - 2(k)w(k) g?(k)(0-16)當(dāng)rank A22(k) =min p -r,n _ r,故存在偽逆A?(k),左

7、乘(0-13)得到下式X2(k) = -A22(k)%(k)x(k) -相(k)g2(k) - A?2*) 2(k)w(k)(0-17)將(0-17玳入(0-13)(0-15卿到一個(gè)r維子系統(tǒng)x1(k 1) = Ao(k)x1(k) G(k)二(k)w(k)(0-18)y(k) =Co(k)Xi(k)-H2(k)AT2(k)g2(k)(k)(0-19)其中A(k)=工1%)-工)A2(k)A*k)A21(k) , C0(k)=H1(k)-H2(k)A2(k)A21(k),G(k)=工,g1(k)工Az(k)A22g2(k), a(k)=工,入«)工人2*)&*)2(k)，”k

8、) = -小心出9十小)，S(k)=E(w(k)nT(j)=(S(k)-Q(k)r2r(k)A22(k)HT(k)風(fēng),T(k)=ES(k)”T(j) = R(k) + H2(k)A22(k)r2(k)Q(k)rT(k)(A22(k)THT (k)-ST(k)： 2(k)(A22(k)THT(k)-H2(k)A22(k)： 2(k)S(k)、國首先將相關(guān)噪聲系統(tǒng)轉(zhuǎn)化為帶不相關(guān)噪聲系統(tǒng)。在(0-18)式右邊形式地加一項(xiàng)等于零的項(xiàng)：為出+1)=4的 +G(k)+a(k)w(k)+J(k) (k)-C0(k)x1(k)+H2(k)AT2(k)g2(k)-n(k)i(0-20)其中J(k)是待定矩陣，記

9、A0(k) =4(k) -J(k)C0(k)/k) - ： (k)w(k) - J(k) (k)/k)= “k)w(k) - J(k)|-H2(k)A22(k)- 2(k)w(k) v(k)(k) - p(k) J(k)H2(k)A22(k)- 2(k) w(k) - J(k)v(k)則狀態(tài)方程(0-20)化為X(k 1) = A(k)x1(k) G(k) J(k)y(k) J(k)H2(k)AT2(k)g2(k) Yk)(0-21)觀測(cè)方程仍為(0-19),注意P(k)仍為白噪聲，且有EF(k) =0 ,E(k) T(k) =E*(k)w(k)-J(k) (k) T(k)E _-(k) T(

10、k) =E(k) S(k)-Q(k)T(k)A22T(k)HT(k) -J(k) (k) T(k)由上式看到，取J(k)為J(k)=:(k) S(k)Q(k)： T(k)A22T(k)HT(k) -J(k)(0-22)其中-(k)=Ei (k) T(j)； = R(k)凡的相的：2(k)Q(k)： T(k)(A22(k)THT(k)-ST(k)： 2(k)(A22(k)THT(k) -出相的：2(k)S(k) kj則P(k)于。(k)不相關(guān)，即E(k) T(j) =0k, j易知白噪聲F(k)的協(xié)方差陣為E-(k) "(k) =(： (k) J(k)H2(k)A22(k) 2(k)Q

11、(k)(二(k) J(k)H2(k)A22(k) 2(k)T(：(k) J(k)H2(k)A22(k)： 2(k)S(k)JT(k) - J(k)S(k)(： (k) J(k)H2(k)A22(k)： 2(k)T J(k)R(k)JT(k)從而得到噪聲不相關(guān)系統(tǒng)：Xi(k 1) = A(k)K(k) G(k) J(k)y(k) J(k)H2(k)AT2(k)g2(k) 1 (k)(0-23)y(k) =C°(k)x(k)-H2(k)A2(k)g2(k)(k)(0-24)卡爾曼濾波器：對(duì)(0-23)取射影運(yùn)算引出濾波與預(yù)報(bào)關(guān)系TX1(k 1| k) = A)(k)*(k|k) G(k)

12、 J(k)y(k) J(k)Hz(k)A22(k)g2(k) (0-25)由(0-23)減(0-25)引出預(yù)報(bào)誤差與濾波誤差的關(guān)系Xi(k 1|k) =A，(k|k) ：(k)于是有關(guān)系匕(k 1| k) 二 E |x1(k 1| k)xT(k 1| k)Pii(k+1|k) =A0(k)P11(k|k)A(T(k) + E P(k)PT(k)l(0-26)應(yīng)用遞推射影公式可得X1(k +1| k +1) =X1(k 十1| k)十E x(k + 1)/(k +1)任k + 12T(k +1)“ k+1) (0-27)其中新息表達(dá)式為(0-28)(k 1) = y(k 1)-?(k 1|k)(

13、k 1) =C0(k)x1(k 1|k) (k 1)應(yīng)用(0-29)x1(k 1) =%(k 1|k) x1(k 1| k)得出E x1(k 1)；T(k 1)=E (R(k+1| k)+x1(k +1| k) X 7(k + 1)x1(k 十1| k)十"(k +1) (0-30)=印 1|k)CT(k 1)E 式k +1)wT(k + 1) = C0(k +1)P1(k + 1| k)CT (k + 1)十年(k +1)(0-31)于是有£(k+1 k+1)=x1(k+1|k)+(P11(k+1| k)CT(k +1)(0-32)C0(k 1)匕(k 1|k)CT(k

14、1)'(k 1)；(k 1)其中濾波增益TT1Kf(k) = n1(k|k-1)CT(k)(C0(k)P11(k|k-1)C；(k)+(k) )(0-33)由濾波誤差x1(k+1| k + 1) = x1(k+1)-x1(k+1| k+1)得到預(yù)報(bào)誤差x1(k 1| k) =x1(k 1) - x1(k 1| k) Kf (k 1) ；(k 1)-(0-34)=x1(k 1| k 1) -Kf (k 1) ；(k 1) 利用 x1(k+1| k+1) _L “k +1)引入P11(k 1| k 1) -E |x)(k 1| k 1)xT(k 1| k 1)(0-35)或P1(k+1k+

15、1) = P1(k+1|k) P1(k +1|k)CT(k)t .(0-36)(C0(k)P1 (k |k-1)CT (k) +" (k) ) C°(k)R1 (k +1| k)(0-37)P1(k 1|k 1) -|In -Kf(k 1)C0(k) P11(k 1|k)在保證P1(k | k)正定的前提下，進(jìn)行 cholesky分解P,1(k |k) = SST。令 Pi(k+1|k)=P", Ri(k+1k+1) = p+ Pi(k|k) = P , Qp=EP(k)P(k)T 將預(yù)測(cè)狀態(tài)協(xié)方差式(0-26)簡寫成：P-= A0PA0T +Qp,這里P-代表預(yù)測(cè)狀態(tài)協(xié)方差，