def propagate_cov(self, u_odo, u_fog, dt, G_cor):
F = self.F
G = self.G
v, _ = self.encoder2speed(u_odo, dt)
J = torch.Tensor([[0, 1], [-1, 0]])
Rot = torch.Tensor([[self.x[5].cos(), self.x[5].sin()],
[-self.x[5].sin(), self.x[5].cos()]])
F[:2, 5] = J.mv(Rot.mv(torch.Tensor([v * dt, 0])))
A = torch.zeros(2, 3)
A[0, 0] = 1
A[0, 1] = self.x[3].sin() * self.x[4].tan()
A[0, 2] = self.x[3].cos() * self.x[4].tan()
A[1, 1] = self.x[3].cos()
A[1, 2] = -self.x[3].sin()
F[3:5, 6:9] = A * dt
B = torch.Tensor([
[1 / 2, 1 / 2], # v_l, v_r to v_forward
[0, 0]
])
F[3, 3] = 1 + self.x[7] * self.x[3].sin() * self.x[4].tan() * dt
F[3, 4] = 1 + (self.x[7] * self.x[3].sin() +
self.x[8] * self.x[3].cos()) * self.x[4].tan() * dt
F[4, 3] = 1 - self.x[8] * self.x[3].sin() * dt
G[:2, :2] = SO2.from_angle(
self.x[5].unsqueeze(0)).as_matrix().mm(B) * dt
# add Jacobian correction
Q = torch.zeros_like(self.P)
for i in range(G_cor.shape[0]):
Q += (G + G_cor[i]).mm(self.Q).mm((G + G_cor[i]).t())
self.P = F.mm(self.P).mm(F.t()) + Q
def test_from_angle_batch():
angles = torch.Tensor([np.pi / 2., np.pi])
assert torch.allclose(
torch.tensor([
[[0, -1], [1, 0]],
[[-1, 0], [0, 1]],
],
dtype=torch.float32),
SO2.from_angle(angles).mat)
def test_from_angle_to_angle_batch():
angles = torch.Tensor([-1., 0, 1.])
assert utils.allclose(SO2.from_angle(angles).to_angle(), angles)
def test_from_angle_to_angle():
angle = torch.Tensor([np.pi / 2.])
assert utils.allclose(SO2.from_angle(angle).to_angle(), angle)
def test_from_angle():
angle = torch.Tensor([np.pi / 2.])
assert torch.allclose(torch.tensor([[0, -1], [1, 0]], dtype=torch.float32),
SO2.from_angle(angle).mat)