第2期 伍明，等，一种机器人未知环境下动态目标跟踪交互多模滤波算法 ·133· 的值，A红+为目标运动模型状态转移阵，Q+1k为状 FRgC(R:,T)(AI). 态误差阵。 同样，机器人与特征1目标与特征的协方差子阵 机器人和目标之间协方差子阵预测值为 预测值分别为 C(R,T)=FxgC(R,T)(AI).(32) C(Rk-uk,Im)=FxgC(R:,Im:), (33) 其推导如下： C(Tk-uk,Im:)AtC(R&,Im;). (34) 因为C(R+k,T)=FC(R,T), (I) 综上所述，最终系统协方差阵变换如下， C(R,T)=C(R,T:)(AI),(I) 所以由式(I)、（Ⅱ)可得：C(R+k,Tk+k)= P=ConvariancePredict(P:",At,A)= C(Rs+k,Rs+e）C(Rk+k,Ts+）C(Rk+nk,lm1）…C(Rk+Hk,lmn) C(T4k,R4k)C(Tk4k,Tk+)C(Tk+k,m）… C(Tk+k,lmn） C(Im1,Rg+uk) C(m1,Tk+） C(Im,Im) C(Im,Im) (35) C(Im ,R+uk) C(Im,Tx-uk) C(Im,Im) C(lmn,lmn）J 2.2.3更新阶段 Hit 当观测值出现时，EKF将对系统状态向量和协 方差阵进行更新从而纠正系统的误差.这里假设已 Hk+1= (36) 经完成数据关联工作，即已知观测值和目标、特征的 HT 对应关系.设观测值分为对于目标的观测值z+和 式中：H!和H为2×dim(Xk+)的矩阵，Hg+1为 对于特征的观测值z1,i=1,…,m+1（m+1为此次 目标状态子向量+和机器人状态子向量+1k 观测到的特征数量)2部分组成。 对应目标观测值z的观测矩阵.其值为 下面首先需要建立此次系统的观测矩阵Hk+1, 该矩阵形式如式(36)： H4= h LaXklke aXu6 02x (37) ”2x(9+2n) 式中： x处-x 发E-yE dhr √(k-x)+(y4a-y √(+s-x)+(4k-4) aXi-u 38) - 是一e √(4+k-经)2+(yt-y2了 √(+t-4)+(y4k-y发)了 - yiu-yhcu ohT √(k-)+(g+k-) V(k-发+)'+(a-么4了 aXuk 02x yheu -yict k一 √(s-xe)+(y-发e)了 √(k-+e)+(yk-发E了 (39) 式(38)、(39)分别为式(4)对于X1和X+的雅 假设每个观测值，与系统状态变量中特征的 可比阵。 对应关系已经确定，即，z对应系统状态向量中的