Example #1
0
    def setUp(self):

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

        self.c_sim.prepare()
        self.py_sim.prepare()
Example #2
0
    def setUp(self):

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

        self.c_sim.prepare()
        self.py_sim.prepare()
Example #3
0
    def setUp(self):
        if C_IMP:
            self.sim = CINS()
        else:
            self.sim = PyINS()
        self.sim.prepare()

        import simulation
        self.model = simulation.Simulation()
Example #4
0
    def load(self, filename):
        self.sim = CINS()
        self.sim.prepare()

        # add python directory to search path so the modules can be loaded
        import sys,os
        sys.path.insert(1, os.path.dirname(sys.path[0]))

        import taulabs
        import cPickle

        # load data from file
        handle = open(filename, 'rb')
        githash = cPickle.load(handle)
        print "Attempting to load data with githash of " + `githash`
        uavo_parsed = cPickle.load(handle)
        handle.close()

        # prepare the parser
        uavo_defs = taulabs.uavo_collection.UAVOCollection()
        uavo_defs.from_git_hash(githash)
        parser = taulabs.uavtalk.UavTalk(uavo_defs)

        print "Converting log records into python objects"
        uavo_list = taulabs.uavo_list.UAVOList(uavo_defs)
        for obj_id, data, timestamp in uavo_parsed:
            if obj_id in uavo_defs:
                obj = uavo_defs[obj_id]
                u = obj.instance_from_bytes(data, timestamp)
                uavo_list.append(u)
            else:
                print "Missing key: " + `obj_id`

        # We're done with this (potentially very large) variable, delete it.
        del uavo_parsed

        self.uavo_list = uavo_list
Example #5
0
 def setUp(self):
     if C_IMP:
         self.sim = CINS()
     else:
         self.sim = PyINS()
     self.sim.prepare()
Example #6
0
class ReplayFlightFunctions():

    def __init__(self):
        self.uavolist = []

    def load(self, filename):
        self.sim = CINS()
        self.sim.prepare()

        # add python directory to search path so the modules can be loaded
        import sys,os
        sys.path.insert(1, os.path.dirname(sys.path[0]))

        import taulabs
        import cPickle

        # load data from file
        handle = open(filename, 'rb')
        githash = cPickle.load(handle)
        print "Attempting to load data with githash of " + `githash`
        uavo_parsed = cPickle.load(handle)
        handle.close()

        # prepare the parser
        uavo_defs = taulabs.uavo_collection.UAVOCollection()
        uavo_defs.from_git_hash(githash)
        parser = taulabs.uavtalk.UavTalk(uavo_defs)

        print "Converting log records into python objects"
        uavo_list = taulabs.uavo_list.UAVOList(uavo_defs)
        for obj_id, data, timestamp in uavo_parsed:
            if obj_id in uavo_defs:
                obj = uavo_defs[obj_id]
                u = obj.instance_from_bytes(data, timestamp)
                uavo_list.append(u)
            else:
                print "Missing key: " + `obj_id`

        # We're done with this (potentially very large) variable, delete it.
        del uavo_parsed

        self.uavo_list = uavo_list

    def run_uavo_list(self):

        import taulabs
        import math

        uavo_list = self.uavo_list

        print "Replaying log file"

        attitude = uavo_list.as_numpy_array(taulabs.uavo.UAVO_AttitudeActual)
        gyros = uavo_list.as_numpy_array(taulabs.uavo.UAVO_Gyros)
        accels = uavo_list.as_numpy_array(taulabs.uavo.UAVO_Accels)
        #ned = uavo_list.as_numpy_array(taulabs.uavo.UAVO_NEDPosition)
        gps = uavo_list.as_numpy_array(taulabs.uavo.UAVO_GPSPosition)
        vel = uavo_list.as_numpy_array(taulabs.uavo.UAVO_GPSVelocity)
        mag = uavo_list.as_numpy_array(taulabs.uavo.UAVO_Magnetometer)
        baro = uavo_list.as_numpy_array(taulabs.uavo.UAVO_BaroAltitude)

        if gps.size == 0:
            print "Unable to process flight. No GPS data"
            return

        # set home location as first sample and linearize around that
        lat0 = gps['Latitude'][0,0]
        lon0 = gps['Longitude'][0,0]
        alt0 = gps['Altitude'][0,0]

        T = [alt0+6.378137E6, cos(lat0 / 10e6 * math.pi / 180.0)*(alt0+6.378137E6)]

        STEPS = gyros['time'].size
        history = numpy.zeros((STEPS,16))
        history_rpy = numpy.zeros((STEPS,3))
        times = numpy.zeros((STEPS,1))

        ned_history = numpy.zeros((gps['Latitude'].size,3))

        steps = 0
        t = gyros['time'][0]
        gyro_idx = 0
        accel_idx = 0
        gps_idx = 0
        vel_idx = 0
        mag_idx = 0
        baro_idx = 0

        dT = numpy.mean(numpy.diff(gyros['time']))

        for gyro_idx in numpy.arange(STEPS):

            t = gyros['time'][gyro_idx]
            steps = gyro_idx
            accel_idx = (numpy.abs(accels['time']-t)).argmin()
                
            gyros_dat = numpy.array([gyros['x'][gyro_idx],gyros['y'][gyro_idx],gyros['z'][gyro_idx]]).T[0]
            accels_dat = numpy.array([accels['x'][accel_idx],accels['y'][accel_idx],accels['z'][accel_idx]]).T[0]

            gyros_dat = gyros_dat / 180 * math.pi
            self.sim.predict(gyros_dat,accels_dat,dT)

            if (gps_idx < gps['time'].size) and (gps['time'][gps_idx] < t):
                pos = [(gps['Latitude'][gps_idx,0] - lat0) / 10e6 * math.pi / 180.0 * T[0], (gps['Longitude'][gps_idx,0] - lon0)  / 10e6 * math.pi / 180.0 * T[1], -(gps['Altitude'][gps_idx,0]-alt0)]
                self.sim.correction(pos=pos)
                ned_history[gps_idx,0] = pos[0]
                ned_history[gps_idx,1] = pos[1]
                ned_history[gps_idx,2] = pos[2]
                gps_idx = gps_idx + 1

            if (vel_idx < vel['time'].size) and (vel['time'][vel_idx] < t):
                self.sim.correction(vel=[vel['North'][vel_idx,0], vel['East'][vel_idx,0], vel['Down'][vel_idx,0]])
                vel_idx = vel_idx + 1

            if (mag_idx < mag['time'].size) and (mag['time'][mag_idx] < t):
                self.sim.correction(mag=[mag['x'][mag_idx,0], mag['y'][mag_idx,0], mag['z'][mag_idx,0]])
                mag_idx = mag_idx + 1

            if (baro_idx < baro['time'].size) and (baro['time'][baro_idx] < t):
                self.sim.correction(baro=baro['Altitude'][baro_idx,0])
                baro_idx = baro_idx + 1

            history[steps,:] = self.sim.state
            history_rpy[steps,:] = quat_rpy(self.sim.state[6:10])
            times[steps] = t
        print "Plotting results"
       
        import matplotlib.pyplot as plt
        fig, ax = plt.subplots(2,2,sharex=True)
        ax[0][0].cla()
        ax[0][0].plot(gps['time'],ned_history[:,0],'b--',label="North")
        ax[0][0].plot(gps['time'],ned_history[:,1],'g--',label="East")
        ax[0][0].plot(gps['time'],ned_history[:,2],'r--',label="Down"),
        ax[0][0].plot(baro['time'], -baro['Altitude'],'--',label="Baro")
        ax[0][0].plot(times,history[:,0],'b',label="Replay")
        ax[0][0].plot(times,history[:,1],'g',label="Replay")
        ax[0][0].plot(times,history[:,2],'r',label="Replay")
        ax[0][0].set_title('Position')
        plt.sca(ax[0][0])
        plt.ylabel('m')
        plt.legend()

        ax[0][1].cla()
        ax[0][1].plot(vel['time'],vel['North'],'b--',label="North")
        ax[0][1].plot(vel['time'],vel['East'],'g--',label="East")
        ax[0][1].plot(vel['time'],vel['Down'],'r--',label="Down")
        ax[0][1].plot(times,history[:,3],'b',label="Replay")
        ax[0][1].plot(times,history[:,4],'g',label="Replay")
        ax[0][1].plot(times,history[:,5],'r',label="Replay")
        ax[0][1].set_title('Velocity')
        plt.sca(ax[0][1])
        plt.ylabel('m/s')
        plt.legend()

        ax[1][0].cla()
        ax[1][0].plot(attitude['time'],attitude['Roll'],'--',label="Roll")
        ax[1][0].plot(attitude['time'],attitude['Pitch'],'--',label="Pitch")
        ax[1][0].plot(attitude['time'],attitude['Yaw'],'--',label="Yaw")
        ax[1][0].plot(times,history_rpy[:,:], label="Replay")
        ax[1][0].set_title('Attitude')
        plt.sca(ax[1][0])
        plt.ylabel('Angle (Deg)')
        plt.xlabel('Time (s)')
        plt.legend()

        ax[1][1].cla()
        ax[1][1].plot(times,history[:,10:13]*180/3.1415,label="Gyro Bias")
        ax[1][1].plot(times,history[:,-1],label="Z Bias")
        ax[1][1].set_title('Biases')
        plt.sca(ax[1][1])
        plt.ylabel('Bias (rad/s)')
        plt.xlabel('Time (s)')
        plt.legend()

        plt.draw()
        plt.show()

        return ins
Example #7
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])
Example #8
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])