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