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)
