def test_level_hovering_with_bad_attitude_and_gravity(self):
imu = localization.sensors.imu.ImuCompensated(
10., 1000., 'Madgwick', gravity_drift=9.81)
imu.setGravity(9.75)
imu.setGravityCorrectionRate(0.5)
self.assertAlmostEqual(9.75, imu.gravity)
# Set a zero covariance matrix to make tests deterministic
imu.sensor.covariance = numpy.zeros([6, 6])
state_vector = numpy.zeros([15, 1])
state_vector[localization.util.StateMember.roll, 0] = 0.1
state_vector[localization.util.StateMember.pitch, 0] = 0.2
for t in numpy.arange(10., 30., 1. / 1000.):
imu.getStateMeasurementsUntil(state_vector, t)
meas = imu.getStateMeasurementsUntil(state_vector, 30.)
self.assertAlmostEqual(9.81, imu.gravity, delta=0.001)
self.assertIsNotNone(meas)
self.assertSequenceEqual([6, 1], meas.measurement.shape)
self.assertAlmostEqual(0., meas.measurement[0, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[1, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[2, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[3, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[4, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[5, 0], delta=0.001)
def test_level_hovering_with_bad_attitude_and_gravity(self):
imu = localization.sensors.imu.ImuCompensated(10.,
1000.,
'Madgwick',
gravity_drift=9.81)
imu.setGravity(9.75)
imu.setGravityCorrectionRate(0.5)
self.assertAlmostEqual(9.75, imu.gravity)
# Set a zero covariance matrix to make tests deterministic
imu.sensor.covariance = numpy.zeros([6, 6])
state_vector = numpy.zeros([15, 1])
state_vector[localization.util.StateMember.roll, 0] = 0.1
state_vector[localization.util.StateMember.pitch, 0] = 0.2
for t in numpy.arange(10., 30., 1. / 1000.):
imu.getStateMeasurementsUntil(state_vector, t)
meas = imu.getStateMeasurementsUntil(state_vector, 30.)
self.assertAlmostEqual(9.81, imu.gravity, delta=0.001)
self.assertIsNotNone(meas)
self.assertSequenceEqual([6, 1], meas.measurement.shape)
self.assertAlmostEqual(0., meas.measurement[0, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[1, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[2, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[3, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[4, 0], delta=0.001)
self.assertAlmostEqual(0., meas.measurement[5, 0], delta=0.001)
def test_level_hovering_with_bad_gravity_corrected(self):
imu = localization.sensors.imu.ImuCompensated(10.,
0.5,
'Madgwick',
gravity_drift=9.81)
imu.setGravity(9.5)
imu.setGravityCorrectionRate(0.1)
self.assertAlmostEqual(9.5, imu.gravity)
self.assertAlmostEqual(10., imu.next_measurement_time)
desired_covariance = numpy.diag([8.726646259971648e-05] * 3 +
[1.4709975e-3] * 3)
self.assertSequenceEqual([6, 6], imu.covariance.shape)
for i in xrange(6):
for j in xrange(6):
self.assertAlmostEqual(desired_covariance[i, j],
imu.covariance[i, j])
# Set a zero covariance matrix to make tests deterministic
imu.sensor.covariance = numpy.zeros([6, 6])
state_vector = numpy.zeros([15, 1])
for t in numpy.arange(10., 1000., 1. / 4000.):
imu.getStateMeasurementsUntil(state_vector, t)
meas = imu.getStateMeasurementsUntil(state_vector, 1000.)
self.assertAlmostEqual(9.81, imu.gravity)
self.assertIsNotNone(meas)
self.assertSequenceEqual([6, 1], meas.measurement.shape)
self.assertAlmostEqual(0., meas.measurement[0, 0])
self.assertAlmostEqual(0., meas.measurement[1, 0])
self.assertAlmostEqual(0., meas.measurement[2, 0])
self.assertAlmostEqual(0., meas.measurement[3, 0])
self.assertAlmostEqual(0., meas.measurement[4, 0])
self.assertAlmostEqual(0., meas.measurement[5, 0])
def test_level_hovering_with_bad_gravity_corrected(self):
imu = localization.sensors.imu.ImuCompensated(
10., 0.5, 'Madgwick', gravity_drift=9.81)
imu.setGravity(9.5)
imu.setGravityCorrectionRate(0.1)
self.assertAlmostEqual(9.5, imu.gravity)
self.assertAlmostEqual(10., imu.next_measurement_time)
desired_covariance = numpy.diag(
[8.726646259971648e-05] * 3 + [1.4709975e-3] * 3)
self.assertSequenceEqual([6, 6], imu.covariance.shape)
for i in xrange(6):
for j in xrange(6):
self.assertAlmostEqual(
desired_covariance[i, j], imu.covariance[i, j])
# Set a zero covariance matrix to make tests deterministic
imu.sensor.covariance = numpy.zeros([6, 6])
state_vector = numpy.zeros([15, 1])
for t in numpy.arange(10., 1000., 1. / 4000.):
imu.getStateMeasurementsUntil(state_vector, t)
meas = imu.getStateMeasurementsUntil(state_vector, 1000.)
self.assertAlmostEqual(9.81, imu.gravity)
self.assertIsNotNone(meas)
self.assertSequenceEqual([6, 1], meas.measurement.shape)
self.assertAlmostEqual(0., meas.measurement[0, 0])
self.assertAlmostEqual(0., meas.measurement[1, 0])
self.assertAlmostEqual(0., meas.measurement[2, 0])
self.assertAlmostEqual(0., meas.measurement[3, 0])
self.assertAlmostEqual(0., meas.measurement[4, 0])
self.assertAlmostEqual(0., meas.measurement[5, 0])