平方根扩展卡尔曼滤波.docx

《平方根扩展卡尔曼滤波.docx》由会员分享，可在线阅读，更多相关《平方根扩展卡尔曼滤波.docx（6页珍藏版）》请在第一文库网上搜索。

1、考虑随机非线性离散广义系统(0-1)(0-2)Mx(k + 1) = fx(k k) g(x(Z), k)w(k)y(k + 1) = h(x(k + 1),/: + 1) + vk + 1)其中：火为离散时间，状态变量x(6况，观测变量y(2)9,输入噪声以外,观测噪声以0，(),和力()对式外是可微的，加况、是奇特别数矩阵,rank M = r minp。假设1处伏)和仇行是零均值的不相关白噪声w()-v()-w7(y) vU)=Q(k)S(k)S(k)R(k)其中E为均值符号，T为转置符号，=1, Oj=0(kj).假设2非线性广义系统(0-l)(0-2)是强可控的。假如在Z时刻，状态变

2、量x(Q的最小方差线性估值2(ZZ)已知，则可以将非线性系统(0-1)在 X(k) = x(k | k), (0-2) x(k) = x( + l)四周进行 Taylor 级数绽开,只保留一次项，近似可得M(k D= 铀 I k),k)+嗒浮 I (M 幻-次 I 初+g(MD )(0-3)y* + 1) =砥x(Z +1 出,幻 + 笠Lzy % (x(Q -虱k | 幻)+ V* +1) (0-4)其中：离散状态一步猜测为x(k + k) = f(x(kkk)，并认为g(x(k),k) = g(x(k | k),k) o于是离散系统的模型就可以表示为Mxk + 1) = ( + 1 幻双&

3、) + (f(A | k)-(1 k)x(k | &) + (k) (0-5)y(k + 1) = H* + l)x(k + 1) + (x(k + 1) + 1)-H(k l)x( +11Q + 优左 +1)(0-6)其中0(Z + 1A) 二等x (k)(0-8)口/7加”j_加工(4 + 1),氏 + 1)H(k + V)xk + 1)- (k +1)l(k+i)=(k+M)，r（k） = g（虱 kk）,k）（0-9）由于却1+1Q = 仪眉行水）,即（女+1出）与人女伏）在）时刻的取值相关,所以在Z + 1时刻，乳l + lZ）为已知的常数,从而以加t + 1伙）,左+ 1）在与+

4、1时刻也是已知的常数，同时在k+ 1时刻H（Z + 1）也是已知的。由于成M = r，依据矩阵奇异值分解，存在正交矩1）阵U 况”，V 况,使得UMV(0-10)式中 = diag（,b2,br）,9（，=1,2.）矩阵加的奇异值。令x（%） = V（x1r（A）,%；/） J用U左乘式（0-5）的两端并把式（0-10）代入（0-5）应用U(k)V =4(女)人屋64(幻 A22(k),U(k)=（心）、r（机,H(k)V = (H仆)%(幻)推导系统得E 0x1( + l)0 0x2(Zs + l)/= U(k)V+ U/第 1卧0（咽湍）+ U(k)w(k)(0-11)x1( + 1)x)

5、(攵 +1)Al()42/)&(%) 42(*)+ (女) G(Z)w(Z)(0-12)分解得到x ( + 1) = Ali (k)xl (&) + 12()x2() + x ()vv() + & (k)(0-13)0 = A2l (k)xi (k) + A22 (k)x2 (k) + 2(k)w(k) + g2(k)(0-14)y(k) = Hx (k)xl (%) + W2()2() + v()(0-15)其中玉（幻9Y,工2（6况一Q13）9-15）为非方广义系统的奇异值标准形式。由式（014）可以得到-A22(k)x2(k) = A2i(k)xl (左)+ 2()w() + g2(k)

6、(0-16)当rank = vmp - r,n- r,故存在伪逆&(Q,左乘(0-13)得到下式九2(%) = -42(幻&(幻芯(幻一修(%於2(幻一42(%)(幻以幻(0-17)将(017)代入(013)(0-15)得到一个r维子系统%1( + l) = 4(Z)(R) + G(Z) + (Z)w(Z)(0-18)y(k) = C(k)&*) H 2*) &(k) g 2*) + (k)(0-19)其中4(幻=r14伏)一万幺2出62(幻&(，G(Z) =修(幻一”2(幻片(幻&(幻，G(k) = -igl(k)-iAi2(k)2g2(k) f (Z) = %(&)-尸/(幻孤出心，(k)

7、 = -H2(k)A2(k)2(k)w(k) + v(k),亍(q = ew(%M/(/) = (s(z)q(z)G(q(z)h；(q)b，(k) = E7(6zf() =r(Z) + 2(QA；2(Q，2(Z)Q(Z)G(kXX2(k)(k)-S1 (k)r.k)(Ak H； (k) - %(k)A(匚(Z)S(Z) fij首先将相关噪声系统转化为带不相关噪声系统。在(0-18)式右边形式地加一项等于零的项：%( +1) = 4丙(女)+ G(Z) + (攵)以女)+ J(k) y(A) - C()(Z)X (%) + H 4k)必(k) g ?(k) - (攵)(0-20)其中J(左)是待

8、定矩阵，记4)(幻=4(幻/(攵)勒(幻(k) = a(k)w*) - J (k)(k)P(Z) = (Qw(Z)-/(%)2(%)&(攵)2(卬(%)+ 贝幻(k) =。(幻 + J(k)H2(k)A;2(k)2(k) w(k) - Jg(k)则状态方程(0-20)化为王(攵+ 1) = 4(攵M(Z) + G(Z) + J(Z)y0l) + 0l)2(Z)Aj2(Z)g2(6 + P(Z)(0-21)观测方程仍为(0T9),留意以幻仍为白噪声，且有E0(k) = O,/?(攵)/ (左)=E(Z)w(Z) - J(Q(Q)7f (%)E(Z)f (Z) = E=(6(S(Z) - Q(Z)

9、K(Z)424)H；(Z) - J(Z)z7(Z)(Z)由上式看到，取J(幻为J(k) = a(k)(S(k) - Q(k)F；(6修，(k)H；斤 *)(0-22)其中(k) =耳根6/式刈幻+也出人口幻心伏发出；(Q(62(幻PH 也)-S (k)F2(kXA；2 (k) H1(k)- H2(k)A;2 匚(A)S(A) ,则伙幻于 (%)不相关,即E(k)(j) = ykJ易知白噪声以Z)的协方差阵为E(k)(k) = (A) + J(Q2(6抽2(2)。(须。出 + J(k)H2(k)A;2(k)2(k)-(次幻+攵巩(幻修(6式幻)S(幻小)- Jr(QS(幻(Z) + J供)乩(女

10、内(Q(Q)7+J(k)R*)jT(k)从而得到噪声不相关系统：x(Z + l) = 40()x1 (k) + G() + J(k)y(k) + J(k)H2(k)A2(k)g2(k) + k)(0-23)y(幻= G(Z)F(%)-2(幻名(2必2(幻+ (%)(0-24)卡尔曼滤波器：对(0-23)取射影运算引出滤波与预报关系%l( + l | k) = A0(k)xi(k | k) + G(k) + J(k)y(k) + J(k)H2(k)A2(k)g2(k)(0-25)由(0-23)减(0-25)引出预报误差与滤波误差的关系xl(fe + l | ) = A0xi(kk) + (k)于

11、是有关系1( + 1) = ex1( + 1)(k + l 攵) + ( +1)= %(A + 1A)C(A + 1)E(k +1)/伏 +1) = C()(k +1)% (左 +11 k)C(k + 1) + (k +1)于是有1(% + 1|% + 1) = 3(攵 + 1|幻 + (耳(女 + 1|幻品( + 1)(C(女 +1)%(A +11 A)C；( + l) + 小k + l)f k +1)其中滤波增益K/(幻=% (Z | - 1)C;(Z) (c(Z)片(Z | Z 1)C;(2) + 万(6厂由滤波误差后(后+ 1|4+ 1)=,(攵+ 1)-岳(4+ 1|1+ 1)得到预

12、报误差xl(l I ) = X(Z +1) X(4 + 11 Z) + K(Z + l)e(Z + 1)=X (% +11Z +1) K (Z + l)g(k + 1)采用用(女 + 1k+ 1)，伏+ 1)引入(0-30)(0-31)(0-32)(0-33)(0-34)6(A + lA + l) = e1(k + lA + l)W(A + lA + l)匕( + 1|% + 1)=品( + 1攵)兄(2 + 1|攵)。：(幻()(%)%(& | k -l)Cj() + 万(6厂 C0(Q耳(A +11 外(0-35)(0-36)品( +1 Z +1) = 口 -Kf(k + l)C0a)%( +11Q(0-37)在保证1(ZZ)正定的前提下，进行cholesky分解片(Z |攵)=SS，。令凡(Z + 1Z) = P-，匕(Z + lZ + l) = P+, &(kk) = P, Q = E(k)(k)将猜测状态协方差式(0-26)简写成：产=A*+Q”,这里P一代表猜测状态协方差，且要求P = SS7假设已经进行了分解或=而也f,则有：p- = a47=4ss7%+，其中q,A=4)s 国=；a