예제 #1
0
    def test_init_bad_q(self):
        """ test convergence with bad initial attitude
        """

        ins.set_state(q=numpy.array([0.7, 0.7, 0, 0]))
        state, history, times = self.run_static()  #STEPS=5000)
        self.assertState(state)
예제 #2
0
    def test_init_bad_bias(self):
        """ test convergence with bad initial gyro biases
        """

        ins.set_state(gyro_bias=numpy.array([0.05, 0.05, -0.05]))
        state, history, times = self.run_static(STEPS=100000)
        self.assertState(state)
예제 #3
0
    def test_init_100m(self):
        """ test convergence to origin when initialized 100m away
        """

        ins.set_state(pos=numpy.array([100.0, 100.0, 50]))
        state, history, times = self.run_static(STEPS=50000)
        self.assertState(state)
예제 #4
0
파일: test.py 프로젝트: ducminhkt/dRonin
    def test_init_bad_q_bias(self):
        """ test convergence with bad initial attitude and biases
        """

        ins.set_state(q=numpy.array([0.7, 0.7, 0, 0]), gyro_bias=numpy.array([0.05, 0.05, -0.05]))
        state, history, times = self.run_static(STEPS=100000)
        self.assertState(state)
예제 #5
0
파일: test.py 프로젝트: ducminhkt/dRonin
    def test_init_bad_q(self):
        """ test convergence with bad initial attitude
        """

        ins.set_state(q=numpy.array([0.7, 0.7, 0, 0]))
        state, history, times = self.run_static()  # STEPS=5000)
        self.assertState(state)
예제 #6
0
파일: test.py 프로젝트: ducminhkt/dRonin
    def test_init_100m(self):
        """ test convergence to origin when initialized 100m away
        """

        ins.set_state(pos=numpy.array([100.0, 100.0, 50]))
        state, history, times = self.run_static(STEPS=50000)
        self.assertState(state)
예제 #7
0
    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])
예제 #8
0
파일: test.py 프로젝트: ducminhkt/dRonin
    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]
        )