particle_filter.py 文件源码

def __resampling(self, px, pw):
        '''?????????
            ?????????(ESS)?????????????????????????
        ???
            px??????????
            pw??????????
        ????
            px???????????????
            pw???????????????
        '''
        ess = float(np.reciprocal(pw @ pw.T))
        if ess < self.__ESS_TH:
            pw_cum = np.cumsum(pw)
            base_id = np.arange(0.0, 1.0, self.__NP_RECIP)
            ofs = np.random.rand() * self.__NP_RECIP  # ?????????
            resample_id = base_id + ofs
            px_temp = np.copy(px)
            idx = 0
            for i in range(self.__NP):
                while resample_id[i] > pw_cum[idx]:
                    idx += 1
                px[:, i] = px_temp[:, idx]
            pw = np.copy(self.__pw_ini)  # ??????

        return px, pw