def test_equality_array(self):
q_rot_x_neg = Quaternions([0, -1, 0, 0])
qs1 = Quaternions.from_quaternions(q_rot_x_neg,
QuaternionsTest.q_rot_x)
qs2 = Quaternions.from_quaternions(QuaternionsTest.q_rot_x,
q_rot_x_neg)
self.assertEqual(qs1, qs2)
def test_find_mean_near_pi(self):
dist = 0.1
q_near = Quaternions.from_vectors(np.array([-np.pi + dist, 0, 0]))
q_pi = Quaternions.from_vectors(np.array([np.pi, 0, 0]))
q_expected = Quaternions.from_vectors(
np.array([-np.pi + dist / 2, 0, 0]))
q_0 = QuaternionsTest.get_random_qs(1)[0]
qs = Quaternions.from_quaternions(q_near, q_pi)
self.assertEqual(qs.find_q_mean(q_0), q_expected)
def estimate_state(self):
"""
Uses data provided by the source to estimate the state history of the filter
After calling this function, the state and rotation history will be defined
"""
self.rots = np.zeros((3, 3, self.num_data))
self.rots[..., 0] = np.identity(3)
self.quats.append(Quaternions([1, 0, 0, 0]))
for i in range(0, self.num_data - 1):
dt = self.ts_imu[i + 1] - self.ts_imu[i]
self._filter_next(self.vel_data[:, i], dt)
def estimate_state(self):
"""
Uses data provided by the source to estimate the state history of the filter
After calling this function, the state and rotation history will be defined
"""
self.imu_data[:3] = self._normalize_data(self.imu_data[:3])
self.rots = np.zeros((3, 3, self.state.shape[-1]))
self.rots[..., 0] = Quaternions(self.state[:4, 0]).to_rotation_matrix()
for i in range(1, self.state.shape[-1]):
dt = self.ts_imu[i] - self.ts_imu[i - 1]
self._t = self.ts_imu[i]
self.state[:, i], self.covariance[..., i] = self._filter_next(
self.covariance[..., i - 1],
self.state[:, i - 1],
self.imu_data[:, i],
dt
)
self.rots[..., i] = Quaternions(self.state[:4, i]).to_rotation_matrix()
class QuaternionsTest(unittest.TestCase):
q_rot_all = Quaternions([0, 1, 1, 1])
q_identity = Quaternions([1, 0, 0, 0])
q_rot_x = Quaternions([0, 1, 0, 0])
q_rot_y = Quaternions([0, 0, 1, 0])
q_rot_z = Quaternions([0, 0, 0, 1])
epsilon = 1e-5
@staticmethod
def get_random_qs(n):
qs = []
for i in range(n):
np.random.seed(i)
qs.append(Quaternions(np.random.randn(Quaternions.NDIM)))
return tuple(qs)
def test_invalid_dim_input(self):
invalid = np.ones(5)
self.assertRaises(ValueError, Quaternions, invalid)
def test_equality_singleton(self):
q_rot_x_neg = Quaternions([0, -1, 0, 0])
self.assertEqual(q_rot_x_neg, QuaternionsTest.q_rot_x)
def test_equality_array(self):
q_rot_x_neg = Quaternions([0, -1, 0, 0])
qs1 = Quaternions.from_quaternions(q_rot_x_neg,
QuaternionsTest.q_rot_x)
qs2 = Quaternions.from_quaternions(QuaternionsTest.q_rot_x,
q_rot_x_neg)
self.assertEqual(qs1, qs2)
def test_mult_inverse_singleton(self):
q = QuaternionsTest.get_random_qs(1)[0]
self.assertEqual(q.q_multiply(q.inverse()), QuaternionsTest.q_identity)
def test_mult_inverse_array(self):
qlist = QuaternionsTest.get_random_qs(2)
qs = Quaternions.from_quaternions(*qlist)
qs_identity = Quaternions.from_quaternions(QuaternionsTest.q_identity,
QuaternionsTest.q_identity)
self.assertEqual(qs.q_multiply(qs.inverse()), qs_identity)
def test_mult_associativity(self):
qlist = QuaternionsTest.get_random_qs(3)
self.assertEqual(qlist[0].q_multiply(qlist[1].q_multiply(qlist[2])),
qlist[0].q_multiply(qlist[1]).q_multiply(qlist[2]))
def test_round_trip_vector(self):
qlist = QuaternionsTest.get_random_qs(2)
qs = Quaternions.from_quaternions(*qlist)
self.assertEqual(Quaternions.from_vectors(qs.to_vectors()), qs)
def test_zero_vector(self):
self.assertEqual(Quaternions.from_vectors(np.zeros(3)),
QuaternionsTest.q_identity)
def test_find_mean_at_pi(self):
q_rot_x_neg = Quaternions([0, -1, 0, 0])
q_0 = QuaternionsTest.get_random_qs(1)[0]
qs = Quaternions.from_quaternions(q_rot_x_neg, QuaternionsTest.q_rot_x)
self.assertEqual(qs.find_q_mean(q_0), q_rot_x_neg)
def test_find_mean_near_pi(self):
dist = 0.1
q_near = Quaternions.from_vectors(np.array([-np.pi + dist, 0, 0]))
q_pi = Quaternions.from_vectors(np.array([np.pi, 0, 0]))
q_expected = Quaternions.from_vectors(
np.array([-np.pi + dist / 2, 0, 0]))
q_0 = QuaternionsTest.get_random_qs(1)[0]
qs = Quaternions.from_quaternions(q_near, q_pi)
self.assertEqual(qs.find_q_mean(q_0), q_expected)
def test_find_mean_at_pi(self):
q_rot_x_neg = Quaternions([0, -1, 0, 0])
q_0 = QuaternionsTest.get_random_qs(1)[0]
qs = Quaternions.from_quaternions(q_rot_x_neg, QuaternionsTest.q_rot_x)
self.assertEqual(qs.find_q_mean(q_0), q_rot_x_neg)
def test_zero_vector(self):
self.assertEqual(Quaternions.from_vectors(np.zeros(3)),
QuaternionsTest.q_identity)
def test_round_trip_vector(self):
qlist = QuaternionsTest.get_random_qs(2)
qs = Quaternions.from_quaternions(*qlist)
self.assertEqual(Quaternions.from_vectors(qs.to_vectors()), qs)
def test_mult_inverse_array(self):
qlist = QuaternionsTest.get_random_qs(2)
qs = Quaternions.from_quaternions(*qlist)
qs_identity = Quaternions.from_quaternions(QuaternionsTest.q_identity,
QuaternionsTest.q_identity)
self.assertEqual(qs.q_multiply(qs.inverse()), qs_identity)
def test_equality_singleton(self):
q_rot_x_neg = Quaternions([0, -1, 0, 0])
self.assertEqual(q_rot_x_neg, QuaternionsTest.q_rot_x)
def get_random_qs(n):
qs = []
for i in range(n):
np.random.seed(i)
qs.append(Quaternions(np.random.randn(Quaternions.NDIM)))
return tuple(qs)
def _filter_next(self, velocity, dt):
quat_delta = Quaternions.from_vectors(velocity * dt)
i = len(self.quats)
self.quats.append(quat_delta.q_multiply(self.quats[-1]))
self.rots[..., i] = self.quats[-1].to_rotation_matrix()
def _filter_next(self, cov_last, state_last, measurement_this, dt): # pylint: disable=too-many-locals
sigma_offset = self._calc_sigma_distances(cov_last)
# Equation 34: Form sigma points based on prior mean and covariance data
quats_offset = Quaternions.from_vectors(sigma_offset[:3])
quat_last = Quaternions(state_last[:4])
quats_sigpt = quats_offset.q_multiply(quat_last)
vels_offset = sigma_offset[3:]
vel_last = state_last[4:]
vels_projected = vel_last.reshape(-1, 1) + vels_offset
# Equations 9-11: Form quaternion projection
quat_delta = Quaternions.from_vectors(vels_projected * dt)
# Equation 22: Apply non-linear function A with process noise of zero
quats_projected = quat_delta.q_multiply(quats_sigpt)
# Equations 52-55: Use mean-finding algorithm to satisfy Equation 38
quat_state_est = quats_projected.find_q_mean(quats_projected[0])
vel_state_est = np.mean(vels_projected, axis=1)
# Equations 65-67: Find the predicted process model error from the estimated state
orientations_error = quat_state_est.inverse().q_multiply(quats_projected).to_vectors()
vels_error = vels_projected - vel_state_est.reshape(-1, 1)
process_model_error = np.concatenate((orientations_error, vels_error))
# Equation 64: Estimate covariance matrix as covariance of process model
cov_this_est = process_model_error @ process_model_error.T
cov_this_est /= sigma_offset.shape[1]
# Equation 27 and 40: Estimate measurements that are expected given the process model
gs_est = quats_projected.rotate_vector(self.G_VECTOR)
measurements_est = np.concatenate((gs_est, vels_projected))
# Equation 48: Take the mean of expected measurements as the estimated measurement
measurement_est = np.mean(measurements_est, axis=1)
# Equation 68, 70: Calculate measurement covariance and cross-correlation
measurement_error = measurements_est - measurement_est.reshape(-1, 1)
cov_measurement = measurement_error @ measurement_error.T
cross_correlation = process_model_error @ measurement_error.T
cov_measurement /= sigma_offset.shape[1]
cross_correlation /= sigma_offset.shape[1]
# Equation 69: Include the noise from the measurement model in estimated covariance
cov_innovation = cov_measurement + self.measurement_noise
# Equation 72: Kalman gain as cross correlation to measurement covariance ratio
kalman_gain = cross_correlation @ np.linalg.inv(cov_innovation)
# Equation 74: Obtain quaternion and velocity parts of measurement correction
innovation = (measurement_this - measurement_est).reshape(-1, 1)
correction = np.matmul(kalman_gain, innovation).reshape(-1)
quat_correction = Quaternions.from_vectors(correction[:3])
vel_correction = correction[3:]
# Equation 46: Apply the measurement correction to the estimate from the process model
state_this = np.zeros(STATE_DOF + 1)
state_this[:4] = quat_state_est.q_multiply(quat_correction).array
state_this[4:] = vel_state_est + vel_correction
# Equation 75: Update the covariance estimate given this measurement update
cov_this = cov_this_est - kalman_gain @ cov_innovation @ kalman_gain.T
return state_this, cov_this