正在加载图片...
第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对应系统状态向量中的
<<向上翻页向下翻页>>
©2008-现在 cucdc.com 高等教育资讯网 版权所有