Ejemplo n.º 1
0
class SimulatedFlightTests(unittest.TestCase):
    def setUp(self):
        if C_IMP:
            self.sim = CINS()
        else:
            self.sim = PyINS()
        self.sim.prepare()

        import simulation
        self.model = simulation.Simulation()

    def assertState(self,
                    state,
                    pos=[0, 0, 0],
                    vel=[0, 0, 0],
                    rpy=[0, 0, 0],
                    bias=[0, 0, 0, 0, 0, 0]):
        """ check that the state is near a desired position
        """

        # check position
        self.assertAlmostEqual(state[0], pos[0], places=0)
        self.assertAlmostEqual(state[1], pos[1], places=0)
        self.assertAlmostEqual(state[2], pos[2], places=0)

        # check velocity
        self.assertAlmostEqual(state[3], vel[0], places=1)
        self.assertAlmostEqual(state[4], vel[1], places=1)
        self.assertAlmostEqual(state[5], vel[2], places=1)

        # check attitude (in degrees)
        s_rpy = quat_rpy(state[6:10])
        self.assertAlmostEqual(s_rpy[0], rpy[0], places=0)
        self.assertAlmostEqual(s_rpy[1], rpy[1], places=0)
        self.assertTrue(abs(math.fmod(s_rpy[2] - rpy[2], 360)) < 5)

        # check bias terms (gyros and accels)
        self.assertAlmostEqual(state[10], bias[0], places=0)
        self.assertAlmostEqual(state[11], bias[1], places=0)
        self.assertAlmostEqual(state[12], bias[2], places=0)
        self.assertAlmostEqual(state[13], bias[3], places=0)
        self.assertAlmostEqual(state[14], bias[4], places=0)
        self.assertAlmostEqual(state[15], bias[5], places=1)

    def plot(self, times, history, history_rpy, true_pos, true_vel, true_rpy):
        from numpy import cos, sin

        import matplotlib.pyplot as plt
        fig, ax = plt.subplots(2, 2, sharex=True)

        k = times.size

        ax[0][0].cla()
        ax[0][0].plot(times[0:k:4], true_pos[0:k:4, :], 'k--')
        ax[0][0].plot(times[0:k:4], history[0:k:4, 0:3])
        ax[0][0].set_title('Position')
        plt.sca(ax[0][0])
        plt.ylabel('m')

        ax[0][1].cla()
        ax[0][1].plot(times[0:k:4], true_vel[0:k:4, :], 'k--')
        ax[0][1].plot(times[0:k:4], history[0:k:4, 3:6])
        ax[0][1].set_title('Velocity')
        plt.sca(ax[0][1])
        plt.ylabel('m/s')

        ax[1][0].cla()
        ax[1][0].plot(times[0:k:4], true_rpy[0:k:4, :], 'k--')
        ax[1][0].plot(times[0:k:4], history_rpy[0:k:4, :])
        ax[1][0].set_title('Attitude')
        plt.sca(ax[1][0])
        plt.ylabel('Angle (Deg)')
        plt.xlabel('Time (s)')

        ax[1][1].cla()
        ax[1][1].plot(times[0:k:4], history[0:k:4, 10:13])
        ax[1][1].plot(times[0:k:4], history[0:k:4, -1])
        ax[1][1].set_title('Biases')
        plt.sca(ax[1][1])
        plt.ylabel('Bias (rad/s)')
        plt.xlabel('Time (s)')

        plt.suptitle(unittest.TestCase.shortDescription(self))
        plt.show()

    def test_circle(self, STEPS=50000):
        """ test that the INS gets a good fit for a simulated flight of 
        circles
        """

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        numpy.random.seed(1)

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        for k in range(STEPS):

            model.fly_circle(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3, ) * 1e-3
            na = numpy.random.randn(3, ) * 1e-3
            np = numpy.random.randn(3, ) * 1e-3
            nv = numpy.random.randn(3, ) * 1e-3
            nm = numpy.random.randn(3, ) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            sim.predict(gyro + ng, accel + na, dT=dT)

            if True and k % 60 == 59:
                sim.correction(pos=true_pos[k, :] + np)

            if True and k % 60 == 59:
                sim.correction(vel=true_vel[k, :] + nv)

            if k % 20 == 8:
                sim.correction(baro=-true_pos[k, 2] + np[2])

            if True and k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

            if k > 100:
                numpy.testing.assert_almost_equal(sim.state[0:3],
                                                  true_pos[k, :],
                                                  decimal=0)
                numpy.testing.assert_almost_equal(sim.state[3:6],
                                                  true_vel[k, :],
                                                  decimal=0)
                # only test roll and pitch because of wraparound issues
                numpy.testing.assert_almost_equal(history_rpy[k, 0:2],
                                                  true_rpy[k, 0:2],
                                                  decimal=1)

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel,
                      true_rpy)

        return sim.state, history, times

    def test_gyro_bias_circle(self):
        """ test that while flying a circle the location converges
        """

        STEPS = 100000

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        numpy.random.seed(1)

        for k in range(STEPS):

            model.fly_circle(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3, ) * 1e-3
            na = numpy.random.randn(3, ) * 1e-3
            np = numpy.random.randn(3, ) * 1e-3
            nv = numpy.random.randn(3, ) * 1e-3
            nm = numpy.random.randn(3, ) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            # add simulated bias
            gyro = gyro + numpy.array([0.1, -0.05, 0.15])

            sim.predict(gyro + ng, accel + na, dT=dT)

            pos = model.get_pos()

            if k % 60 == 59:
                sim.correction(pos=pos + np)
            if k % 60 == 59:
                sim.correction(vel=model.get_vel() + nv)
            if k % 20 == 8:
                sim.correction(baro=-pos[2] + np[2])
            if k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel,
                      true_rpy)

        self.assertState(sim.state,
                         pos=model.get_pos(),
                         vel=model.get_vel(),
                         rpy=model.get_rpy(),
                         bias=[0, 0, 0, 0, 0, 0])

    def test_accel_bias_circle(self):
        """ test that while flying a circle the location converges
        """

        STEPS = 100000

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        numpy.random.seed(1)

        for k in range(STEPS):

            model.fly_circle(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3, ) * 1e-3
            na = numpy.random.randn(3, ) * 1e-3
            np = numpy.random.randn(3, ) * 1e-3
            nv = numpy.random.randn(3, ) * 1e-3
            nm = numpy.random.randn(3, ) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            # add simulated bias
            accel = accel + numpy.array([0.0, 0, 0.2])

            sim.predict(gyro + ng, accel + na, dT=dT)

            pos = model.get_pos()

            if k % 60 == 59:
                sim.correction(pos=pos + np)
            if k % 60 == 59:
                sim.correction(vel=model.get_vel() + nv)
            if k % 20 == 8:
                sim.correction(baro=-pos[2] + np[2])
            if k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel,
                      true_rpy)

        self.assertState(sim.state,
                         pos=model.get_pos(),
                         vel=model.get_vel(),
                         rpy=model.get_rpy(),
                         bias=[0, 0, 0, 0, 0, 0.2])

    def test_bad_init_q(self):
        """ test that while flying a circle the location converges with a bad initial attitude
        """

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        STEPS = 60 * 666

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        numpy.random.seed(1)

        ins.set_state(q=numpy.array([math.sqrt(2), math.sqrt(2), 0, 0]))

        for k in range(STEPS):

            model.fly_circle(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3, ) * 1e-3
            na = numpy.random.randn(3, ) * 1e-3
            np = numpy.random.randn(3, ) * 1e-3
            nv = numpy.random.randn(3, ) * 1e-3
            nm = numpy.random.randn(3, ) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            sim.predict(gyro + ng, accel + na, dT=dT)

            pos = model.get_pos()

            if k % 60 == 59:
                sim.correction(pos=pos + np)
            if k % 60 == 59:
                sim.correction(vel=model.get_vel() + nv)
            if k % 20 == 8:
                sim.correction(baro=-pos[2] + np[2])
            if k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel,
                      true_rpy)

        self.assertState(sim.state,
                         pos=model.get_pos(),
                         vel=model.get_vel(),
                         rpy=model.get_rpy(),
                         bias=[0, 0, 0, 0, 0, 0])

    def rock_and_turn(self, STEPS=50000):
        """ test the biases work when rocking and turning. this tests the
            mag attitude compensation """

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        numpy.random.seed(1)

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        for k in range(STEPS):

            model.rock_and_turn(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3, ) * 1e-3
            na = numpy.random.randn(3, ) * 1e-3
            np = numpy.random.randn(3, ) * 1e-3
            nv = numpy.random.randn(3, ) * 1e-3
            nm = numpy.random.randn(3, ) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            sim.predict(gyro + ng, accel + na, dT=dT)

            if True and k % 60 == 59:
                sim.correction(pos=true_pos[k, :] + np)

            if True and k % 60 == 59:
                sim.correction(vel=true_vel[k, :] + nv)

            if k % 20 == 8:
                sim.correction(baro=-true_pos[k, 2] + np[2])

            if True and k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

            if k > 100:
                numpy.testing.assert_almost_equal(sim.state[0:3],
                                                  true_pos[k, :],
                                                  decimal=0)
                numpy.testing.assert_almost_equal(sim.state[3:6],
                                                  true_vel[k, :],
                                                  decimal=0)
                # only test roll and pitch because of wraparound issues
                numpy.testing.assert_almost_equal(history_rpy[k, 0:2],
                                                  true_rpy[k, 0:2],
                                                  decimal=1)

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel,
                      true_rpy)

        return sim.state, history, times
Ejemplo n.º 2
0
class StepTestFunctions(unittest.TestCase):
    def setUp(self):
        if C_IMP:
            self.sim = CINS()
        else:
            self.sim = PyINS()
        self.sim.prepare()

    def run_step(self,
                 accel=[0.0, 0.0, -CINS.GRAV],
                 gyro=[0.0, 0.0, 0.0],
                 mag=[400, 0, 1600],
                 pos=[0, 0, 0],
                 vel=[0, 0, 0],
                 noise=False,
                 STEPS=200000,
                 CHANGE=6660 * 3):
        """ simulate a static set of inputs and measurements
        """

        sim = self.sim

        dT = 1.0 / 666.0

        #numpy.random.seed(1)

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))
        times = numpy.zeros((STEPS, 1))

        for k in range(STEPS):
            ng = numpy.zeros(3, )
            na = numpy.zeros(3, )
            np = numpy.zeros(3, )
            nv = numpy.zeros(3, )
            nm = numpy.zeros(3, )

            if noise:
                ng = numpy.random.randn(3, ) * 1e-3
                na = numpy.random.randn(3, ) * 1e-3
                np = numpy.random.randn(3, ) * 1e-3
                nv = numpy.random.randn(3, ) * 1e-3
                nm = numpy.random.randn(3, ) * 10.0

            if k < CHANGE:
                sim.predict(ng, numpy.array([0, 0, -CINS.GRAV]) + na, dT=dT)
            else:
                sim.predict(gyro + ng, accel + na, dT=dT)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

            if True and k % 60 == 59:
                sim.correction(pos=pos + np)

            if True and k % 60 == 59:
                sim.correction(vel=vel + nv)

            if k % 20 == 8:
                sim.correction(baro=-pos[2] + np[2])

            if True and k % 20 == 15:
                sim.correction(mag=mag + nm)

        if VISUALIZE:
            from numpy import cos, sin

            import matplotlib.pyplot as plt
            fig, ax = plt.subplots(2, 2, sharex=True)

            k = STEPS

            ax[0][0].cla()
            ax[0][0].plot(times[0:k:4], history[0:k:4, 0:3])
            ax[0][0].set_title('Position')
            plt.sca(ax[0][0])
            plt.ylabel('m')
            ax[0][1].cla()
            ax[0][1].plot(times[0:k:4], history[0:k:4, 3:6])
            ax[0][1].set_title('Velocity')
            plt.sca(ax[0][1])
            plt.ylabel('m/s')
            #plt.ylim(-2,2)
            ax[1][0].cla()
            ax[1][0].plot(times[0:k:4], history_rpy[0:k:4, :])
            ax[1][0].set_title('Attitude')
            plt.sca(ax[1][0])
            plt.ylabel('Angle (Deg)')
            plt.xlabel('Time (s)')
            #plt.ylim(-1.1,1.1)
            ax[1][1].cla()
            ax[1][1].plot(times[0:k:4], history[0:k:4, 10:13], label="Gyro")
            ax[1][1].plot(times[0:k:4], history[0:k:4, -1], label="Accel")
            ax[1][1].set_title('Biases')
            plt.sca(ax[1][1])
            plt.ylabel('Bias (rad/s)')
            plt.xlabel('Time (s)')
            plt.legend()

            plt.suptitle(unittest.TestCase.shortDescription(self))
            plt.show()

        return sim.state, history, times

    def assertState(self,
                    state,
                    pos=[0, 0, 0],
                    vel=[0, 0, 0],
                    rpy=[0, 0, 0],
                    bias=[0, 0, 0, 0, 0, 0]):
        """ check that the state is near a desired position
        """

        # check position
        self.assertAlmostEqual(state[0], pos[0], places=1)
        self.assertAlmostEqual(state[1], pos[1], places=1)
        self.assertAlmostEqual(state[2], pos[2], places=1)

        # check velocity
        self.assertAlmostEqual(state[3], vel[0], places=1)
        self.assertAlmostEqual(state[4], vel[1], places=1)
        self.assertAlmostEqual(state[5], vel[2], places=1)

        # check attitude (in degrees)
        s_rpy = quat_rpy(state[6:10])
        self.assertAlmostEqual(s_rpy[0], rpy[0], places=0)
        self.assertAlmostEqual(s_rpy[1], rpy[1], places=0)
        self.assertTrue(abs(math.fmod(s_rpy[2] - rpy[2], 360)) < 1)

        # check bias terms (gyros and accels)
        self.assertAlmostEqual(state[10], bias[0], places=2)
        self.assertAlmostEqual(state[11], bias[1], places=2)
        self.assertAlmostEqual(state[12], bias[2], places=2)
        self.assertAlmostEqual(state[13], bias[3], places=2)
        self.assertAlmostEqual(state[14], bias[4], places=2)
        self.assertAlmostEqual(state[15], bias[5], places=2)

    def test_accel_bias(self):
        """ test convergence with biased accelerometers
        """

        bias = -0.20
        state, history, times = self.run_step(accel=[0, 0, -PyINS.GRAV + bias],
                                              STEPS=90 * 666)
        self.assertState(state, bias=[0, 0, 0, 0, 0, bias])

    def test_gyro_bias(self):
        """ test convergence with biased gyros
        """

        state, history, times = self.run_step(gyro=[0.1, -0.05, 0.06],
                                              STEPS=90 * 666)
        self.assertState(state, bias=[0.1, -0.05, 0.06, 0, 0, 0])

    def test_gyro_bias_xy_rate(self):
        """ test gyro xy bias converges within 10% in a fixed time
        """

        TIME = 90  # seconds
        FS = 666  # sampling rate
        MAX_ERR = 0.1
        BIAS = 10.0 * math.pi / 180.0

        state, history, times = self.run_step(gyro=[BIAS, -BIAS, 0],
                                              STEPS=TIME * FS)

        self.assertAlmostEqual(state[10], BIAS, delta=BIAS * MAX_ERR)
        self.assertAlmostEqual(state[11], -BIAS, delta=BIAS * MAX_ERR)

    def test_gyro_bias_z_rate(self):
        """ test gyro z bias converges within 10% in a fixed time
        """

        TIME = 90  # seconds
        FS = 666  # sampling rate
        MAX_ERR = 0.1
        BIAS = 10.0 * math.pi / 180.0

        state, history, times = self.run_step(gyro=[0, 0, BIAS],
                                              STEPS=TIME * FS)

        self.assertAlmostEqual(state[12], BIAS, delta=BIAS * MAX_ERR)

    def test_accel_bias_z_rate(self):
        """ test accel z bias converges within 10% in a fixed time
        """

        TIME = 90  # seconds
        FS = 666  # sampling rate
        MAX_ERR = 0.1
        BIAS = 1

        state, history, times = self.run_step(accel=[0, 0, -PyINS.GRAV + BIAS],
                                              STEPS=TIME * FS)

        self.assertAlmostEqual(state[15], BIAS, delta=BIAS * MAX_ERR)
Ejemplo n.º 3
0
class CompareFunctions(unittest.TestCase):
    def setUp(self):

        self.c_sim = CINS()
        self.py_sim = PyINS()

        self.c_sim.prepare()
        self.py_sim.prepare()

    def run_static(self,
                   accel=[0.0, 0.0, -PyINS.GRAV],
                   gyro=[0.0, 0.0, 0.0],
                   mag=[400, 0, 1600],
                   pos=[0, 0, 0],
                   vel=[0, 0, 0],
                   noise=False,
                   STEPS=200000):
        """ simulate a static set of inputs and measurements
        """

        c_sim = self.c_sim
        py_sim = self.py_sim

        dT = 1.0 / 666.0

        numpy.random.seed(1)

        c_history = numpy.zeros((STEPS, 16))
        c_history_rpy = numpy.zeros((STEPS, 3))
        py_history = numpy.zeros((STEPS, 16))
        py_history_rpy = numpy.zeros((STEPS, 3))
        times = numpy.zeros((STEPS, 1))

        for k in range(STEPS):
            print ` k `
            ng = numpy.zeros(3, )
            na = numpy.zeros(3, )
            np = numpy.zeros(3, )
            nv = numpy.zeros(3, )
            nm = numpy.zeros(3, )

            if noise:
                ng = numpy.random.randn(3, ) * 1e-3
                na = numpy.random.randn(3, ) * 1e-3
                np = numpy.random.randn(3, ) * 1e-3
                nv = numpy.random.randn(3, ) * 1e-3
                nm = numpy.random.randn(3, ) * 10.0

            c_sim.predict(gyro + ng, accel + na, dT=dT)
            py_sim.predict(gyro + ng, accel + na, dT=dT)

            times[k] = k * dT
            c_history[k, :] = c_sim.state
            c_history_rpy[k, :] = quat_rpy(c_sim.state[6:10])
            py_history[k, :] = py_sim.state
            py_history_rpy[k, :] = quat_rpy(py_sim.state[6:10])

            if False and k % 60 == 59:
                c_sim.correction(pos=pos + np)
                py_sim.correction(pos=pos + np)

            if False and k % 60 == 59:
                c_sim.correction(vel=vel + nv)
                py_sim.correction(vel=vel + nv)

            if True and k % 20 == 8:
                c_sim.correction(baro=-pos[2] + np[2])
                py_sim.correction(baro=-pos[2] + np[2])

            if True and k % 20 == 15:
                c_sim.correction(mag=mag + nm)
                py_sim.correction(mag=mag + nm)

            self.assertState(c_sim.state, py_sim.state)

        if VISUALIZE:
            from numpy import cos, sin

            import matplotlib.pyplot as plt
            fig, ax = plt.subplots(2, 2)

            k = STEPS

            ax[0][0].cla()
            ax[0][0].plot(times[0:k:4], c_history[0:k:4, 0:3])
            ax[0][0].set_title('Position')
            plt.sca(ax[0][0])
            plt.ylabel('m')
            ax[0][1].cla()
            ax[0][1].plot(times[0:k:4], c_history[0:k:4, 3:6])
            ax[0][1].set_title('Velocity')
            plt.sca(ax[0][1])
            plt.ylabel('m/s')
            #plt.ylim(-2,2)
            ax[1][0].cla()
            ax[1][0].plot(times[0:k:4], c_history_rpy[0:k:4, :])
            ax[1][0].set_title('Attitude')
            plt.sca(ax[1][0])
            plt.ylabel('Angle (Deg)')
            plt.xlabel('Time (s)')
            #plt.ylim(-1.1,1.1)
            ax[1][1].cla()
            ax[1][1].plot(times[0:k:4], c_history[0:k:4, 10:])
            ax[1][1].set_title('Biases')
            plt.sca(ax[1][1])
            plt.ylabel('Bias (rad/s)')
            plt.xlabel('Time (s)')

            plt.suptitle(unittest.TestCase.shortDescription(self))
            plt.show()

        return sim.state, history, times

    def assertState(self, c_state, py_state):
        """ check that the state is near a desired position
        """

        # check position
        self.assertAlmostEqual(c_state[0], py_state[0], places=1)
        self.assertAlmostEqual(c_state[1], py_state[1], places=1)
        self.assertAlmostEqual(c_state[2], py_state[2], places=1)

        # check velocity
        self.assertAlmostEqual(c_state[3], py_state[3], places=1)
        self.assertAlmostEqual(c_state[4], py_state[4], places=1)
        self.assertAlmostEqual(c_state[5], py_state[5], places=1)

        # check attitude
        self.assertAlmostEqual(c_state[0], py_state[0], places=0)
        self.assertAlmostEqual(c_state[1], py_state[1], places=0)
        self.assertAlmostEqual(c_state[2], py_state[2], places=0)
        self.assertAlmostEqual(c_state[3], py_state[3], places=0)

        # check bias terms (gyros and accels)
        self.assertAlmostEqual(c_state[10], py_state[10], places=2)
        self.assertAlmostEqual(c_state[11], py_state[11], places=2)
        self.assertAlmostEqual(c_state[12], py_state[12], places=2)
        self.assertAlmostEqual(c_state[13], py_state[13], places=2)
        self.assertAlmostEqual(c_state[14], py_state[14], places=2)
        self.assertAlmostEqual(c_state[15], py_state[15], places=2)

    def test_face_west(self):
        """ test convergence to face west
        """

        mag = [0, -400, 1600]
        state, history, times = self.run_static(mag=mag, STEPS=50000)
        self.assertState(state, rpy=[0, 0, 90])
Ejemplo n.º 4
0
class CompareFunctions(unittest.TestCase):

    def setUp(self):

        self.c_sim = CINS()
        self.py_sim = PyINS()

        self.c_sim.prepare()
        self.py_sim.prepare()

    def run_static(self, accel=[0.0,0.0,-PyINS.GRAV],
        gyro=[0.0,0.0,0.0], mag=[400,0,1600],
        pos=[0,0,0], vel=[0,0,0],
        noise=False, STEPS=200000):
        """ simulate a static set of inputs and measurements
        """

        c_sim = self.c_sim
        py_sim = self.py_sim

        dT = 1.0 / 666.0

        numpy.random.seed(1)

        c_history = numpy.zeros((STEPS,16))
        c_history_rpy = numpy.zeros((STEPS,3))
        py_history = numpy.zeros((STEPS,16))
        py_history_rpy = numpy.zeros((STEPS,3))
        times = numpy.zeros((STEPS,1))

        for k in range(STEPS):
            print `k`
            ng = numpy.zeros(3,)
            na = numpy.zeros(3,)
            np = numpy.zeros(3,)
            nv = numpy.zeros(3,)
            nm = numpy.zeros(3,)

            if noise:
                ng = numpy.random.randn(3,) * 1e-3
                na = numpy.random.randn(3,) * 1e-3
                np = numpy.random.randn(3,) * 1e-3
                nv = numpy.random.randn(3,) * 1e-3
                nm = numpy.random.randn(3,) * 10.0

            c_sim.predict(gyro+ng, accel+na, dT=dT)
            py_sim.predict(gyro+ng, accel+na, dT=dT)

            times[k] = k * dT
            c_history[k,:] = c_sim.state
            c_history_rpy[k,:] = quat_rpy(c_sim.state[6:10])
            py_history[k,:] = py_sim.state
            py_history_rpy[k,:] = quat_rpy(py_sim.state[6:10])

            if False and k % 60 == 59:
                c_sim.correction(pos=pos+np)
                py_sim.correction(pos=pos+np)

            if False and k % 60 == 59:
                c_sim.correction(vel=vel+nv)
                py_sim.correction(vel=vel+nv)

            if True and k % 20 == 8:
                c_sim.correction(baro=-pos[2]+np[2])
                py_sim.correction(baro=-pos[2]+np[2])

            if True and k % 20 == 15:
                c_sim.correction(mag=mag+nm)
                py_sim.correction(mag=mag+nm)

            self.assertState(c_sim.state, py_sim.state)

        if VISUALIZE:
            from numpy import cos, sin

            import matplotlib.pyplot as plt
            fig, ax = plt.subplots(2,2)

            k = STEPS

            ax[0][0].cla()
            ax[0][0].plot(times[0:k:4],c_history[0:k:4,0:3])
            ax[0][0].set_title('Position')
            plt.sca(ax[0][0])
            plt.ylabel('m')
            ax[0][1].cla()
            ax[0][1].plot(times[0:k:4],c_history[0:k:4,3:6])
            ax[0][1].set_title('Velocity')
            plt.sca(ax[0][1])
            plt.ylabel('m/s')
            #plt.ylim(-2,2)
            ax[1][0].cla()
            ax[1][0].plot(times[0:k:4],c_history_rpy[0:k:4,:])
            ax[1][0].set_title('Attitude')
            plt.sca(ax[1][0])
            plt.ylabel('Angle (Deg)')
            plt.xlabel('Time (s)')
            #plt.ylim(-1.1,1.1)
            ax[1][1].cla()
            ax[1][1].plot(times[0:k:4],c_history[0:k:4,10:])
            ax[1][1].set_title('Biases')
            plt.sca(ax[1][1])
            plt.ylabel('Bias (rad/s)')
            plt.xlabel('Time (s)')

            plt.suptitle(unittest.TestCase.shortDescription(self))
            plt.show()


        return sim.state, history, times

    def assertState(self, c_state, py_state):
        """ check that the state is near a desired position
        """

        # check position
        self.assertAlmostEqual(c_state[0],py_state[0],places=1)
        self.assertAlmostEqual(c_state[1],py_state[1],places=1)
        self.assertAlmostEqual(c_state[2],py_state[2],places=1)

        # check velocity
        self.assertAlmostEqual(c_state[3],py_state[3],places=1)
        self.assertAlmostEqual(c_state[4],py_state[4],places=1)
        self.assertAlmostEqual(c_state[5],py_state[5],places=1)

        # check attitude
        self.assertAlmostEqual(c_state[0],py_state[0],places=0)
        self.assertAlmostEqual(c_state[1],py_state[1],places=0)
        self.assertAlmostEqual(c_state[2],py_state[2],places=0)
        self.assertAlmostEqual(c_state[3],py_state[3],places=0)

        # check bias terms (gyros and accels)
        self.assertAlmostEqual(c_state[10],py_state[10],places=2)
        self.assertAlmostEqual(c_state[11],py_state[11],places=2)
        self.assertAlmostEqual(c_state[12],py_state[12],places=2)
        self.assertAlmostEqual(c_state[13],py_state[13],places=2)
        self.assertAlmostEqual(c_state[14],py_state[14],places=2)
        self.assertAlmostEqual(c_state[15],py_state[15],places=2)

    def test_face_west(self):
        """ test convergence to face west
        """

        mag = [0,-400,1600]
        state, history, times = self.run_static(mag=mag, STEPS=50000)
        self.assertState(state,rpy=[0,0,90])
Ejemplo n.º 5
0
class SimulatedFlightTests(unittest.TestCase):
    def setUp(self):
        if C_IMP:
            self.sim = CINS()
        else:
            self.sim = PyINS()
        self.sim.prepare()

        import simulation

        self.model = simulation.Simulation()

    def assertState(self, state, pos=[0, 0, 0], vel=[0, 0, 0], rpy=[0, 0, 0], bias=[0, 0, 0, 0, 0, 0]):
        """ check that the state is near a desired position
        """

        # check position
        self.assertAlmostEqual(state[0], pos[0], places=0)
        self.assertAlmostEqual(state[1], pos[1], places=0)
        self.assertAlmostEqual(state[2], pos[2], places=0)

        # check velocity
        self.assertAlmostEqual(state[3], vel[0], places=1)
        self.assertAlmostEqual(state[4], vel[1], places=1)
        self.assertAlmostEqual(state[5], vel[2], places=1)

        # check attitude (in degrees)
        s_rpy = quat_rpy(state[6:10])
        self.assertAlmostEqual(s_rpy[0], rpy[0], places=0)
        self.assertAlmostEqual(s_rpy[1], rpy[1], places=0)
        self.assertTrue(abs(math.fmod(s_rpy[2] - rpy[2], 360)) < 5)

        # check bias terms (gyros and accels)
        self.assertAlmostEqual(state[10], bias[0], places=0)
        self.assertAlmostEqual(state[11], bias[1], places=0)
        self.assertAlmostEqual(state[12], bias[2], places=0)
        self.assertAlmostEqual(state[13], bias[3], places=0)
        self.assertAlmostEqual(state[14], bias[4], places=0)
        self.assertAlmostEqual(state[15], bias[5], places=1)

    def plot(self, times, history, history_rpy, true_pos, true_vel, true_rpy):
        from numpy import cos, sin

        import matplotlib.pyplot as plt

        fig, ax = plt.subplots(2, 2, sharex=True)

        k = times.size

        ax[0][0].cla()
        ax[0][0].plot(times[0:k:4], true_pos[0:k:4, :], "k--")
        ax[0][0].plot(times[0:k:4], history[0:k:4, 0:3])
        ax[0][0].set_title("Position")
        plt.sca(ax[0][0])
        plt.ylabel("m")

        ax[0][1].cla()
        ax[0][1].plot(times[0:k:4], true_vel[0:k:4, :], "k--")
        ax[0][1].plot(times[0:k:4], history[0:k:4, 3:6])
        ax[0][1].set_title("Velocity")
        plt.sca(ax[0][1])
        plt.ylabel("m/s")

        ax[1][0].cla()
        ax[1][0].plot(times[0:k:4], true_rpy[0:k:4, :], "k--")
        ax[1][0].plot(times[0:k:4], history_rpy[0:k:4, :])
        ax[1][0].set_title("Attitude")
        plt.sca(ax[1][0])
        plt.ylabel("Angle (Deg)")
        plt.xlabel("Time (s)")

        ax[1][1].cla()
        ax[1][1].plot(times[0:k:4], history[0:k:4, 10:13])
        ax[1][1].plot(times[0:k:4], history[0:k:4, -1])
        ax[1][1].set_title("Biases")
        plt.sca(ax[1][1])
        plt.ylabel("Bias (rad/s)")
        plt.xlabel("Time (s)")

        plt.suptitle(unittest.TestCase.shortDescription(self))
        plt.show()

    def test_circle(self, STEPS=50000):
        """ test that the INS gets a good fit for a simulated flight of 
        circles
        """

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        numpy.random.seed(1)

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        for k in range(STEPS):

            model.fly_circle(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3) * 1e-3
            na = numpy.random.randn(3) * 1e-3
            np = numpy.random.randn(3) * 1e-3
            nv = numpy.random.randn(3) * 1e-3
            nm = numpy.random.randn(3) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            sim.predict(gyro + ng, accel + na, dT=dT)

            if True and k % 60 == 59:
                sim.correction(pos=true_pos[k, :] + np)

            if True and k % 60 == 59:
                sim.correction(vel=true_vel[k, :] + nv)

            if k % 20 == 8:
                sim.correction(baro=-true_pos[k, 2] + np[2])

            if True and k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

            if k > 100:
                numpy.testing.assert_almost_equal(sim.state[0:3], true_pos[k, :], decimal=0)
                numpy.testing.assert_almost_equal(sim.state[3:6], true_vel[k, :], decimal=0)
                # only test roll and pitch because of wraparound issues
                numpy.testing.assert_almost_equal(history_rpy[k, 0:2], true_rpy[k, 0:2], decimal=1)

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy)

        return sim.state, history, times

    def test_gyro_bias_circle(self):
        """ test that while flying a circle the location converges
        """

        STEPS = 100000

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        numpy.random.seed(1)

        for k in range(STEPS):

            model.fly_circle(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3) * 1e-3
            na = numpy.random.randn(3) * 1e-3
            np = numpy.random.randn(3) * 1e-3
            nv = numpy.random.randn(3) * 1e-3
            nm = numpy.random.randn(3) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            # add simulated bias
            gyro = gyro + numpy.array([0.1, -0.05, 0.15])

            sim.predict(gyro + ng, accel + na, dT=dT)

            pos = model.get_pos()

            if k % 60 == 59:
                sim.correction(pos=pos + np)
            if k % 60 == 59:
                sim.correction(vel=model.get_vel() + nv)
            if k % 20 == 8:
                sim.correction(baro=-pos[2] + np[2])
            if k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy)

        self.assertState(
            sim.state, pos=model.get_pos(), vel=model.get_vel(), rpy=model.get_rpy(), bias=[0, 0, 0, 0, 0, 0]
        )

    def test_accel_bias_circle(self):
        """ test that while flying a circle the location converges
        """

        STEPS = 100000

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        numpy.random.seed(1)

        for k in range(STEPS):

            model.fly_circle(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3) * 1e-3
            na = numpy.random.randn(3) * 1e-3
            np = numpy.random.randn(3) * 1e-3
            nv = numpy.random.randn(3) * 1e-3
            nm = numpy.random.randn(3) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            # add simulated bias
            accel = accel + numpy.array([0.0, 0, 0.2])

            sim.predict(gyro + ng, accel + na, dT=dT)

            pos = model.get_pos()

            if k % 60 == 59:
                sim.correction(pos=pos + np)
            if k % 60 == 59:
                sim.correction(vel=model.get_vel() + nv)
            if k % 20 == 8:
                sim.correction(baro=-pos[2] + np[2])
            if k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy)

        self.assertState(
            sim.state, pos=model.get_pos(), vel=model.get_vel(), rpy=model.get_rpy(), bias=[0, 0, 0, 0, 0, 0.2]
        )

    def test_bad_init_q(self):
        """ test that while flying a circle the location converges with a bad initial attitude
        """

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        STEPS = 60 * 666

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        numpy.random.seed(1)

        ins.set_state(q=numpy.array([math.sqrt(2), math.sqrt(2), 0, 0]))

        for k in range(STEPS):

            model.fly_circle(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3) * 1e-3
            na = numpy.random.randn(3) * 1e-3
            np = numpy.random.randn(3) * 1e-3
            nv = numpy.random.randn(3) * 1e-3
            nm = numpy.random.randn(3) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            sim.predict(gyro + ng, accel + na, dT=dT)

            pos = model.get_pos()

            if k % 60 == 59:
                sim.correction(pos=pos + np)
            if k % 60 == 59:
                sim.correction(vel=model.get_vel() + nv)
            if k % 20 == 8:
                sim.correction(baro=-pos[2] + np[2])
            if k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy)

        self.assertState(
            sim.state, pos=model.get_pos(), vel=model.get_vel(), rpy=model.get_rpy(), bias=[0, 0, 0, 0, 0, 0]
        )

    def rock_and_turn(self, STEPS=50000):
        """ test the biases work when rocking and turning. this tests the
            mag attitude compensation """

        sim = self.sim
        model = self.model

        dT = 1.0 / 666.0

        numpy.random.seed(1)

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))

        true_pos = numpy.zeros((STEPS, 3))
        true_vel = numpy.zeros((STEPS, 3))
        true_rpy = numpy.zeros((STEPS, 3))

        times = numpy.zeros((STEPS, 1))

        for k in range(STEPS):

            model.rock_and_turn(dT=dT)

            true_pos[k, :] = model.get_pos()
            true_vel[k, :] = model.get_vel()
            true_rpy[k, :] = model.get_rpy()

            ng = numpy.random.randn(3) * 1e-3
            na = numpy.random.randn(3) * 1e-3
            np = numpy.random.randn(3) * 1e-3
            nv = numpy.random.randn(3) * 1e-3
            nm = numpy.random.randn(3) * 10.0

            # convert from rad/s to deg/s
            gyro = model.get_gyro() / 180.0 * math.pi
            accel = model.get_accel()

            sim.predict(gyro + ng, accel + na, dT=dT)

            if True and k % 60 == 59:
                sim.correction(pos=true_pos[k, :] + np)

            if True and k % 60 == 59:
                sim.correction(vel=true_vel[k, :] + nv)

            if k % 20 == 8:
                sim.correction(baro=-true_pos[k, 2] + np[2])

            if True and k % 20 == 15:
                sim.correction(mag=model.get_mag() + nm)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

            if k > 100:
                numpy.testing.assert_almost_equal(sim.state[0:3], true_pos[k, :], decimal=0)
                numpy.testing.assert_almost_equal(sim.state[3:6], true_vel[k, :], decimal=0)
                # only test roll and pitch because of wraparound issues
                numpy.testing.assert_almost_equal(history_rpy[k, 0:2], true_rpy[k, 0:2], decimal=1)

        if VISUALIZE:
            self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy)

        return sim.state, history, times
Ejemplo n.º 6
0
class StepTestFunctions(unittest.TestCase):
    def setUp(self):
        if C_IMP:
            self.sim = CINS()
        else:
            self.sim = PyINS()
        self.sim.prepare()

    def run_step(
        self,
        accel=[0.0, 0.0, -CINS.GRAV],
        gyro=[0.0, 0.0, 0.0],
        mag=[400, 0, 1600],
        pos=[0, 0, 0],
        vel=[0, 0, 0],
        noise=False,
        STEPS=200000,
        CHANGE=6660 * 3,
    ):
        """ simulate a static set of inputs and measurements
        """

        sim = self.sim

        dT = 1.0 / 666.0

        # numpy.random.seed(1)

        history = numpy.zeros((STEPS, 16))
        history_rpy = numpy.zeros((STEPS, 3))
        times = numpy.zeros((STEPS, 1))

        for k in range(STEPS):
            ng = numpy.zeros(3)
            na = numpy.zeros(3)
            np = numpy.zeros(3)
            nv = numpy.zeros(3)
            nm = numpy.zeros(3)

            if noise:
                ng = numpy.random.randn(3) * 1e-3
                na = numpy.random.randn(3) * 1e-3
                np = numpy.random.randn(3) * 1e-3
                nv = numpy.random.randn(3) * 1e-3
                nm = numpy.random.randn(3) * 10.0

            if k < CHANGE:
                sim.predict(ng, numpy.array([0, 0, -CINS.GRAV]) + na, dT=dT)
            else:
                sim.predict(gyro + ng, accel + na, dT=dT)

            history[k, :] = sim.state
            history_rpy[k, :] = quat_rpy(sim.state[6:10])
            times[k] = k * dT

            if True and k % 60 == 59:
                sim.correction(pos=pos + np)

            if True and k % 60 == 59:
                sim.correction(vel=vel + nv)

            if k % 20 == 8:
                sim.correction(baro=-pos[2] + np[2])

            if True and k % 20 == 15:
                sim.correction(mag=mag + nm)

        if VISUALIZE:
            from numpy import cos, sin

            import matplotlib.pyplot as plt

            fig, ax = plt.subplots(2, 2, sharex=True)

            k = STEPS

            ax[0][0].cla()
            ax[0][0].plot(times[0:k:4], history[0:k:4, 0:3])
            ax[0][0].set_title("Position")
            plt.sca(ax[0][0])
            plt.ylabel("m")
            ax[0][1].cla()
            ax[0][1].plot(times[0:k:4], history[0:k:4, 3:6])
            ax[0][1].set_title("Velocity")
            plt.sca(ax[0][1])
            plt.ylabel("m/s")
            # plt.ylim(-2,2)
            ax[1][0].cla()
            ax[1][0].plot(times[0:k:4], history_rpy[0:k:4, :])
            ax[1][0].set_title("Attitude")
            plt.sca(ax[1][0])
            plt.ylabel("Angle (Deg)")
            plt.xlabel("Time (s)")
            # plt.ylim(-1.1,1.1)
            ax[1][1].cla()
            ax[1][1].plot(times[0:k:4], history[0:k:4, 10:13], label="Gyro")
            ax[1][1].plot(times[0:k:4], history[0:k:4, -1], label="Accel")
            ax[1][1].set_title("Biases")
            plt.sca(ax[1][1])
            plt.ylabel("Bias (rad/s)")
            plt.xlabel("Time (s)")
            plt.legend()

            plt.suptitle(unittest.TestCase.shortDescription(self))
            plt.show()

        return sim.state, history, times

    def assertState(self, state, pos=[0, 0, 0], vel=[0, 0, 0], rpy=[0, 0, 0], bias=[0, 0, 0, 0, 0, 0]):
        """ check that the state is near a desired position
        """

        # check position
        self.assertAlmostEqual(state[0], pos[0], places=1)
        self.assertAlmostEqual(state[1], pos[1], places=1)
        self.assertAlmostEqual(state[2], pos[2], places=1)

        # check velocity
        self.assertAlmostEqual(state[3], vel[0], places=1)
        self.assertAlmostEqual(state[4], vel[1], places=1)
        self.assertAlmostEqual(state[5], vel[2], places=1)

        # check attitude (in degrees)
        s_rpy = quat_rpy(state[6:10])
        self.assertAlmostEqual(s_rpy[0], rpy[0], places=0)
        self.assertAlmostEqual(s_rpy[1], rpy[1], places=0)
        self.assertTrue(abs(math.fmod(s_rpy[2] - rpy[2], 360)) < 1)

        # check bias terms (gyros and accels)
        self.assertAlmostEqual(state[10], bias[0], places=2)
        self.assertAlmostEqual(state[11], bias[1], places=2)
        self.assertAlmostEqual(state[12], bias[2], places=2)
        self.assertAlmostEqual(state[13], bias[3], places=2)
        self.assertAlmostEqual(state[14], bias[4], places=2)
        self.assertAlmostEqual(state[15], bias[5], places=2)

    def test_accel_bias(self):
        """ test convergence with biased accelerometers
        """

        bias = -0.20
        state, history, times = self.run_step(accel=[0, 0, -PyINS.GRAV + bias], STEPS=90 * 666)
        self.assertState(state, bias=[0, 0, 0, 0, 0, bias])

    def test_gyro_bias(self):
        """ test convergence with biased gyros
        """

        state, history, times = self.run_step(gyro=[0.1, -0.05, 0.06], STEPS=90 * 666)
        self.assertState(state, bias=[0.1, -0.05, 0.06, 0, 0, 0])

    def test_gyro_bias_xy_rate(self):
        """ test gyro xy bias converges within 10% in a fixed time
        """

        TIME = 90  # seconds
        FS = 666  # sampling rate
        MAX_ERR = 0.1
        BIAS = 10.0 * math.pi / 180.0

        state, history, times = self.run_step(gyro=[BIAS, -BIAS, 0], STEPS=TIME * FS)

        self.assertAlmostEqual(state[10], BIAS, delta=BIAS * MAX_ERR)
        self.assertAlmostEqual(state[11], -BIAS, delta=BIAS * MAX_ERR)

    def test_gyro_bias_z_rate(self):
        """ test gyro z bias converges within 10% in a fixed time
        """

        TIME = 90  # seconds
        FS = 666  # sampling rate
        MAX_ERR = 0.1
        BIAS = 10.0 * math.pi / 180.0

        state, history, times = self.run_step(gyro=[0, 0, BIAS], STEPS=TIME * FS)

        self.assertAlmostEqual(state[12], BIAS, delta=BIAS * MAX_ERR)

    def test_accel_bias_z_rate(self):
        """ test accel z bias converges within 10% in a fixed time
        """

        TIME = 90  # seconds
        FS = 666  # sampling rate
        MAX_ERR = 0.1
        BIAS = 1

        state, history, times = self.run_step(accel=[0, 0, -PyINS.GRAV + BIAS], STEPS=TIME * FS)

        self.assertAlmostEqual(state[15], BIAS, delta=BIAS * MAX_ERR)