def test(): from math import pi import numpy # pose in frame A a = EKF.column_vector(1, 2, pi / 2) # pose in frame B b = EKF.column_vector(1, 1, 0) # actual origin of B in A B = EKF.column_vector(2, 1, pi / 2) # calculated origin of B in A BB = EKF.get_offset(a, b) # use it to convert b into a aa = EKF.coord_transform(b, BB) # assert that things match assert numpy.equal(B, BB).all() assert numpy.equal(a, aa).all()