def __init__(self, alphas, sensor_cov, num_states, num_landmarks, ts=0.1): self.g = MotionModel(alphas, noise=False) self.h = MeasurementModel() self.Q = sensor_cov self.dt = ts self.n = num_states self.N = num_landmarks self.mu_x = np.zeros((num_states, 1)) self.mu_m = np.zeros((2 * num_landmarks, 1)) self.sig_xx = np.zeros((num_states, num_states)) self.sig_xm = np.zeros((num_states, 2 * num_landmarks)) self.sig_mm = np.eye(2 * num_landmarks) * 100.
def __init__(self, alphas, sensor_cov, num_particles, num_landmarks, \ ts=0.1, avg_type='mean'): self.g = MotionModel(alphas, noise=True) self.h = MeasurementModel(num_particles, calc_jacobians=True) self.Q = sensor_cov self.dt = ts self.N = num_particles self.NL = num_landmarks self.chi = np.zeros((4, num_particles)) self.chi[-1] = 1 / num_particles self.mu_m = np.zeros((num_particles, num_landmarks, 2, 1)) self.sig_m = np.empty((num_particles, num_landmarks, 2, 2)) self.type = avg_type self._update_belief()
def __init__(self, alphas, Q, num_particles=1000, limits=[-5, 5, -5, 5], landmarks=np.empty(0)): self.Q = Q.diagonal().reshape((len(Q), 1)) self.M = num_particles self.landmarks = landmarks self.g = MotionModel(alphas, noise=True) self.h = MeasurementModel() self.chi = np.empty((4, num_particles)) self.chi[0] = np.random.uniform(limits[0], limits[1], self.chi[0].shape) self.chi[1] = np.random.uniform(limits[2], limits[3], self.chi[1].shape) self.chi[2] = np.random.uniform(-np.pi, np.pi, self.chi[2].shape) self.chi[3] = 1 / num_particles self.mu = wrap(np.mean(self.chi[:3], axis=1, keepdims=True), dim=2) mu_diff = wrap(self.chi[:3] - self.mu, dim=2) self.sigma = np.cov(mu_diff) self.z_nan = 50 self.z_hat = np.ones((2, len(self.landmarks))) * self.z_nan self.z = np.ones((2, len(self.landmarks))) * self.z_nan self.n = 3
def __init__(self, sensor_cov, motion_cov, ts=0.1, mu0=np.zeros((3, 1)), sigma0=np.eye(3), landmarks=np.empty(0)): self.Q = sensor_cov self.Q_inv = np.linalg.inv(sensor_cov) self.M = motion_cov self.g = MotionModel(motion_cov, ts, noise=False) self.h = MeasurementModel() self.landmarks = landmarks self.mu = mu0 self.sigma = sigma0 self.info_mat = np.linalg.inv(self.sigma) self.info_vec = self.info_mat @ self.mu
class ExtendedInfoFilter: def __init__(self, sensor_cov, motion_cov, ts=0.1, mu0=np.zeros((3, 1)), sigma0=np.eye(3), landmarks=np.empty(0)): self.Q = sensor_cov self.Q_inv = np.linalg.inv(sensor_cov) self.M = motion_cov self.g = MotionModel(motion_cov, ts, noise=False) self.h = MeasurementModel() self.landmarks = landmarks self.mu = mu0 self.sigma = sigma0 self.info_mat = np.linalg.inv(self.sigma) self.info_vec = self.info_mat @ self.mu def predictionStep(self, u): # line 2 is done after correction step for plotting reasons # do line 5 1st to get jacobians self.mu = self.g(u, self.mu) # line 5 Gt, Vt = self.g.jacobians() # map noise from control space to state space R = Vt @ self.M @ Vt.T self.sigma = Gt @ self.sigma @ Gt.T + R # line 3 (w/o inverse) self.info_mat = np.linalg.inv(self.sigma) # line 3 (take inverse once) self.info_vec = self.info_mat @ self.mu # line 4 return self.mu def correctionStep(self, z): zhat = np.zeros((2, len(self.landmarks))) for i, (mx, my) in enumerate(self.landmarks): zhat[:, i:i + 1] = self.h(self.mu, mx, my) # do h(mu) 1st to get Hi Hi = self.h.jacobian() HiT_Qinv = Hi.T @ self.Q_inv self.info_mat += HiT_Qinv @ Hi # line 6 # DON'T WRAP (Hi * mu)! Lots of issues if you do # Do wrap (z - zhat) or else theta error will spike a few times innovation = wrap(z[:, i:i + 1] - zhat[:, i:i + 1], dim=1) + Hi @ self.mu self.info_vec += HiT_Qinv @ innovation # line 7 # convert back to moment space for plotting self.sigma = np.linalg.inv(self.info_mat) self.mu = wrap(self.sigma @ self.info_vec, dim=2) return self.info_vec, self.mu, self.sigma, zhat
def __init__(self, sensor_cov, motion_cov, ts=0.1, x0=np.zeros((3, 1)), landmarks=np.empty(0)): self.Q_sqrt = np.sqrt(sensor_cov) self.g = MotionModel(motion_cov, ts, noise=False) self.h = MeasurementModel() self.x = wrap(x0, dim=2) self.landmarks = landmarks
def __init__(self, alphas, sensor_covariance, x0=np.zeros((3, 1)), ts=0.1, landmarks=np.empty(0), fov=360): self.g = MotionModel(alphas, noise=True) self.h = MeasurementModel() self.Q_sqrt = np.sqrt(sensor_covariance) self.x = wrap(x0, dim=2) self.dt = ts self.landmarks = landmarks self.bearing_lim = rad(fov / 2)
class FastSLAM: def __init__(self, alphas, sensor_cov, num_particles, num_landmarks, \ ts=0.1, avg_type='mean'): self.g = MotionModel(alphas, noise=True) self.h = MeasurementModel(num_particles, calc_jacobians=True) self.Q = sensor_cov self.dt = ts self.N = num_particles self.NL = num_landmarks self.chi = np.zeros((4, num_particles)) self.chi[-1] = 1 / num_particles self.mu_m = np.zeros((num_particles, num_landmarks, 2, 1)) self.sig_m = np.empty((num_particles, num_landmarks, 2, 2)) self.type = avg_type self._update_belief() def _update_belief(self): if self.type == 'mean': self.mu = wrap(np.mean(self.chi[:3], axis=1, keepdims=True), dim=2) self.mu_lm = np.mean(self.mu_m, axis=0) self.sig_lm = np.mean(self.sig_m, axis=0) elif self.type == 'best': idx = np.argmax(self.chi[-1]) self.mu = self.chi[:3, idx:idx + 1] self.mu_lm = self.mu_m[idx] self.sig_lm = self.sig_m[idx] self.sigma = np.cov(self.chi[:3]) def _low_var_resample(self): N_inv = 1 / self.N r = np.random.uniform(low=0, high=N_inv) c = np.cumsum(self.chi[-1]) U = np.arange(self.N) * N_inv + r diff = c - U[:, None] i = np.argmax(diff > 0, axis=1) n = 3 # num states P = np.cov(self.chi[:n]) self.chi = self.chi[:, i] self.mu_m = self.mu_m[i] self.sig_m = self.sig_m[i] uniq = np.unique(i).size if uniq * N_inv < 0.1: Q = P / ((self.N * uniq)**(1 / n)) noise = Q @ randn(*self.chi[:n].shape) self.chi[:n] = wrap(self.chi[:n] + noise, dim=2) # self.chi[-1] = N_inv def predictionStep(self, u): self.chi[:3] = self.g(u, self.chi[:3], self.dt) self._update_belief() return self.mu def correctionStep(self, z): zhat = np.zeros((2, self.NL)) self.chi[-1] = 1 for i in range(self.NL): # don't do anything when no measurement is received to landmark i if np.isnan(z[0, i]): zhat[:, i] = np.nan continue # if landmark has never been seen before, initialize it mx = self.mu_m[:, i, 0].flatten() my = self.mu_m[:, i, 1].flatten() if self.mu_m[:, i].item(0) == 0: r, phi = z[:, i] mx = self.chi[0] + r * np.cos(phi + self.chi[2]) my = self.chi[1] + r * np.sin(phi + self.chi[2]) self.mu_m[:, i, 0] = mx[:, None] self.mu_m[:, i, 1] = my[:, None] Zi = self.h(self.chi[:3], mx, my) H_inv = np.linalg.inv(self.h.jacobian()) self.sig_m[:, i] = H_inv @ self.Q @ H_inv.transpose((0, 2, 1)) continue Zi = self.h(self.chi[:3], mx, my) H = self.h.jacobian() sig_H_T = self.sig_m[:, i] @ H.transpose((0, 2, 1)) Si = H @ sig_H_T + self.Q Si_inv = np.linalg.inv(Si) Ki = sig_H_T @ Si_inv innov = wrap(z[:, i:i + 1] - Zi, dim=1) self.mu_m[:, i] += Ki @ innov.T[:, :, None] self.sig_m[:, i] = (np.eye(2) - Ki @ H) @ self.sig_m[:, i] exp = np.exp(-0.5 * innov.T[:, None, :] @ Si_inv @ innov.T[:, :, None]) z_prob = np.linalg.det(2 * np.pi * Si)**(-0.5) * exp.flatten() self.chi[-1] *= z_prob # normalize so weights sum to 1 z_prob /= np.sum(z_prob) zhat[:, i] = np.sum(z_prob * Zi, axis=1) self.chi[-1] /= np.sum(self.chi[-1]) self._low_var_resample() self._update_belief() return self.mu, self.sigma, self.chi, self.mu_lm, self.sig_lm, zhat
class EKFSlam: def __init__(self, alphas, sensor_cov, num_states, num_landmarks, ts=0.1): self.g = MotionModel(alphas, noise=False) self.h = MeasurementModel() self.Q = sensor_cov self.dt = ts self.n = num_states self.N = num_landmarks self.mu_x = np.zeros((num_states, 1)) self.mu_m = np.zeros((2 * num_landmarks, 1)) self.sig_xx = np.zeros((num_states, num_states)) self.sig_xm = np.zeros((num_states, 2 * num_landmarks)) self.sig_mm = np.eye(2 * num_landmarks) * 100. def predictionStep(self, u): self.mu_x = self.g(u, self.mu_x, self.dt) Gt, R = self.g.jacobians() self.sig_xx = Gt @ self.sig_xx @ Gt.T + R self.sig_xm = Gt @ self.sig_xm return self.mu_x def correctionStep(self, z): zhat = np.zeros((2, self.N)) for i in range(self.N): if np.isnan(z[0, i]): zhat[:, i] = np.nan continue idx = np.array([2 * i, 2 * i + 1]) mx, my = self.mu_m[idx] if mx == my == 0: r, phi = z[:, i] mx = self.mu_x[0] + r * np.cos(phi + self.mu_x[2]) my = self.mu_x[1] + r * np.sin(phi + self.mu_x[2]) self.mu_m[idx] = mx, my zhat[:, i:i + 1] = self.h(self.mu_x, mx, my) H1 = self.h.jacobian() H2 = -H1[:, :2] Hi = np.zeros((2, self.n + 2 * self.N)) Hi[:, :self.n] = H1 Hi[:, self.n + idx] = H2 sig_Hi_T = np.block( [[self.sig_xx @ H1.T + self.sig_xm[:, idx] @ H2.T], [(H1 @ self.sig_xm).T + self.sig_mm[:, idx] @ H2.T]]) inv = np.linalg.inv(H1 @ sig_Hi_T[:self.n] + H2 @ sig_Hi_T[self.n + idx] + self.Q) Ki = sig_Hi_T @ inv innovation = wrap(z[:, i:i + 1] - zhat[:, i:i + 1], dim=1) mu = Ki @ innovation self.mu_x += mu[:3] self.mu_m += mu[3:] self.mu_x = wrap(self.mu_x, dim=2) sigma = np.block([[self.sig_xx, self.sig_xm], [self.sig_xm.T, self.sig_mm]]) sigma = (np.eye(self.n + 2 * self.N) - Ki @ Hi) @ sigma self.sig_xx = sigma[:self.n, :self.n] self.sig_xm = sigma[:self.n, self.n:] self.sig_mm = sigma[self.n:, self.n:] return self.mu_x, self.sig_xx, self.mu_m, self.sig_mm