第2期 伍明，等，一种机器人未知环境下动态目标跟踪交互多模滤波算法 ·131· 此时系统的已知特征数量为n+1. 1)针对目标的系统协方差阵扩展.假设系统状 在对系统状态向量完成扩展后还须对系统协方 态向量已经包括n个特征向量（不包括特征的情况 差阵P.进行扩展，同样分别对目标和新特征分别进 类似)，则针对目标T系统协方差阵扩展方法如下. 行说明. C(R,R:) C(Rk,m1） C(Rx,Im,) C(m1,R)C(lm1,lm1）…C(lm1,lmn) augment(P:,CE.g,CE:): C(mn,Rs)C(mn,m1）…C(mn,lmn） 扩展前 C(Rx,R) C(Rk,T)C(Rk,lm1)… C(Rk,lmn） C(T:,R) C(Tk,T)C(Tk,lm)… C(T,Im,) C(m1,R)C(lm1T)C(lm1,lm1)…C(lm1,lmn)） (12) C(Im,R)C(Im,T)C(Im,Im)..C(Im,Im)- 扩展后 式中 CE= converse() C(T,R)=(C(R,T.))=CExgC(Rx,R), z (13) cos(0+y)-di sin(+) (17) C(T:,T:)=CExC(R:,R:)(CExs)T sin(0+) d话cos(0瞻+y) CE:R"(CE:t)", (14) 式(16)、(17)分别为式(8)对机器人状态X和目 C(T:,Im;)=(C(Imi,T:))= 标规测值z的雅可比阵，式(13)~(15)分别由误 CExgC(Rx,Im;),i=1,2,...n (15) 差传播公式得到. a converse() 2)针对新发现目标的系统协方差阵扩展.这里 OXR 假设此时系统状态向量已经包括目标状态和n个特 [1 0 -d sin(0+) 征状态（不包含的情况可以类似得到），以下用m+ Lo 1 (16) di cos() 代表新发现的特征，那么系统协方差阵扩展如下， C(R:,R:)C(R:,T:)C(R:,Im).C(R:,Im,) C(Tx,R) C(Tk,Tk）C(Tk,lm1)…C(Tk,lmn) augment(P:,CExg,CE): C(Im,R:)C(Im,Tx)C(im,Im)..C(Im,Im) -C(mn,Rg)C(lmn,Tk)C(lmn,lm）…C(mn,lmn） 扩展前 C(R,R) C(Rx,Tx) C(R,1m1) C(RIm C(Rk,lma+i） C(T,Rx) C(Ti,T) C(Tk,1m1）… C(T&,Im,) C(Tx,Im) C(Im,Rx) C(m1,T)C(lm1,lm1)… C(lm1,lmn） C(lm1,lm+l） (18) C(Im,Rx) C(Im,,T:)C(Im,,Im) C(Im.Im. C(Im,Im) C(lml,Rs)C(m+1,T）C(m+1,m1）…C(m+1,m,）C(ml,mi) 扩展后