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)
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)