def recoverMeanAndCovar(
Chi
): # I think there is an issue recovering the mean around theta = pi b/c average between -pi and pi is 0
mu = np.mean(Chi, axis=1)
temp_x = Chi - mu.reshape((3, 1))
temp_x[2] = unwrap(temp_x[2])
Sigma = np.cov(temp_x)
return mu, Sigma
def getMeasurements(state):
ds = params.lms - state[0:2].reshape((2, 1))
r = np.sqrt(np.sum(ds**2, axis=0)) + np.random.normal(
0, params.sigma_r, size=(params.lms.shape[1]))
theta = (np.arctan2(ds[1, :], ds[0, :]) - state[2]) + np.random.normal(
0, params.sigma_theta, size=(params.lms.shape[1]))
theta = unwrap(theta)
z = np.array([[r.flatten()], [theta.flatten()]]).reshape((2, 3))
return z
def getMeasurements(state): #Will need to change if using not 360 deg vision
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.squeeze()
phi0 = params.theta0
state = np.array([x0, y0, phi0])
dead_reckon = np.array([x0, y0, phi0])
Chi = np.zeros((3, params.M))
Chi[0:2, :] = np.random.uniform(-10.0, 10.0, size=(2, params.M))
Chi[2, :] = np.random.uniform(-np.pi, np.pi, size=(params.M))
mu = np.mean(Chi, axis=1)
Sigma = np.cov(mu.reshape((3, 1)) - Chi)
for i in range(t.size):
#stuff for plotting
x_hist.append(state)
mu_hist.append(mu)
err = state - mu
err[2] = unwrap(err[2])
err_hist.append(err)
x_covar_hist.append(Sigma[0, 0])
y_covar_hist.append(Sigma[1, 1])
psi_covar_hist.append(Sigma[2, 2])
Car.animateCar(state, mu, dead_reckon, Chi)
plt.pause(0.02)
state = filter.propagateState(state, v[i], w[i])
zt = getMeasurements(state)
mu, Sigma, Chi = filter.update(mu, Sigma, Chi, zt, vc[i], wc[i])
dead_reckon = filter.propagateState(dead_reckon, vc[i], wc[i])
fig1, ax1 = plt.subplots(nrows=3, ncols=1, sharex=True)
x_hist = np.array(x_hist).T
psi_covar_hist = []
state = np.zeros(3)
dead_reckon = np.zeros(3)
Chi = np.zeros((3, params.M))
wp = np.ones(params.M) #Evenly distributed weights
mu = np.mean(Chi, axis=1)
Sigma = np.cov(mu.reshape((3, 1)) - Chi)
j = 0 #Index of the best particle
for i in range(t.size):
#stuff for plotting
x_hist.append(state)
mu_hist.append(mu)
err = state - mu
err[2] = unwrap(err[2])
err_hist.append(err)
x_covar_hist.append(Sigma[0, 0])
y_covar_hist.append(Sigma[1, 1])
psi_covar_hist.append(Sigma[2, 2])
Car.animateCar(state, mu, dead_reckon, Chi, filter.lm_filters[j])
plt.pause(0.02)
state = filter.propagateState(state, v[i], w[i])
dead_reckon = filter.propagateState(dead_reckon, vc[i], wc[i])
zt, ind = getMeasurements(state)
Chi, j, wp = filter.update(Chi, wp, zt, ind, vc[i], wc[i])
mu, Sigma = recoverMeanAndCovar(Chi)
mu[2] = unwrap(mu[2])