def test_robot_move_right_from_non_zero_position_and_45_deg_angle_when_applying_forward_control_input_without_noise():
x, y, alpha = 1., 3., np.deg2rad(45.)
r = np.array([x, y, alpha])
u = np.array([1, np.deg2rad(-90.)])
n = np.array([0, 0.])
r_new = move(r, u, n)
assert assert_array_equal(r_new, np.array([1.+1./np.sqrt(2), 3.-1./np.sqrt(2), np.deg2rad(-45.)])) is None
def test_robot_move_forward_from_zero_position_when_applying_forward_control_input_without_noise():
x, y, alpha = 0., 0, 0
r = np.array([x, y, alpha])
u = np.array([1, 0.])
n = np.array([0, 0.])
r_new = move(r, u, n)
assert assert_array_equal(r_new, [1, 0, 0.]) is None
def test_robot_move_left_from_non_zero_position_when_applying_left_control_input_without_noise():
x, y, alpha = 1., 3., 0.
r = np.array([x, y, alpha])
u = np.array([1, np.deg2rad(90.)])
n = np.array([0, 0.])
r_new = move(r, u, n)
assert assert_array_equal(r_new, np.array([1., 4., np.deg2rad(90.)])) is None
def state_propagation(X, U, n=None):
"""
Propagate the state estimates forward one time step using the control input
:param X: previous states
:param U: control input (d_x, d_alpha)
:param n: perturbation to control input (d_x, d_alpha)
:return:
"""
# ---------- propagate state vector (update robot pose but leave landmarks unchanged) -------
r = X[:3]
if n is None:
n = np.array([0, 0])
r_new = move(r, U, n)
# X[:3] = r_new
X_new = np.concatenate((r_new, X[3:]))
return X_new
def main():
# random seed
random.seed(20)
np.random.seed(20)
# simulation time step (Kalman filter propagation frequency) [s]
time_step = 0.020
# measurement update period [s]
measurement_period = 0.5
# total simulation duration [s]
sim_duration = 5.
# initial robot pose (x [m]; y [m]; alpha [deg])
r_true = [0., 0., np.deg2rad(90.)]
# angular_velocity [rad/sec]. Used to simulate random walk on the angle control
d_alpha = 0
# process noise (for control input [x; alpha])
u_x_stddev = 2.0 * time_step # [m]
u_alpha_stddev = np.deg2rad(4.0 * time_step) # [rad]
# measurement noise
range_meas_stddev = 0.1 # [m]
bearing_meas_stddev = np.deg2rad(5.) # [rad]
# landmarks ([meters])
min_x = -5.
max_x = 5.
min_y = 0.
max_y = 10.
num_landmarks = 2
landmarks_true = np.random.random_sample((num_landmarks, 2))
landmarks_true[:, 0] = min_x + landmarks_true[:, 0] * (max_x - min_x)
landmarks_true[:, 1] = min_y + landmarks_true[:, 1] * (max_y - min_y)
# EKF-SLAM estimator
est = EKFSLAM()
est.X = np.concatenate([r_true, est.X[3:]]) # we know the true robot pose initially
est.P = np.zeros((3, 3)) * 1.
est.Q = np.array([[u_x_stddev ** 2, 0], [0, u_alpha_stddev ** 2]])
est.R = np.array([[range_meas_stddev ** 2, 0], [0, bearing_meas_stddev ** 2]])
# simulate robot
for i in range(int(sim_duration / time_step)):
t_start = time.time()
# generate control input
control_input_type = "straight_with_noise"
if control_input_type == "straight_with_noise":
u = [2. * time_step, 0]
# generate perturbation
n = [u_x_stddev * np.random.randn(), d_alpha]
elif control_input_type == "straight_with_random_walk_angle_noise":
u = [2. * time_step, 0]
# generate perturbation
d_alpha = d_alpha + u_alpha_stddev * np.random.randn()
n = [u_x_stddev * np.random.randn(), d_alpha]
elif control_input_type == "straight_without_noise":
u = [2. * time_step, 0]
# generate perturbation
n = [0, 0]
elif control_input_type == "circle_with_noise":
u = [2. * time_step, np.deg2rad(15.) * time_step]
n = [u_x_stddev * np.random.randn(), d_alpha]
elif control_input_type == "circle_without_noise":
u = [2. * time_step, np.deg2rad(15.) * time_step]
n = [0, 0]
else:
raise ValueError("Use valid control input motion type")
# move robot
r_true = move(r_true, u, n)
# propagate EKF
est.state_and_state_cov_propagation(u)
print("States:", est.X, "\nP:", est.P)
# sensor readings of environment
raw_measurements = [rbs.observe_range_bearing(r_true, landmarks_true[j, :])
for j in range(landmarks_true.shape[0])] # TODO: add some noise to the measurements
# measurement update of EKF
# TODO: need to first add the landmarks before we can update them
# [est.measurement_update_range_bearing(y, ii) for ii, y in enumerate(raw_measurements)]
# estimated landmark positions (by using estimated robot pose and inverse sensor measurements)
landmarks_est = [rbs.inv_observe_range_bearing(r_true, m) for m in raw_measurements] # TODO: use estimated robot pose?
print(time.time() - t_start)
# plot robot and map
if i % 5 == 0:
print(i, "current pose:", r_true, ", control input:", u, ", noise:", n)
display(r_true, est, landmarks_true, landmarks_est, raw_measurements, i)