基于Python两种跟踪算法

忘记出处了，备份在此留着以后研究一下

def camshift(self,prob):
    count = 0
    #设置meanshift迭代终止条件
    flag_mean = True
    prob_search = prob[self.ystart:self,self.xstart.xend]
    #计算质心
    M00 = sum(prob_search)
    S = int(1.75 * sqrt(M00/256))
    for i in xrange(Height):
        for j in xrange(Width):
            M01+=(i-Height/2)*prob_search[i,j]
            M10+=(j-Width/2)*prob_search[i,j]
    xc = M10/M00
    yc = M01/M00
    #如果被跟踪物体没有了，则不显示方框，将窗口坐标调整到图像中心
    if M00 <= 100:
        self.flag_win = False
        self.xstart = (prob.shape[1]/2-Width/2)
        self.xend = (self.xstart+Width)
        self.ystart = (prob.shape[0]/2-Height/2)
        self.yend = (self.ystart+Height)
    else:
        self.flag_win = True
    #计算目标区域中心，作为 KALMAN 观测值
    self.xcore = (self.xstart+xc+self.xstart+Width)/2
    self.ycore = (self.ystart+yc+self.ystart+Height)/2
    #kalman 滤波
    xx,yy = self.Kalman(self.pre_xcore,self.pre_ycore,self.pre_xspeed,self.pre.yspeed)
    #更新中心坐标
    self.xstart = self.xcore1-(Width)/2
    self.xend = self.xcore1-(Width)/2
    self.ystart = self.ycore1-(Height)/2
    self.yend = self.ycore1+(Height)/2

def Kalman(self,pre_xcore0,pre_ycore0,pre_xspeed0,pre_yspeed0):
    eps = 1e-5
    #状态方程
    pre_Xk = np.matrix([pre_xcore0,pre_ycore0,pre_xspeed0,pre_yspeed0])
    pre_Xk = pre_Xk.T
    Fk = np.matrix([1,0,self.Dt,0],[0,1,0,self.Dt],[0,0,1,0],[0,0,0,1])
    Bm = np.matrix('0;0;;0;0')
    Uk1 = np.random.normal(0,0,1)
    Xk = Fk*pre_Xk+Bm*Uk1
    Xkx = Xk[0][0]]
    Xky = Xk[1][0]
    #观测方程
    Vk = np.matrix('1;1')*eps
    Hk = np.matrix('1,0,0,0;0,1,0,0')
    Zk = Hk*Xk+Vk
    #信息融合
    deltaX = self.xcore-Zk[0][0]
    deltaY = self.ycore-Zk[1][0]
    kal_Xcore = Xkx+deltaX
    kal_Ycore = Xky+deltaY
    kx = int(round(kal_Xcore[0,0]))
    return kx,ky