particle_filter.py 文件源码

def __f(self, x):
        '''??x(k+1)??
            ??????x(k+1) = A * x(k) + B * u(k)
        ???
            x???x(k)
        ????
            x_next???x(k+1)
        '''
        yaw = x[2, :]
        a = self.__DT_s * np.cos(yaw)
        b = self.__DT_s * np.sin(yaw)
        c = np.full_like(a, self.__DT_s)
        u = np.array([a,
                      b,
                      c])

        x_next = (self.__A @ x) + (self.__B @ u)

        for i in range(x_next.shape[1]):
            x_next[2, i] = limit.limit_angle(x_next[2, i])

        return x_next