def _get_landmark_position(state, distance, bearing):
"""
Predicts the landmark position based on a current state and observation distance and bearing.
"""
angle = wrap_angle(state.theta() + bearing)
x_relative = distance * np.cos(angle)
y_relative = distance * np.sin(angle)
x = x_relative + state.x()
y = y_relative + state.y()
return gtsam.Point2(x, y)
def _get_motion_gtsam_format(motion):
"""
Predicts the landmark position based on a current state and observation distance and bearing.
"""
drot1, dtran, drot2 = motion
theta = drot1 + drot2
x = dtran * np.cos(theta)
y = dtran * np.sin(theta)
# Wrap the angle between [-pi, +pi].
theta = wrap_angle(theta)
return gtsam.Pose2(x, y, theta)
def _get_motion_prediction(state, motion):
"""
Predicts the next state given state and the motion command.
"""
x = state.x()
y = state.y()
theta = state.theta()
drot1, dtran, drot2 = motion
theta += drot1
x += dtran * np.cos(theta)
y += dtran * np.sin(theta)
theta += drot2
# Wrap the angle between [-pi, +pi].
theta = wrap_angle(theta)
return gtsam.Pose2(x, y, theta)