def test_ekf_state_propagation_initialised_correctly():
est = EKFSLAM()
assert est.X.shape == (3, )
assert est.P.shape == (3, 3)
assert est.Q.shape == (2, 2)
assert assert_array_equal(est.X[:3], np.array([0, 0, 0])) is None
def test_ekf_state_propagation_forward_control_input_updates_state_vector_x_position(
):
est = EKFSLAM()
# est.X[:3] = [0, 0, 0]
est.P = np.zeros((3, 3)) * 1.
est.Q = np.array([[0.2, 0], [0, 0.3]])
est.state_and_state_cov_propagation([1, 0])
assert assert_array_almost_equal(est.X, [1, 0, 0.]) is None
est.state_and_state_cov_propagation([1, 0])
assert assert_array_almost_equal(est.X, [2, 0, 0.]) is None
def test_ekf_state_propagation_state_covariance_matrix_remains_zero_if_zero_process_noise(
):
est = EKFSLAM()
# est.X[:3] = [0, 0, 0]
est.P = np.zeros((3, 3)) * 1.
est.Q = np.zeros((2, 2))
est.state_and_state_cov_propagation([1, 0])
assert assert_array_equal(est.P, np.zeros((3, 3))) is None
def test_jacobian_f_X_r_at_specific_test_points_compared_to_auto_diff():
x_u, x_n, alpha_r, alpha_u, alpha_n = 0., 0.1, np.deg2rad(45.), np.deg2rad(
0.), np.deg2rad(1.)
# x_u, x_n, alpha_r, alpha_u, alpha_n = 2.3, 0.1, np.deg2rad(45.), np.deg2rad(5.), np.deg2rad(1.)
X = np.array([0, 0, alpha_r])
U = np.array([x_u, alpha_u])
n = np.array([x_n, alpha_n])
f = EKFSLAM.state_propagation
F_x = EKFSLAM.jacobian_f_X_r(x_u, x_n, alpha_r, alpha_u, alpha_n)
# compare to automatic differentiation result (differentiate w.r.t. X vector)
J = jax.jacfwd(f, argnums=0)(X, U, n)
assert assert_array_almost_equal(J, F_x) is None
def test_ekf_state_propagation_angled_control_input_updates_state_vector_x_and_y_position(
):
est = EKFSLAM()
# est.X[:3] = [0, 0, 0.]
est.P = np.zeros((3, 3)) * 1.
est.Q = np.array([[0.2, 0], [0, 0.3]])
est.state_and_state_cov_propagation([1, np.deg2rad(45.)])
assert assert_array_almost_equal(
est.X, [1. / np.sqrt(2), 1. / np.sqrt(2),
np.deg2rad(45.)]) is None
est.state_and_state_cov_propagation([1, 0])
assert assert_array_almost_equal(
est.X, [2. / np.sqrt(2), 2. / np.sqrt(2),
np.deg2rad(45.)]) is None
def test_ekf_state_propagation_forward_control_input_updates_state_covariance(
):
est = EKFSLAM()
# est.X[:3] = [0, 0, 0]
est.P = np.zeros((3, 3)) * 1.
est.Q = np.array([[0.2, 0], [0, 0.3]])
est.state_and_state_cov_propagation([1, 0])
# TODO: need to double check that these values are as expected
assert assert_array_almost_equal(
est.P, np.array([[0.2, 0, 0], [0, 0.3, 0.3], [0, 0.3, 0.3]])) is None
est.state_and_state_cov_propagation([1, 0])
assert assert_array_almost_equal(
est.P, np.array([[0.4, 0, 0], [0, 1.5, 0.9], [0, 0.9, 0.6]])) is None
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)