f_rect_gate.m

多目标跟踪 (三维)-算法matlab编制

function u=f_rect_gate(i,z_predict,s_predict,r,kg)
%u1:矩形门x坐标最小值
%u2:矩形门x坐标最大值
%u3:矩形门y坐标最小值
%u4:矩形门y坐标最大值
%u5:矩形门z坐标最小值
%u6:矩形门z坐标最大值

u1=z_predict(1)-kg*sqrt(s_predict(1,1)+r(1,1));%计算跟踪门边界，此为矩形跟踪门
u2=z_predict(1)+kg*sqrt(s_predict(1,1)+r(1,1));%x  
u3=z_predict(2)-kg*sqrt(s_predict(2,2)+r(2,2));%计算跟踪门边界，此为矩形跟踪门
u4=z_predict(2)+kg*sqrt(s_predict(2,2)+r(2,2));%y 
u5=z_predict(3)-kg*sqrt(s_predict(3,3)+r(3,3));%计算跟踪门边界，此为矩形跟踪门
u6=z_predict(3)+kg*sqrt(s_predict(3,3)+r(3,3));%z   
u=[u1 u2 u3 u4 u5 u6]';