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)