正在加载图片...
第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) 扩展后
<<向上翻页向下翻页>>
©2008-现在 cucdc.com 高等教育资讯网 版权所有