예제 #1
0
 def test_yaw_0_360(self):
     # 0/360: x 0, y +
     mag = [0.0, 40.0, 40.0]
     attitude = self._compute_attitude(mag)
     self.assertAlmostEqual(attitude[0], 0.0, 3)
     self.assertAlmostEqual(attitude[1], 0.0, 3)
     self.assertAlmostEqual(attitude[2], 0.0, 3)
     self.assertAlmostEqual(attitude[3], 1.0, 3)
     axis_angle = sensorfusion.quaternion_to_axis_angle(*attitude)
     self.assertEqual(axis_angle, (0, 0, 0, 0))
예제 #2
0
 def test_yaw_270(self):
     # 270: x +, y 0
     mag = [40.0, 0.0, 40.0]
     attitude = self._compute_attitude(mag)
     self.assertAlmostEqual(attitude[0], 0.0, 3)
     self.assertAlmostEqual(attitude[1], 0.0, 3)
     self.assertAlmostEqual(attitude[2], 0.707, 3)
     self.assertAlmostEqual(attitude[3], 0.707, 3)
     axis_angle = sensorfusion.quaternion_to_axis_angle(*attitude)
     self.assertAlmostEqual(axis_angle[0], 90, 3)
     self.assertAlmostEqual(axis_angle[1], 0)
     self.assertAlmostEqual(axis_angle[2], 0)
     self.assertAlmostEqual(axis_angle[3], 1)
예제 #3
0
 def test_yaw_180(self):
     # 180: x 0, y -
     mag = [0.0, -40.0, -40.0]
     attitude = self._compute_attitude(mag)
     self.assertAlmostEqual(attitude[0], 0.0, 3)
     self.assertAlmostEqual(attitude[1], 0.0, 3)
     self.assertAlmostEqual(attitude[2], 1, 3)
     self.assertAlmostEqual(attitude[3], 0, 3)
     axis_angle = sensorfusion.quaternion_to_axis_angle(*attitude)
     self.assertAlmostEqual(axis_angle[0], 180.0, 1)
     self.assertAlmostEqual(axis_angle[1], 0)
     self.assertAlmostEqual(axis_angle[2], 0)
     self.assertAlmostEqual(axis_angle[3], 1.0)
예제 #4
0
 def test_yaw_90(self):
     # 90: x -, y 0
     mag = [-40.0, 0.0, 40.0]
     attitude = self._compute_attitude_z(mag)
     self.assertAlmostEqual(attitude[0], 0.0, 3)
     self.assertAlmostEqual(attitude[1], 0.0, 3)
     self.assertAlmostEqual(attitude[2], -0.707, 3)
     self.assertAlmostEqual(attitude[3], 0.707, 3)
     axis_angle = sensorfusion.quaternion_to_axis_angle(*attitude)
     self.assertAlmostEqual(axis_angle[0], 90, 3)
     self.assertAlmostEqual(axis_angle[1], 0, 3)
     self.assertAlmostEqual(axis_angle[2], 0, 3)
     self.assertAlmostEqual(axis_angle[3], -1, 3)
예제 #5
0
    def _test_roll_angles(self):
        mags = [
            [-4385, -1112, -1782],
            [-452, -1381, -793],
            [-5137, -870, 2033],
            [-1663, -974, 3062],
        ]
        expected_angle_axis = [
            (104.3, -1.0),
            (131.6, -1.0),
            (24.4, 1.0),
            (121.3, 1.0)
            ]

        for j, mag in enumerate(mags):
            mag = [(mag[i] - self.MAG_BIAS[i])/self.MAG_SCALE[i] for i in range(3)]
            attitude = self._compute_attitude_y(mag)
            axis_angle = sensorfusion.quaternion_to_axis_angle(*attitude)
            self.assertAlmostEqual(axis_angle[0], expected_angle_axis[j][0], 1)
            self.assertAlmostEqual(axis_angle[1], 0)
            self.assertAlmostEqual(axis_angle[2], expected_angle_axis[j][1], 3)
            self.assertAlmostEqual(axis_angle[3], 0)
예제 #6
0
    def test_yaw_angles(self):
        mags = [
            [-4473, 6554, 4485],  # DOOR WALL
            [-3629, 1532, 4938],  # MIRROR
            [-793, 6677, 4569],   # RIGHT WALL
            [-730, 2380, 5387],   # WINDOW
        ]
        expected_angle_axis = [
            (44.1, -1.0),
            (131.6, -1.0),
            (24.4, 1.0),
            (121.3, 1.0)
            ]

        for j, mag in enumerate(mags):
            mag = [(mag[i] - self.MAG_BIAS[i])/self.MAG_SCALE[i] for i in range(3)]
            attitude = self._compute_attitude_z(mag)
            axis_angle = sensorfusion.quaternion_to_axis_angle(*attitude)
            self.assertAlmostEqual(axis_angle[0], expected_angle_axis[j][0], 1)
            self.assertAlmostEqual(axis_angle[1], 0, 3)
            self.assertAlmostEqual(axis_angle[2], 0, 3)
            self.assertAlmostEqual(axis_angle[3], expected_angle_axis[j][1], 3)
예제 #7
0
    def _test_pitch_angles(self):
        mags = [
            [-6978, 5034, -2936],  # DOOR WALL
            [-7456, 1348, -1137],  # MIRROR
            [-7231, 6709, 732],    # RIGHT WALL
            [-7525, 3502, 2523]    # WINDOW
        ]
        expected_angle_axis = [
            (104.3, -1.0),
            (131.6, -1.0),
            (24.4, 1.0),
            (121.3, 1.0)
            ]

        for j, mag in enumerate(mags):
            mag = [(mag[i] - self.MAG_BIAS[i])/self.MAG_SCALE[i] for i in range(3)]
            attitude = self._compute_attitude_x(mag)
            axis_angle = sensorfusion.quaternion_to_axis_angle(*attitude)
            print j, axis_angle
            self.assertAlmostEqual(axis_angle[0], expected_angle_axis[j][0], 1)
            self.assertAlmostEqual(axis_angle[1], expected_angle_axis[j][1], 3)
            self.assertAlmostEqual(axis_angle[2], 0)
            self.assertAlmostEqual(axis_angle[3], 0)
예제 #8
0
    def _test_mag_ranges(self):
        from collections import defaultdict
        angles = defaultdict(list)
        for x in range(-100, 100, 10):
            for y in range(-100, 100, 10):
                for z in range(-100, 100, 10):
                    self.fusion = sensorfusion.Fusion()

                    acc = [0, sensorfusion.GRAVITY_EARTH, 0]
                    mag = [float(x), float(y), float(z)]
                    gyro = [1e-6, 1e-6, 1e-6]
                    dT = 0.010

                    for _ in range(sensorfusion.MIN_GYRO_SAMPLES):
                        self.fusion.handleAcc(acc)
                        self.fusion.handleMag(mag)
                        self.fusion.handleGyro(gyro, dT)

                    if self.fusion.hasEstimate():
                        matrix = self.fusion.getRotationMatrix()
                        attitude = self.fusion.getAttitude()
                        axis_angle = sensorfusion.quaternion_to_axis_angle(*attitude)
                        angle = math.acos(matrix[0]) * 180. / math.pi
                        # angle = (360 - angle) if matrix[6] < 0 else angle
                        # angle = axis_angle[0]
                        is_nan = angle != angle
                        angles[angle if not is_nan else 'nan'].append(mag)
        print 'nan', angles.pop('nan')
        print min(angles.keys()), max(angles.keys())

        for angle in sorted(angles):
            xs = [x for x, y, z in angles[angle]]
            ys = [y for x, y, z in angles[angle]]
            zs = [z for x, y, z in angles[angle]]
            print angle, (min(xs), max(xs)), (min(ys), max(ys)), (min(zs), max(zs)), angles[angle]
        raise Exception('END')