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