def h_hat(self, u_odo): def odo2speed(u): v = 1 / 2 * (u[0] + u[1]) return torch.Tensor([ v * torch.cos(self.x_prev[5]), v * torch.sin(self.x_prev[5]), 0 ]) # initial speed v0 = odo2speed(u_odo[0]) # end speed v_end = odo2speed(u_odo[1]) R0 = SO3.from_rpy(self.x_prev[3:6]).as_matrix() Rend = SO3.from_rpy(self.x[3:6]).as_matrix() p0 = self.x_prev[:3] p_end = self.x[:3] delta_R = SO3.from_matrix(R0.t().mm(Rend)).log() delta_v = R0.t().mv(v_end - v0 - self.g * self.Delta_t) delta_p = R0.t().mv(p_end - p0 - v0 * self.Delta_t - 1 / 2 * self.g * (self.Delta_t**2)) return torch.cat((delta_R, delta_v, delta_p), 0)
def correct(self, x, u_odo, u_fog, compute_G=False, full_cov=False): u_odo_fog = torch.cat((u_odo, u_fog), 1).unsqueeze(0) u_odo_fog.requires_grad = True Xnew = self.normalize(u_odo_fog) # take mean to speed up correction y_cor_nor, _ = self.gp_f.forward(Xnew, full_cov) # # sample corrections and take mean # N = 100 # mean, cov = self.gp_f.forward(Xnew, full_cov=True) # y_cor_nor = torch.zeros(6) # dist = torch.distributions.MultivariateNormal(loc=mean, cov) # for i in range(N): # y_cor_nor += 1/N * dist.sample() y_cor = self.unnormalize(y_cor_nor.t(), var="y_odo_fog").squeeze() G_cor = self.correct_cov(u_odo_fog, y_cor, compute_G) u_odo_fog.requires_grad = False y_cor = y_cor.detach() y_cor[[3, 4]] = 0 # pitch and roll corrections are set to 0 G_cor[[3, 4], :] = 0 Rot = SO3.from_rpy(x[3:6]).as_matrix() # correct state dRot_cor = SO3.exp(y_cor[3:]).as_matrix() x[:3] = x[:3] + Rot.mv(SE3.exp(y_cor).as_matrix()[:3, 3]) x[3:6] = SO3.from_matrix(Rot.mm(dRot_cor)).to_rpy() return x, G_cor
def integrate_odo_fog(self, u_odo, u_fog, delta_t): if self.nclt: v = 1 / 2 * (u_odo[:, 0] + u_odo[:, 1]) else: v, _ = self.encoder2speed(u_odo, delta_t) xi = u_odo.new_zeros(u_odo.shape[0], 6) xi[:, 0] = v * delta_t xi[:, 5] = u_fog.squeeze() Rot = SO3.from_rpy(xi[:, 3:]).as_matrix() p = xi[:, :3] return Rot, p
def compute_mate(self, t, x, chi, dataset_name): chi_est = torch.zeros(x.shape[0], 4, 4) chi_est[:, :3, :3] = SO3.from_rpy(x[:, 3:6]).as_matrix() chi_est[:, :3, 3] = x[:, :3] chi_est[:, 3, 3] = 1 chi_est = SE3.from_matrix(chi_est) chi = SE3.from_matrix(chi) error = (chi.inv().dot(chi_est)).log() mate_translation = error[:, :3].abs().mean() mate_rotation = error[:, 3:].abs().mean() return mate_translation, mate_rotation
def propagate(self, u_odo, u_fog, delta_t, compare): self.x_prev = self.x for i in range(u_odo.shape[0]): self.integrate_odo_fog(u_odo[i], u_fog[i], delta_t) if self.gp_odo_fog: self.x, G_cor = self.gp_odo_fog.correct( self.x, u_odo, u_fog, compute_G=(compare == 'filter')) else: G_cor = torch.zeros(u_odo.shape[0], 15, 9) if compare == 'filter': self.propagate_cov(u_odo[i], u_fog[i], delta_t, G_cor) else: self.x[3:6] = SO3.from_rpy(self.x[3:6]).to_rpy()
def compute_jac_update(self, u_odo): J = self.J H = self.H Rot_prev = SO3.from_rpy(self.x_prev[3:6]).as_matrix() Rot_new = SO3.from_rpy(self.x[3:6]).as_matrix() # speed is not in state J[0, 3:6, 6:] = -Rot_prev.t()[:3, :2] J[-1, 3:6, 6:] = -J[0, 3:6, 6:] J[0, 6:9, 6:] = J[0, 6:9, 6:] * self.Delta_t v = torch.Tensor([1 / 2 * (u_odo[0][0] + u_odo[0][1]), 0, 0]) H[:3, 3:6] = -Rot_prev.t().mm(Rot_new) H[:3, 9:12] = -Rot_prev.t() H[3:6, 9:12] = -Rot_prev.t().mm( self.skew(self.x[:3] - self.x_prev[:3] - v * self.Delta_t - 1 / 2 * self.g * self.Delta_t**2)) H[6:9, :3] = Rot_prev.t() H[6:9, 12:15] = -Rot_prev.t() H[6:9, 9:12] = self.skew(self.x[:3] - self.x_prev[:3] - v * self.Delta_t - 1 / 2 * self.g * self.Delta_t**2) return H, J
def update_state(self, K_prefix, S, dy): dx = K_prefix.mm(torch.gesv(dy, S)[0]).squeeze(1) # K*dy self.x[:3] += dx[:3] self.x[3:6] += (SO3.exp(dx[3:6]).dot(SO3.from_rpy( self.x[3:6]))).to_rpy() self.x[6:9] += dx[6:9]
def test_rpy_batch(): rpy = torch.Tensor([[np.pi / 12, np.pi / 6, np.pi / 3], [0, 0, 0]]) C_got = SO3.from_rpy(rpy) C_expected = SO3.rotz(rpy[:, 2]).dot( SO3.roty(rpy[:, 1]).dot(SO3.rotx(rpy[:, 0]))) assert utils.allclose(C_got.mat, C_expected.mat)
def test_rpy(): rpy = torch.Tensor([np.pi / 12, np.pi / 6, np.pi / 3]) C_got = SO3.from_rpy(rpy) C_expected = SO3.rotz(torch.Tensor([rpy[2]])).dot( SO3.roty(torch.Tensor([rpy[1]])).dot(SO3.rotx(torch.Tensor([rpy[0]])))) assert utils.allclose(C_got.mat, C_expected.mat)