def test_add_points_4(points_2):
c = Calibraxis(verbose=False)
points = points_2 / ((2 ** 15) / 8.)
c.add_points(points.tolist())
np.testing.assert_almost_equal(np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_batch_apply(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
out = c.batch_apply(points_1)
normed = np.sqrt((np.array(out)**2).sum(axis=1))
np.testing.assert_array_almost_equal(normed, 1.0, 2)
def test_batch_apply(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
out = c.batch_apply(points_1)
normed = np.sqrt((np.array(out) ** 2).sum(axis=1))
np.testing.assert_array_almost_equal(normed, 1.0, 2)
def test_verbose_prints_progress(points_2, capsys):
c = Calibraxis(verbose=True)
c.add_points(points_2)
c.calibrate_accelerometer()
out, err = capsys.readouterr()
for row in filter(None, out.split('\n')):
assert re.match('^([0-9]+):\s([0-9\-\.e]+)\s*(\([0-9\s\-\.e,]+\))$', row)
def test_add_points_3(points_1):
c = Calibraxis(verbose=False)
points = points_1 / ((2 ** 15) / 8.)
for p in points:
c.add_points(tuple(p))
np.testing.assert_almost_equal(np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_verbose_prints_progress(points_2, capsys):
c = Calibraxis(verbose=True)
c.add_points(points_2)
c.calibrate_accelerometer()
out, err = capsys.readouterr()
for row in filter(None, out.split('\n')):
assert re.match('^([0-9]+):\s([0-9\-\.e]+)\s*(\([0-9\s\-\.e,]+\))$',
row)
def test_add_points_4(points_2):
c = Calibraxis(verbose=False)
points = points_2 / ((2**15) / 8.)
c.add_points(points.tolist())
np.testing.assert_almost_equal(
np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_add_points_3(points_1):
c = Calibraxis(verbose=False)
points = points_1 / ((2**15) / 8.)
for p in points:
c.add_points(tuple(p))
np.testing.assert_almost_equal(
np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_recalibration_points_2(points_2):
c = Calibraxis(verbose=False)
points = points_2 / ((2 ** 15) / 16.)
for p in points[:-1, :]:
c.add_points(p)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
c.add_points(points[-1, :])
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_recalibration_points_2(points_2):
c = Calibraxis(verbose=False)
points = points_2 / ((2**15) / 16.)
for p in points[:-1, :]:
c.add_points(p)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
c.add_points(points[-1, :])
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_error_to_few_points(points_2):
c = Calibraxis(verbose=False)
for p in points_2[:5, :]:
c.add_points(p)
with pytest.raises(ValueError):
c.calibrate_accelerometer()
def test_calibration_points_2_scaled(points_2):
c = Calibraxis(verbose=False)
c.add_points(points_2 / ((2**15) / 16.))
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_apply(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(np.linalg.norm(c.apply(points_1[0, :])),
1.0, 2)
def test_apply(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(np.linalg.norm(c.apply(points_1[0, :])), 1.0, 2)
def test_error_to_few_points(points_2):
c = Calibraxis(verbose=False)
for p in points_2[:5, :]:
c.add_points(p)
with pytest.raises(ValueError):
c.calibrate_accelerometer()
def test_calibration_points_1(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_calibration_points_1(points_1):
c = Calibraxis(verbose=False)
c.add_points(points_1)
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
def test_calibration_points_2_scaled(points_2):
c = Calibraxis(verbose=False)
c.add_points(points_2 / ((2 ** 15) / 16.))
c.calibrate_accelerometer()
np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2)
#matplotlib.use('TkAgg')
c = Calibraxis()
cal_points = np.array([[0.07778838, 0.02505269, 0.88501839],
[0.73871366, 0.0255498, -0.81387024],
[-0.95695496, -0.01158575, 0.11527143],
[0.42802767, 0.75920515, -0.6072115],
[0.99437399, -0.11401009, 0.02588071],
[-0.04117689, 0.20844233, 0.85707983],
[0.85112945, 0.02989109, -0.67080324],
[0.90229526, 0.25129969, -0.49528233],
[-0.25559073, -0.28405265, 0.80770797],
[0.99941998, 0.03766609, -0.04686234],
[0.04151319, 1.0228318, 0.14966903]])
# Add points to calibration object's storage.
c.add_points(cal_points)
# Run the calibration parameter optimization.
c.calibrate_accelerometer()
def running_mean(x, N):
cumsum = np.cumsum(np.insert(x, 0, 0))
return (cumsum[N:] - cumsum[:-N]) / N
def update_imu(_quaternion, samplePeriod, gyroscope, accelerometer):
"""
Perform one update step with data from a IMU sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
"""
import numpy as np
from calibraxis import Calibraxis
c = Calibraxis()
points = np.array([[-4772.38754098, 154.04459016, -204.39081967],
[3525.0346179, -68.64924886, -34.54604833],
[-658.17681729, -4137.60248854, -140.49377865],
[-564.18562092, 4200.29150327, -130.51895425],
[-543.18289474, 18.14736842, -4184.43026316],
[-696.62532808, 15.70209974, 3910.20734908],
[406.65271419, 18.46827992, -4064.61085677],
[559.45926413, -3989.69513798, -174.71879106],
[597.22629169, -3655.54153041, -1662.83257031],
[1519.02616089, -603.82472204, 3290.58469588]])
# Add points to calibration object's storage.
c.add_points(points)
# Run the calibration parameter optimization.
c.calibrate_accelerometer()
# Applying the calibration parameters to the calibration data.
c.apply(points[0:])
c.batch_apply(points)
# wait for correct available data length, should be a VERY short wait
fifoCount = mpu.getFIFOCount()
while fifoCount < packetSize:
fifoCount = mpu.getFIFOCount()
result = mpu.getFIFOBytes(packetSize)
q = mpu.dmpGetQuaternion(result)
g = mpu.dmpGetGravity(q)
ypr = mpu.dmpGetYawPitchRoll(q, g)
# rad2deg = (180 / math.pi)
gpoints.append([ypr['yaw'] * rad2deg,ypr['pitch']*rad2deg,ypr['roll']*rad2deg])
# track FIFO count here in case there is > 1 packet available
# (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize
a.add_points(apoints)
print("Points",apoints)
# Run the calibration parameter optimization.
a.calibrate_accelerometer()
self.a=a
while True:
acc_out = np.array([ypr['yaw'] * rad2deg,ypr['pitch']*rad2deg,ypr['roll']*rad2deg
])
acc_out=self.a.apply(acc_out)
print("A",acc_out)