def testOrient3Model(): env = Environment() calibrator = ScaleAndOffsetCalibrator(env, 1000, 1 / 100, 10) for i in range(3): imu = Orient3IMU() cal = calibrator.calibrate(imu) sim = Simulation(environment=env) traj = RandomTrajectory() imu.simulation = sim imu.trajectory = traj sim.time = imu.trajectory.startTime BasicIMUBehaviour(imu, 1 / 256, calibration=cal, initialTime=sim.time) sim.run(traj.endTime) t = imu.accelerometer.calibratedMeasurements.timestamps r = traj.rotation(t) a = r.rotateFrame( traj.acceleration(t) - env.gravitationalField(traj.position(t), t)) m = r.rotateFrame(env.magneticField(traj.position(t), t)) w = r.rotateFrame(traj.rotationalVelocity(t)) g = env.gravitationalField.nominalMagnitude a_within_5g = abs(a) < 5 * g for i in range(3): yield assert_array_almost_equal, \ np.array([a[i ,a_within_5g[i]]]), \ np.array([imu.accelerometer.calibratedMeasurements(t) [i, a_within_5g[i]]]), -2 yield assert_array_almost_equal, m, \ imu.magnetometer.calibratedMeasurements(t), 0 yield assert_array_almost_equal, w, \ imu.gyroscope.calibratedMeasurements(t), 0
def testOrient3Model(): env = Environment() calibrator = ScaleAndOffsetCalibrator(env, 1000, 1/100, 10) for i in range(3): imu = Orient3IMU() cal = calibrator.calibrate(imu) sim = Simulation(environment=env) traj = RandomTrajectory() imu.simulation = sim imu.trajectory = traj sim.time = imu.trajectory.startTime BasicIMUBehaviour(imu, 1/256, calibration=cal, initialTime=sim.time) sim.run(traj.endTime) t = imu.accelerometer.calibratedMeasurements.timestamps r = traj.rotation(t) a = r.rotateFrame(traj.acceleration(t) - env.gravitationalField(traj.position(t), t)) m = r.rotateFrame(env.magneticField(traj.position(t), t)) w = r.rotateFrame(traj.rotationalVelocity(t)) g = env.gravitationalField.nominalMagnitude a_within_5g = abs(a) < 5*g for i in range(3): yield assert_array_almost_equal, \ np.array([a[i ,a_within_5g[i]]]), \ np.array([imu.accelerometer.calibratedMeasurements(t) [i, a_within_5g[i]]]), -2 yield assert_array_almost_equal, m, \ imu.magnetometer.calibratedMeasurements(t), 0 yield assert_array_almost_equal, w, \ imu.gyroscope.calibratedMeasurements(t), 0
def testOffsetTrajectories(): for i in range(10): T = RandomTrajectory() t = T.positionKeyFrames.timestamps dt = t[1] - t[0] p = T.position(t) r = T.rotation(t) offset = randomPosition() rotationOffset = randomRotation() oT = OffsetTrajectory(T, offset, rotationOffset) offsetPositions = p + r.rotateVector(offset) offsetRotations = r * rotationOffset yield checkTrajectory, oT, TimeSeries(t,offsetPositions), TimeSeries(t, offsetRotations)
def testOffsetTrajectories(): for i in range(10): T = RandomTrajectory() t = T.positionKeyFrames.timestamps dt = t[1] - t[0] p = T.position(t) r = T.rotation(t) offset = randomPosition() rotationOffset = randomRotation() oT = OffsetTrajectory(T, offset, rotationOffset) offsetPositions = p + r.rotateVector(offset) offsetRotations = r * rotationOffset yield checkTrajectory, oT, TimeSeries(t, offsetPositions), TimeSeries( t, offsetRotations)
# Set up a behaviour that runs on the # simulated Camera behaviour_camera = DefaultCameraBehaviour(camera) # Set up a behaviour that runs on the # simulated IMU dt = 1. / 200 behaviour_imu = BasicIMUBehaviour(imu, dt) # Set the time inside the simulation sim.time = trajectory.startTime # Run the simulation till the desired # end time sim.run(trajectory.endTime - camera_model.readout) PLOT = False if PLOT: plt.figure() plt.subplot(2, 1, 1) t_camera = behaviour_camera.timestamps t_imu = imu.gyroscope.rawMeasurements.timestamps plt.axhline(1, color='k') plt.axhline(2, color='k') plt.plot(t_camera, 1 * np.ones_like(t_camera), 'o') plt.plot(trajectory.startTime + t_imu, 2 * np.ones_like(t_imu), 'x') plt.ylim(0, 3) plt.subplot(2, 1, 2) ts = np.linspace(trajectory.startTime, trajectory.endTime) plt.plot(ts, trajectory.position(ts).T) plt.show()