def getMeasurements(state):
z = np.zeros_like(params.lms, dtype=float)
for i in range(z.shape[1]):
lm = params.lms[:, i]
ds = lm - state[0:2]
r = np.sqrt(np.sum(ds**2))
theta = np.arctan2(ds[1], ds[0]) - state[2]
# theta = unwrap(theta) #not sure if this should be here or down a few lines
z[0, i] = r + np.random.normal(0, params.sigma_r)
z[1, i] = theta + np.random.normal(0, params.sigma_theta)
z[1, i] = unwrap(z[1, i])
return z
def getMeasurements(state):
z = np.zeros_like(params.lms, dtype=float)
ds = params.lms - state[0:2].reshape(2, 1)
r = np.sqrt(np.sum(ds**2, axis=0))
theta = np.arctan2(ds[1], ds[0]) - state[2]
z[0] = r + np.random.normal(
0, params.sigma_r, size=r.size
) #Measurement noise seems to be what is making everything do really bad
z[1] = theta + np.random.normal(0, params.sigma_theta, size=theta.size)
z[1] = unwrap(z[1])
ind = np.argwhere(np.abs(z[1]) < params.fov)
z = z[:, ind][:, :, 0]
return z, ind
x_covar_hist = []
y_covar_hist = []
psi_covar_hist = []
K_hist = []
state = np.zeros(3)
dead_reckon = np.zeros(3)
mu = ekf.mu
Sigma = ekf.Sigma
for i in range(t.size):
#stuff for plotting
x_hist.append(state)
mu_hist.append(ekf.mu[:3])
err = state - ekf.mu[:3]
err[2] = unwrap(err[2])
err_hist.append(err)
x_covar_hist.append(ekf.Sigma[0, 0])
y_covar_hist.append(ekf.Sigma[1, 1])
psi_covar_hist.append(ekf.Sigma[2, 2])
Car.animateCar(state, ekf.mu[:2], dead_reckon, ekf.mu[3:],
ekf.Sigma[3:, 3:], ekf.lms_found)
plt.pause(0.02)
state = ekf.propagateState(state, v[i], w[i])
zt, lm_ind = getMeasurements(state)
ekf.update(zt, lm_ind, vc[i], wc[i])
dead_reckon = ekf.propagateState(dead_reckon, vc[i], wc[i])
fig1, ax1 = plt.subplots(nrows=3, ncols=1, sharex=True)