Пример #1
0
def sample_from_odometry(state, motion, alphas):
    """
    Predicts the next state (a noisy version) given the current state, and the motion command.

    :param state: The current state of the robot (format: [x, y, theta]).
    :param motion: The motion command (format: [drot1, dtran, drot2]) to execute.
    :param alphas: The motion noise parameters (format: [a1, a2, a3, a4]).
    :return: A noisy version of the state prediction (format: [x, y, theta]).
    """

    assert isinstance(state, np.ndarray)
    assert isinstance(motion, np.ndarray)
    assert isinstance(alphas, np.ndarray)

    assert state.shape == (3, )
    assert motion.shape == (3, )
    assert alphas.shape == (4, )

    a1, a2, a3, a4 = alphas
    drot1, dtran, drot2 = motion
    noisy_motion = np.zeros(motion.size)

    # sample1d ~ output a single sample (one number) from normal distribution N(mean,standard_deviatiopn)
    noisy_motion[0] = sample1d(drot1,
                               np.sqrt(a1 * (drot1**2) + a2 * (dtran**2)))
    noisy_motion[1] = sample1d(
        dtran, np.sqrt(a3 * (dtran**2) + a4 * ((drot1**2) + (drot2**2))))
    noisy_motion[2] = sample1d(drot2,
                               np.sqrt(a1 * (drot2**2) + a2 * (dtran**2)))

    # Ex: if posision.x_prev == 10, motion = 1, noisy_motion = 1.12, position.x = 11.12

    return get_prediction(
        state, noisy_motion
    )  # let's see where do we get ith this noisy motion (in accordance with our transformation(motion) function)
Пример #2
0
def sample_from_odometry(state, motion, alphas):
    """
    Predicts the next state (a noisy version) given the current state, and the motion command.

    :param state: The current state of the robot (format: [x, y, theta]).
    :param motion: The motion command (format: [drot1, dtran, drot2]) to execute.
    :param alphas: The motion noise parameters (format: [a1, a2, a3, a4]).
    :return: A noisy version of the state prediction (format: [x, y, theta]).
    """

    assert isinstance(state, np.ndarray)
    assert isinstance(motion, np.ndarray)
    assert isinstance(alphas, np.ndarray)

    assert state.shape == (3,)
    assert motion.shape == (3,)
    assert alphas.shape == (4,)

    a1, a2, a3, a4 = alphas
    drot1, dtran, drot2 = motion
    noisy_motion = np.zeros(motion.size)

    noisy_motion[0] = sample1d(drot1, np.sqrt(a1 * (drot1 ** 2) + a2 * (dtran ** 2)))
    noisy_motion[1] = sample1d(dtran, np.sqrt(a3 * (dtran ** 2) + a4 * ((drot1 ** 2) + (drot2 ** 2))))
    noisy_motion[2] = sample1d(drot2, np.sqrt(a1 * (drot2 ** 2) + a2 * (dtran ** 2)))

    return get_prediction(state, noisy_motion)