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)