def residual_z_(z, zp):
     """
     z : observation, [r, alpha]
     zp : predicted observation
     """
     r_z = z - zp
     r_z[1] = util.wrap_around(r_z[1])
     return r_z
 def residual_x_(x, xp):
     """
     x : state, [x, y, theta]
     xp : predicted state
     """
     if dim == 3 or dim == 5:
         r_x = x - xp
         r_x[2] = util.wrap_around(r_x[2])
         return r_x
     else:
         return None
Beispiel #3
0
def SE2Dynamics(x, dt, u):
    assert (len(x) == 3)
    tw = dt * u[1]
    # Update the agent state
    if abs(tw) < 0.001:
        diff = np.array([
            dt * u[0] * np.cos(x[2] + tw / 2),
            dt * u[0] * np.sin(x[2] + tw / 2), tw
        ])
    else:
        diff = np.array([
            u[0] / u[1] * (np.sin(x[2] + tw) - np.sin(x[2])),
            u[0] / u[1] * (np.cos(x[2]) - np.cos(x[2] + tw)), tw
        ])
    new_x = x + diff
    new_x[2] = util.wrap_around(new_x[2])
    return new_x
    def gen_rand_pose(self, o_xy, c_theta, min_lin_dist, max_lin_dist, min_ang_dist, max_ang_dist):
        """Generates random position and yaw.
        Parameters
        --------
        o_xy : xy position of a point in the global frame which we compute a distance from.
        c_theta : angular position of a point in the global frame which we compute an angular distance from.
        min_lin_dist : the minimum linear distance from o_xy to a sample point.
        max_lin_dist : the maximum linear distance from o_xy to a sample point.
        min_ang_dist : the minimum angular distance (counter clockwise direction) from c_theta to a sample point.
        max_ang_dist : the maximum angular distance (counter clockwise direction) from c_theta to a sample point.
        """
        if max_ang_dist < min_ang_dist:
            max_ang_dist += 2*np.pi
        rand_ang = util.wrap_around(np.random.rand() * \
                        (max_ang_dist - min_ang_dist) + min_ang_dist + c_theta)

        rand_r = np.random.rand() * (max_lin_dist - min_lin_dist) + min_lin_dist
        rand_xy = np.array([rand_r*np.cos(rand_ang), rand_r*np.sin(rand_ang)]) + o_xy
        is_valid = not(map_utils.is_collision(self.MAP, rand_xy))
        return is_valid, [rand_xy[0], rand_xy[1], rand_ang]
    def update(self, z_t, x_t):
        """
        Parameters
        --------
        z_t : observation - radial and angular distances from the agent.
        x_t : agent state (x, y, orientation) in the global frame.
        """
        # Kalman Filter Update
        r_pred, alpha_pred = util.relative_distance_polar(
            self.state[:2], x_t[:2], x_t[2])
        diff_pred = self.state[:2] - x_t[:2]
        if self.dim == 2:
            Hmat = np.array([[diff_pred[0], diff_pred[1]],
                             [-diff_pred[1] / r_pred, diff_pred[0] / r_pred]
                             ]) / r_pred
        elif self.dim == 4:
            Hmat = np.array([
                [diff_pred[0], diff_pred[1], 0.0, 0.0],
                [-diff_pred[1] / r_pred, diff_pred[0] / r_pred, 0.0, 0.0]
            ]) / r_pred
        else:
            raise ValueError('target dimension for KF must be either 2 or 4')
        innov = z_t - np.array([r_pred, alpha_pred])
        innov[1] = util.wrap_around(innov[1])

        R = np.matmul(np.matmul(Hmat, self.cov), Hmat.T) \
                                + self.obs_noise_func((r_pred, alpha_pred))
        K = np.matmul(np.matmul(self.cov, Hmat.T), LA.inv(R))
        C = np.eye(self.dim) - np.matmul(K, Hmat)

        cov_new = np.matmul(C, self.cov)
        state_new = self.state + np.matmul(K, innov)

        if True:  #LA.det(cov_new) < 1e6:
            self.cov = cov_new
        self.state = np.clip(state_new, self.limit[0], self.limit[1])