def update_motion(self, mean, cov):
"""
Updates all particles by a multivariate normally drawn xyp change.
"""
T_r0_r1 = util.xyp2ht(
np.random.multivariate_normal(mean, cov, self.count))
self.particles = np.matmul(self.particles, T_r0_r1)
def __init__(self,
count,
start,
posrange,
angrange,
polemeans,
polevar,
T_w_o=np.identity(4)):
self.p_min = 0.01
self.d_max = np.sqrt(
-2.0 * polevar *
np.log(np.sqrt(2.0 * np.pi * polevar) * self.p_min))
self.minneff = 0.5
self.estimatetype = 'best'
self.count = count
r = np.random.uniform(low=0.0, high=posrange, size=[self.count, 1])
angle = np.random.uniform(low=-np.pi, high=np.pi, size=[self.count, 1])
xy = r * np.hstack([np.cos(angle), np.sin(angle)])
dxyp = np.hstack([
xy,
np.random.uniform(low=-angrange,
high=angrange,
size=[self.count, 1])
])
self.particles = np.matmul(start, util.xyp2ht(dxyp))
self.weights = np.full(self.count, 1.0 / self.count)
self.polemeans = polemeans # global map data
self.poledist = scipy.stats.norm(loc=0.0, scale=np.sqrt(polevar))
self.kdtree = scipy.spatial.cKDTree(polemeans[:, :2], leafsize=3)
self.T_w_o = T_w_o
self.T_o_w = util.invert_ht(self.T_w_o)
self.Sigma = np.eye(3) * 1e-13
def update_motion(self, mean, cov):
# mean, cov: odometry data [x, y, heading]
T_r0_r1 = util.xyp2ht(
np.random.multivariate_normal(
mean, cov, self.count)) # SE(3) transformation of propagation
self.particles = np.matmul(self.particles, T_r0_r1)
def update_motion_improved(self, mean, cov, poleparams):
for i in range(self.count):
x_out, Pout = self.ukf(self.particles_pre[i], self.Pukf[i, :, :],
mean, cov, poleparams)
x_new = x_out + np.random.multivariate_normal([0, 0, 0], Pout, 1).T
self.particles[i] = util.xyp2ht(x_new)
self.Poutt = Pout
self.xukf = x_out
def estimate_pose(self):
if self.estimatetype == 'mean':
xyp = util.ht2xyp(np.matmul(self.T_o_w, self.particles))
mean = np.hstack([
np.average(xyp[:, :2], axis=0, weights=self.weights),
util.average_angles(xyp[:, 2], weights=self.weights)
])
return self.T_w_o.dot(util.xyp2ht(mean))
if self.estimatetype == 'max':
return self.particles[np.argmax(self.weights)]
if self.estimatetype == 'best':
i = np.argsort(self.weights)[-int(0.1 * self.count):]
xyp = util.ht2xyp(np.matmul(self.T_o_w, self.particles[i]))
mean = np.hstack([
np.average(xyp[:, :2], axis=0, weights=self.weights[i]),
util.average_angles(xyp[:, 2], weights=self.weights[i])
])
return self.T_w_o.dot(util.xyp2ht(mean))
def estimate_pose(self):
if self.estimatetype == 'mean':
xyp = util.ht2xyp(np.matmul(self.T_o_w, self.particles))
mean = np.hstack([
np.average(xyp[:, :2], axis=0, weights=self.weights),
util.average_angles(xyp[:, 2], weights=self.weights)
])
return self.T_w_o.dot(util.xyp2ht(mean))
if self.estimatetype == 'max':
return self.particles[np.argmax(self.weights)]
if self.estimatetype == 'best':
i = np.argsort(self.weights)[-int(0.1 * self.count):]
xyp = util.ht2xyp(np.matmul(self.T_o_w, self.particles[i]))
mean = np.hstack([
np.average(xyp[:, :2], axis=0, weights=self.weights[i]),
util.average_angles(xyp[:, 2], weights=self.weights[i])
])
zero_mean_particles = xyp - mean
zero_mean_particles[:, 2] %= 2 * np.pi #wrap between 0 and 2pi
self.Sigma = (
1 / xyp.shape[0]) * zero_mean_particles.T @ zero_mean_particles
return self.T_w_o.dot(util.xyp2ht(mean))
def estimate_pose(self):
if self.estimatetype == 'mean':
xyp = util.ht2xyp(np.matmul(self.T_o_w, self.particles))
mean = np.hstack([
np.average(xyp[:, :2], axis=0, weights=self.weights),
util.average_angles(xyp[:, 2], weights=self.weights)
])
return self.T_w_o.dot(util.xyp2ht(mean))
if self.estimatetype == 'max':
return self.particles[np.argmax(self.weights)]
if self.estimatetype == 'best':
"""
Skips the worst 10% of indices (the ones with least weight)
Guessing xyp is x, y and phi for rotation angle, same as paper
ht is transformation matrix of particle at time t??
mean is the weighted average x, y and phi multiplied with T_w_o
"""
i = np.argsort(self.weights)[-int(0.1 * self.count):]
xyp = util.ht2xyp(np.matmul(self.T_o_w, self.particles[i]))
mean = np.hstack([
np.average(xyp[:, :2], axis=0, weights=self.weights[i]),
util.average_angles(xyp[:, 2], weights=self.weights[i])
])
return self.T_w_o.dot(util.xyp2ht(mean))
def __init__(self,
count,
start,
posrange,
angrange,
polemeans,
polevar,
T_w_o=np.identity(4)):
self.p_min = 0.01
self.d_max = np.sqrt(
-2.0 * polevar *
np.log(np.sqrt(2.0 * np.pi * polevar) * self.p_min))
self.minneff = 0.5
self.estimatetype = 'best'
self.count = count
"""
r = #count times a uniformly random distance 0 -> posrange.
angle = #count times a random angle -pi -> pi
xy = the angles multiplied by distance (radius)
dxyp = xy and #count random angles from the anglerange
particles = #count points in 2D-space and an angle phi
weights = starts with uniform weight
"""
r = np.random.uniform(low=0.0, high=posrange, size=[self.count, 1])
angle = np.random.uniform(low=-np.pi, high=np.pi, size=[self.count, 1])
xy = r * np.hstack([np.cos(angle), np.sin(angle)])
dxyp = np.hstack([
xy,
np.random.uniform(low=-angrange,
high=angrange,
size=[self.count, 1])
])
self.particles = np.matmul(start, util.xyp2ht(dxyp))
self.weights = np.full(self.count, 1.0 / self.count)
self.polemeans = polemeans
self.poledist = scipy.stats.norm(loc=0.0, scale=np.sqrt(polevar))
self.kdtree = scipy.spatial.cKDTree(polemeans[:, :2], leafsize=3)
self.T_w_o = T_w_o
self.T_o_w = util.invert_ht(self.T_w_o)
def update_motion(self, mean, cov):
T_r0_r1 = util.xyp2ht(
np.random.multivariate_normal(mean, cov, self.count))
self.particles = np.matmul(self.particles, T_r0_r1)
def motion(self, Xin, mean, cov):
T_r0_r1 = util.xyp2ht(np.random.multivariate_normal(mean, cov))
return np.matmul(Xin, T_r0_r1)
def ukf(self, Xin, Pin, mean, cov, poleparams):
# Xin:matrix.
Z, z_pred = self.measurement(Xin, poleparams)
x_in = util.ht2xyp(Xin) #(3,)
Qukf = 0.5 * np.eye(x_in.shape[0])
Rukf = 0.01 * np.eye(z_pred.shape[0])
states = x_in.shape[0] # 3
observations = z_pred.shape[0] # 2*Nout
vNoise = Qukf.shape[1]
wNoise = Rukf.shape[1]
noises = vNoise + wNoise
if noises:
N = scipy.linalg.block_diag(Qukf, Rukf)
P_q = scipy.linalg.block_diag(Pin, N)
x_q = np.vstack([x_in.reshape(-1, 1), np.zeros((noises, 1))])
else:
P_q = Pin
x_q = x_in.reshape(-1, 1)
xSigmaPts, wSigmaPts, nsp = self.scaledSymmetricSigmaPoints(x_q, P_q)
'''
xSigmaPts (3+num_noise,nsp)
wSigmaPts (nsp+1,)
'''
wSigmaPts_xmat = np.tile(wSigmaPts[1:nsp],
(states, 1)) # (3+num_noise,nsp-1)
wSigmaPts_zmat = np.tile(wSigmaPts[1:nsp], (observations, 1))
xPredSigmaPts = np.zeros((states, nsp))
zPredSigmaPts = np.zeros((states, nsp))
for i in range(nsp):
x_pred = self.motion(
util.xyp2ht(xSigmaPts[:states, i].reshape(-1)), mean,
cov) + xSigmaPts[states:states + vNoise, i]
# xPredSigmaPts.append(util.ht2xyp(x_pred).reshape(3,1))
xPredSigmaPts[:, i] = x_pred.reshape(states, 1)
z_pred = self.measurement(
util.xyp2ht(x_pred),
poleparams).reshape(-1) + xSigmaPts[states + vNoise:states +
noises, i] #(2*Nout,1)
# zPredSigmaPts.append(z_pred)
zPredSigmaPts[:, i] = z_pred
xPredSigmaPts = np.array(xPredSigmaPts) # (3,7)
zPredSigmaPts = np.array(zPredSigmaPts) # (2*Nout,7)
xPred = np.sum(wSigmaPts_xmat *
(xPredSigmaPts[:, 1:nsp] -
np.tile(xPredSigmaPts[:, 0].reshape(-1, 1),
(1, nsp - 1))),
axis=1).reshape(-1, 1) # (3,1)
zPred = np.sum(wSigmaPts_zmat *
(zPredSigmaPts[:, 1:nsp] -
np.tile(zPredSigmaPts[:, 0].reshape(-1, 1),
(1, nsp - 1))),
axis=1).reshape(-1, 1) # (2*Nout,1)
xPred = xPred + xPredSigmaPts[:, 0].reshape(-1, 1)
zPred = zPred + zPredSigmaPts[:, 0].reshape(-1, 1)
exSigmaPt = xPredSigmaPts[:, 0].reshape(-1, 1) - xPred
ezSigmaPt = zPredSigmaPts[:, 0].reshape(-1, 1) - zPred
PPred = wSigmaPts[nsp] * exSigmaPt.dot(exSigmaPt.T)
PxzPred = wSigmaPts[nsp] * exSigmaPt.dot(ezSigmaPt.T)
S = wSigmaPts[nsp] * ezSigmaPt.dot(ezSigmaPt.T)
exSigmaPt = xPredSigmaPts[:, 1:nsp] - np.tile(
xPred, (1, nsp - 1)) # (3,nsp-1)
ezSigmaPt = zPredSigmaPts[:, 1:nsp] - np.tile(
zPred, (1, nsp - 1)) # (2*Nout,nsp-1)
PPred = PPred + (wSigmaPts_xmat * exSigmaPt).dot(exSigmaPt.T)
S = S + (wSigmaPts_zmat * ezSigmaPt).dot(ezSigmaPt.T)
PxzPred = PxzPred + exSigmaPt.dot((wSigmaPts_zmat * ezSigmaPt).T)
K = PxzPred.dot(np.linalg.inv(S))
inovation = Z - zPred
Xout = xPred + K.dot(inovation)
Pout = PPred - K @ S @ K.T
return Xout, Pout
