Пример #1
0
def test_dmu380_sim():
    '''
    test Sim with DMU380 algorithm.
    '''
    print("This demo only runs on Ubuntu x64.")
    #### choose a built-in IMU model, typical for IMU380
    imu_err = 'mid-accuracy'
    # do not generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=False)

    #### Algorithm
    # DMU380 algorithm
    from demo_algorithms import dmu380_sim
    cfg_file = os.path.abspath('.//demo_algorithms//dmu380_sim_lib//ekfSim_tilt.cfg')
    algo = dmu380_sim.DMU380Sim(cfg_file)

    #### start simulation
    sim = ins_sim.Sim([fs, 0.0, fs],
                      motion_def_path+"//motion_def-Komatsu_level_50m_0_0.csv",
                      ref_frame=1,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=algo)
    sim.run(10)
    # generate simulation results, summary, and save data to files
    sim.results()  # do not save data
    # plot data
    sim.plot(['att_euler'])
Пример #2
0
def test_path_gen():
    '''
    test only path generation in Sim.
    '''
    #### choose a built-in IMU model, typical for IMU381
    imu_err = 'mid-accuracy'
    # generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)

    #### Algorithm
    # ECF based inclinometer
    from demo_algorithms import inclinometer_mahony
    algo1 = inclinometer_mahony.MahonyFilter()
    from demo_algorithms import inclinometer_acc
    algo2 = inclinometer_acc.TiltAcc()

    #### start simulation
    sim = ins_sim.Sim(
        [fs, fs_gps, fs_mag],
        motion_def_path + "//motion_def-static.csv",
        #   'e://Projects//gnss-ins-sim//demo_saved_data//2018-04-24-14-55-53//',
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,
        algorithm=[algo1, algo2])
    sim.run(3)
    # save simulation data to files
    sim.results('.//demo_saved_data//')
    # plot data, 3d plot of reference positoin, 2d plots of gyro and accel
    sim.plot(['att_euler'], opt={'att_euler': 'error'})
def test_inclinometer_mahony():
    '''
    test Sim with a inclinometer algorithm based on Mahony's theory.
    '''
    #### choose a built-in IMU model, typical for IMU380
    imu_err = 'low-accuracy'
    # do not generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)

    #### Algorithm
    # ECF based inclinometer
    from demo_algorithms import inclinometer_mahony
    algo = inclinometer_mahony.MahonyFilter()

    #### start simulation
    sim = ins_sim.Sim([fs, 0.0, 0.0],
                      motion_def_path + "//motion_def.csv",
                      ref_frame=1,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=algo)
    sim.run()
    # generate simulation results, summary, and save data to files
    sim.results()  # do not save data
    # plot data
    sim.plot(['att_euler', 'ref_att_euler'], opt={'att_euler': 'error'})
Пример #4
0
def test_path_gen():
    '''
    test only path generation in Sim.
    '''
    #### imu_err
    imu_err = {'gyro_b': np.array([1e-2, 2e-2, 3e-2]) / D2R * 3600 * 0,
               'gyro_arw': np.array([1e-5, 1e-5, 1e-5]) / D2R * 60,
               'gyro_b_stability': np.array([5e-6, 5e-6, 5e-6]) / D2R * 3600,
               'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
               'accel_b': np.array([2.0e-3, 1.0e-3, 5.0e-3]) * 0,
               'accel_vrw': np.array([1e-4, 1e-4, 1e-4]) * 60.0,
               'accel_b_stability': np.array([1e-5, 1e-5, 1e-5]) * 1.0,
               'accel_b_corr': np.array([200.0, 200.0, 200.0]),
            #    'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
              }
    # generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True)

    #### start simulation
    sim = ins_sim.Sim([fs, fs_gps, fs_mag],
                      motion_def_path+"//imu_def6.csv",
                      ref_frame=1,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=None)
    sim.run(1)
    # save simulation data to files
    sim.results('/home/zsp/Desktop/imu/data5')
    # plot data, 3d plot of reference positoin, 2d plots of gyro and accel
    # sim.plot(['gyro', 'accel'])
    sim.plot(['ref_pos', 'gyro', 'gps_visibility'], opt={'ref_pos': '3d'})
def test_free_integration():
    '''
    test Sim
    '''
    #### do not need an IMU model
    imu = None

    #### Algorithm
    from demo_algorithms import free_integration
    ini_pos_vel_att = np.genfromtxt(log_dir+"ini.txt", delimiter=',')
    ini_pos_vel_att[0:2] *= attitude.D2R  # For Lat and Lon, deg to rad
    ini_pos_vel_att[6:9] *= attitude.D2R  # Attitude from deg to rad
    if not using_external_g:
        ini_pos_vel_att = ini_pos_vel_att[0:9]
    # create the algorithm object
    algo = free_integration.FreeIntegration(ini_pos_vel_att, earth_rot=False)

    from demo_algorithms import inclinometer_acc
    algo2 = inclinometer_acc.TiltAcc()

    #### start simulation
    sim = ins_sim.Sim([fs, 0.0, 0.0],
                      log_dir,
                      ref_frame=0,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=[algo, algo2])
    # run the simulation
    sim.run(1)
    # generate simulation results, summary
    sim.results('', end_point=True, extra_opt='ned')
    # plot
    sim.plot(['pos', 'vel', 'att_euler', 'accel', 'gyro'],
             opt={'pos':'error', 'vel':'error', 'att_euler':'error'})
Пример #6
0
def test_path_gen():
    '''
    test ANS as GUI.
    '''
    #### simulation config
    motion_def_path = os.path.abspath('.//demo_motion_def_files//')
    fs = 100.0          # IMU sample frequency
    fs_gps = 10.0       # GPS sample frequency
    fs_mag = fs         # magnetometer sample frequency, not used for now

    #### choose a built-in IMU model, typical for IMU381
    imu_err = 'mid-accuracy'
    # generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)

    #### start simulation
    with open(motion_def_path+"//motion_def-3d.csv", 'r') as f:
        tmp = f.read()
    sim = ins_sim.Sim([fs, fs_gps, fs_mag],
                      tmp,
                      ref_frame=0,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=None)
    sim.run(1)
    # save simulation data to files
    sim.results('')

    #### GUI
    gui = gui_ans.GuiAns()
    gui.start(sim)
def test_path_gen():
    '''
    test only path generation in Sim.
    '''
    # imu_err
    imu_err = {'gyro_b': np.array([1e-2, 2e-2, 3e-2]) / D2R * 3600 * 0,
               'gyro_arw': np.array([1e-5, 1e-5, 1e-5]) / D2R * 60 * 0,
               'gyro_b_stability': np.array([1e-5, 1e-5, 1e-5]) / D2R * 3600 * 1e-0,
                  'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
               'accel_b': np.array([2.0e-3, 1.0e-3, 5.0e-3]) * 0,
               'accel_vrw': np.array([1e-4, 1e-4, 1e-4]) * 60.0 * 0,
               'accel_b_stability': np.array([1e-4, 1e-4, 1e-4]) * 1.0 * 1e0,
                  'accel_b_corr': np.array([200.0, 200.0, 200.0]),
               #    'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
               }
    # generate GPS and magnetometer data
    gps_err = {
        'stdp': np.array([1, 1, 1]) * 1e-6,
        'stdv': np.array([0, 0, 0]),
    }
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True, gps_opt=gps_err)

    # start simulation
    sim = ins_sim.Sim([fs, fs_gps, fs_mag],
                      motion_def_path+"//imu_def7.csv",
                      ref_frame=0,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=None)
    sim.run(1)
    # save simulation data to files
    sim.results('/imu/data7')
Пример #8
0
def create_sim_object(algorithm,
                      motion_path_filename,
                      fs=100,
                      fs_gps=0.0,
                      fs_mag=0.0,
                      imu=imu_creation()):
    """
        'ref_frame'	Reference frame used as the navigation frame and the attitude reference.
        0: NED (default), with x axis pointing along geographic north, y axis pointing eastward, z axis pointing downward. Position will be expressed in LLA form, and the velocity of the vehicle relative to the ECEF frame will be expressed in local NED frame.
        1: a virtual inertial frame with constant g, x axis pointing along geographic or magnetic north, z axis pointing along g, y axis completing a right-handed coordinate system. Position and velocity will both be in the [x y z] form in this frame.
        **Notice: For this virtual inertial frame, position is indeed the sum of the initial position in ecef and the relative position in the virutal inertial frame. Indeed, two vectors expressed in different frames should not be added. This is done in this way here just to preserve all useful information to generate .kml files. Keep this in mind if you use this result.

        'fs'	Sample frequency of IMU, units: Hz
        'fs_gps'	Sample frequency of GNSS, units: Hz
        'fs_mag'	Sample frequency of magnetometer, units: Hz
        'time'	Time series corresponds to IMU samples, units: sec.
        'gps_time'	Time series corresponds to GNSS samples, units: sec.
        'algo_time'	Time series corresponding to algorithm output, units: ['s']. If your algorithm output data rate is different from the input data rate, you should include 'algo_time' in the algorithm output.
        'gps_visibility'	Indicate if GPS is available. 1 means yes, and 0 means no.
        'ref_pos'	True position in the navigation frame. When users choose NED (ref_frame=0) as the navigation frame, positions will be given in the form of [Latitude, Longitude, Altitude], units: ['rad', 'rad', 'm']. When users choose the virtual inertial frame, positions (initial position + positions relative to the origin of the frame) will be given in the form of [x, y, z], units: ['m', 'm', 'm'].
        'ref_vel'	True velocity w.r.t the navigation/reference frame expressed in the NED frame, units: ['m/s', 'm/s', 'm/s'].
        'ref_att_euler'	True attitude (Euler angles, ZYX rotation sequency), units: ['rad', 'rad', 'rad']
        'ref_att_quat'	True attitude (quaternions)
        'ref_gyro'	True angular velocity in the body frame, units: ['rad/s', 'rad/s', 'rad/s']
        'ref_accel'	True acceleration in the body frame, units: ['m/s^2', 'm/s^2', 'm/s^2']
        'ref_mag'	True geomagnetic field in the body frame, units: ['uT', 'uT', 'uT'] (only available when axis=9 in IMU object)
        'ref_gps'	True GPS position/velocity, ['rad', 'rad', 'm', 'm/s', 'm/s', 'm/s'] for NED (LLA), ['m', 'm', 'm', 'm/s', 'm/s', 'm/s'] for virtual inertial frame (xyz) (only available when gps=True in IMU object)
        'gyro'	Gyroscope measurements, 'ref_gyro' with errors
        'accel'	Accelerometer measurements, 'ref_accel' with errors
        'mag'	Magnetometer measurements, 'ref_mag' with errors
        'gps'	GPS measurements, 'ref_gps' with errors
        'ad_gyro'	Allan std of gyro, units: ['rad/s', 'rad/s', 'rad/s']
        'ad_accel'	Allan std of accel, units: ['m/s2', 'm/s2', 'm/s2']
        'pos'	Simulation position from algo, units: ['rad', 'rad', 'm'] for NED (LLA), ['m', 'm', 'm'] for virtual inertial frame (xyz).
        'vel'	Simulation velocity from algo, units: ['m/s', 'm/s', 'm/s']
        'att_euler'	Simulation attitude (Euler, ZYX) from algo, units: ['rad', 'rad', 'rad']
        'att_quat'	Simulation attitude (quaternion) from algo
        'wb'	Gyroscope bias estimation, units: ['rad/s', 'rad/s', 'rad/s']
        'ab'	Accelerometer bias estimation, units: ['m/s^2', 'm/s^2', 'm/s^2']
        'gyro_cal'	Calibrated gyro output, units: ['rad/s', 'rad/s', 'rad/s']
        'accel_cal'	Calibrated acceleromter output, units: ['m/s^2', 'm/s^2', 'm/s^2']
        'mag_cal'	Calibrated magnetometer output, units: ['uT', 'uT', 'uT']
        'soft_iron'	3x3 soft iron calibration matrix
        'hard_iron'	Hard iron calibration, units: ['uT', 'uT', 'uT']
    Returns:

    """

    sim = ins_sim.Sim(
        [fs, fs_gps, fs_mag],
        motion_def_path + motion_path_filename,
        ref_frame=1,
        imu=imu,
        # vehicle maneuver capability
        # [max accel, max angular accel, max angular rate]
        mode=None,
        env=None,
        algorithm=algorithm)
    return sim
Пример #9
0
def odometer_sim():
    #### IMU model, typical for IMU381
    imu_err = {
        'gyro_b': np.array([0.0, 0.0, 0.0]),
        'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
        'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
        'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
        'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
        'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
        'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
        'accel_b_corr': np.array([200.0, 200.0, 200.0]),
        'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
    }
    gps_err = {
        'stdp': np.array([5.0, 5.0, 7.0]) * 1.0,
        'stdv': np.array([0.05, 0.05, 0.05]) * 1.0
    }
    # generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True, gps_opt=gps_err)

    #### Algorithm
    # Free integration based on odometer in NED frame
    from demo_algorithms import gps_odo
    '''
    Initial states (position, velocity and attitude) are from motion_def csv file.
    '''
    ini_pos_vel_att = np.genfromtxt(motion_def_path+"//motion_def-ins_odo.csv",\
                                    delimiter=',', skip_header=1, max_rows=1)
    ini_pos_vel_att[0] = ini_pos_vel_att[
        0] * D2R  #将lat (deg),lon (deg), yaw (deg), pitch (deg), roll (deg)单位转为弧度 rad
    ini_pos_vel_att[1] = ini_pos_vel_att[1] * D2R
    ini_pos_vel_att[6:9] = ini_pos_vel_att[6:9] * D2R
    # add initial states error if needed
    ini_vel_err = np.array([0.0, 0.0, 0.0
                            ])  # initial velocity error in the body frame, m/s
    ini_att_err = np.array([0.0, 0.0, 0.0])  # initial Euler angles error, deg
    ini_pos_vel_att[3:6] += ini_vel_err
    ini_pos_vel_att[6:9] += ini_att_err * D2R
    # create the algorith object
    algo = gps_odo.OdometerSim(ini_pos_vel_att)

    #### start simulation
    sim = ins_sim.Sim(
        [fs, fs, 0.0],  # imu, gps, no mag.
        motion_def_path +
        "//motion_def-ins_odo.csv",  #  motion_def-90deg_turn_gps    motion_def-ins_odo
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,
        algorithm=algo)
    # run the simulation for 1 times
    sim.run(1)
    # generate simulation results, summary
    # save results
    sim.results(err_stats_start=-1, gen_kml=True)
    sim.plot(['ref_pos', 'gyro', 'gps_visibility', 'accel'],
             opt={'ref_pos': '3d'})
Пример #10
0
def test_free_integration():
    '''
    test Sim
    '''
    #### IMU model, typical for IMU381
    imu_err = {'gyro_b': np.array([0.0, 0.0, 0.0]),
               'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
               'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
               'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
               'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
               'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
               'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
               'accel_b_corr': np.array([200.0, 200.0, 200.0]),
               'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
              }
    odo_err = {'scale': 0.999,
               'stdv': 0.1}
    # do not generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False, odo=True, odo_opt=odo_err)

    #### Algorithm
    # Free integration in a virtual inertial frame
    from demo_algorithms import free_integration_odo
    from demo_algorithms import free_integration
    '''
    Free integration requires initial states (position, velocity and attitude). You should provide
    theses values when you create the algorithm object.
    '''
    ini_pos_vel_att = np.genfromtxt(motion_def_path+"//motion_def-90deg_turn.csv",\
                                    delimiter=',', skip_header=1, max_rows=1)
    ini_pos_vel_att[0] = ini_pos_vel_att[0] * D2R
    ini_pos_vel_att[1] = ini_pos_vel_att[1] * D2R
    ini_pos_vel_att[6:9] = ini_pos_vel_att[6:9] * D2R
    # add initial states error if needed
    ini_vel_err = np.array([0.0, 0.0, 0.0]) # initial velocity error in the body frame, m/s
    ini_att_err = np.array([0.0, 0.0, 0.0]) # initial Euler angles error, deg
    ini_pos_vel_att[3:6] += ini_vel_err
    ini_pos_vel_att[6:9] += ini_att_err * D2R
    # create the algorith object
    algo1 = free_integration_odo.FreeIntegration(ini_pos_vel_att)
    algo2 = free_integration.FreeIntegration(ini_pos_vel_att)

    #### start simulation
    sim = ins_sim.Sim([fs, 0.0, 0.0],
                      motion_def_path+"//motion_def-90deg_turn.csv",
                      ref_frame=1,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=[algo1, algo2])
    # run the simulation for 1000 times
    sim.run(10)
    # generate simulation results, summary
    # do not save data since the simulation runs for 1000 times and generates too many results
    sim.results(err_stats_start=-1, gen_kml=True)
Пример #11
0
def test_path_gen():
    '''
    test only path generation in Sim.
    '''
    #### choose a built-in IMU model, typical for IMU381
    #imu_err = 'low-accuracy'
    #imu_err = 'mid-accuracy'

    imu_err = {
            # gyro bias, deg/hr
            'gyro_b': np.array([0.0014, 0.0014, 0.0014]),
            # gyro angle random walk, deg/rt-hr
            'gyro_arw': np.array([0.00016, 0.00016, 0.00016]),
            # gyro bias instability, deg/hr
            'gyro_b_stability': np.array([0.0, 0.0, 0.0]),
            # gyro bias instability correlation, sec.
            # set this to 'inf' to use a random walk model
            # set this to a positive real number to use a first-order Gauss-Markkov model
            'gyro_b_corr': np.array([np.Inf, np.Inf, np.Inf]),
            # accelerometer bias, m/s^2
            'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
            # accelerometer velocity random walk, m/s/rt-hr
            'accel_vrw': np.array([5.0e-6, 5.0e-6, 5.0e-6]),
            # accelerometer bias instability, m/s^2
            'accel_b_stability': np.array([0.60e-3, 0.60e-3, 0.80e-3]),
            # accelerometer bias instability correlation, sec. Similar to gyro_b_corr
            'accel_b_corr': np.array([np.Inf, np.Inf, np.Inf]),
            # magnetometer noise std, uT
            'mag_std': np.array([0.2, 0.2, 0.2])
          }
    gps_err = {'stdp': np.array([5.0, 5.0, 7.0]) * 0.2,
               'stdv': np.array([0.05, 0.05, 0.05]) * 1.0}
    odo_err = {'scale': 0.999,
               'stdv': 0.01}
    # generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True, gps_opt=gps_err,
                        odo=True, odo_opt=odo_err)

    #### start simulation
    sim = ins_sim.Sim([fs, fs_gps, fs_mag],
                      motion_def_path+"motion_def-Holland_tunnel.csv",
                      ref_frame=1,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=None)
    sim.run()
    # plot data, 3d plot of reference positoin, 2d plots of gyro and accel
    sim.plot(['ref_att_euler', 'gyro', 'mag', 'accel'], opt={'ref_pos': '2d'})
    # save simulation data to files
    res = []
    res = sim.get_data(['time', 'gps_time', 'ref_gps', 'ref_pos', 'ref_vel', 'ref_accel', 'ref_gyro', 'odo', 'gyro', 'mag', 'accel'])
    return res
Пример #12
0
def get_gnss_ins_sim(motion_def_file, fs_imu, fs_gps):
    '''
    Generate simulated GNSS/IMU data using specified trajectory.
    '''
    # set IMU model:
    imu_err = 'low-accuracy'
    # generate GPS and magnetometer data:
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)

    # init simulation:
    sim = ins_sim.Sim(
        [fs_imu, fs_gps, fs_imu],
        motion_def_file,
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,
        algorithm=None
    )
    
    # run:
    sim.run(1)

    # get simulated data:
    rospy.logwarn(
        "Simulated data size {}".format(
            len(sim.dmgr.get_data_all('gyro').data[0])
        )
    )

    # imu measurements:
    step_size = 1.0 / fs_imu
    for i, (gyro, accel) in enumerate(
        zip(
            # a. gyro
            sim.dmgr.get_data_all('gyro').data[0], 
            # b. accel
            sim.dmgr.get_data_all('accel').data[0]
        )
    ):
        yield {
            'stamp': i * step_size,
            'data': {
                # a. gyro:
                'gyro_x': gyro[0],
                'gyro_y': gyro[1],
                'gyro_z': gyro[2],
                # b. accel:
                'accel_x': accel[0],
                'accel_y': accel[1],
                'accel_z': accel[2]
            }
        }
Пример #13
0
def test_dmu380_sim():
    '''
    test Sim with DMU380 algorithm.
    '''
    #### choose a built-in IMU model, typical for IMU380
    imu_err = {
        'gyro_b': np.array([1.0, -1.0, 0.5]) * 1800.0,
        'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
        'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
        'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
        'accel_b': np.array([5.0e-3, 5.0e-3, 5.0e-3]) * 10.0,
        'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
        'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
        'accel_b_corr': np.array([200.0, 200.0, 200.0]),
        'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
    }
    gps_err = {
        'stdp': np.array([5.0, 5.0, 7.0]) * 0.2,
        'stdv': np.array([0.05, 0.05, 0.05]) * 1.0
    }
    # generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True, gps_opt=gps_err)

    #### Algorithm
    # DMU380 algorithm
    from demo_algorithms import aceinna_ins
    cfg_file = os.path.abspath(
        './/demo_algorithms//dmu380_sim_lib//ekfSim_ins.cfg')
    algo = aceinna_ins.DMU380Sim(cfg_file)

    #### start simulation
    sim = ins_sim.Sim(
        [fs, 1, fs],
        motion_def_path + "//motion_def-long_drive.csv",
        #   ".//demo_saved_data//2019-08-27-10-26-14//",
        ref_frame=0,
        imu=imu,
        mode=None,
        env='[0.01 0.01 0.11]g-random',
        algorithm=algo)
    sim.run(1)
    # generate simulation results, summary, and save data to files
    sim.results('.//demo_saved_data//tmp',
                err_stats_start=210,
                gen_kml=True,
                extra_opt='ned')
    # plot data
    sim.plot(['pos', 'vel', 'wb', 'ab', 'att_euler'],
             opt={
                 'pos': 'error',
                 'vel': 'error',
                 'att_euler': 'error'
             })
Пример #14
0
def mahony_filter_test():
    '''
    test Sim
    '''
    #### IMU model, typical for IMU381
    imu_err = {
        'gyro_b': np.array([0.0, 0.0, 0.0]),
        'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
        'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
        'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
        'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
        'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
        'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
        'accel_b_corr': np.array([200.0, 200.0, 200.0]),
        'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
    }
    # do not generate GPS data
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=False)

    #### Algorithm
    # mahony filter in a virtual inertial frame.
    from demo_algorithms import vg_mahony
    '''
    calculate pitch, roll (and yaw if input mag data) by mahony filter.
    '''
    # create the algorith object
    algo = vg_mahony.MahonyTest()

    #### start simulation
    sim = ins_sim.Sim(
        [fs, 0.0, 0.0],
        # '/Users/songyang/project/code/github/gnss-ins-sim_learn/demo_data_files/bosch',
        motion_def_path +
        "//motion_def-90deg_turn_gps.csv",  #motion_def.csv   motion_def-90deg_turn.csv
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,
        algorithm=[algo])
    # run the simulation for 1000 times
    sim.run(1)
    # generate simulation results, summary
    # do not save data since the simulation runs for 1000 times and generates too many results
    sim.results(err_stats_start=-1, gen_kml=True)
    # plot postion error
    # sim.plot(['pos'], opt={'pos':'error'})
    sim.plot(['att_euler'], opt={'att_euler': 'error'})
Пример #15
0
def test_mag_cal():
    '''
    test soft iron and hard iron calibration.
    '''
    print("This demo only runs on Ubuntu x64.")
    #### IMU model, typical for IMU381
    imu_err = 'mid-accuracy'
    # do not generate GPS data
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=False)
    mag_error = {
        'si': np.eye(3) + np.random.randn(3, 3) * 0.1,
        'hi': np.array([10.0, 10.0, 10.0]) * 1.0
    }
    imu.set_mag_error(mag_error)
    #### Algorithm
    from demo_algorithms import mag_calibrate
    algo = mag_calibrate.MagCal()

    #### start simulation
    sim = ins_sim.Sim(
        [fs, fs_gps, fs_mag],
        motion_def_path + "//motion_def_mag_cal.csv",
        #   motion_def_path+"//test_mag_cal//",
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,
        algorithm=algo)
    sim.run(1)
    # save simulation data to files
    sim.results()
    # plot data
    sim.plot(['mag', 'mag_cal'],
             opt={
                 'mag': 'projection',
                 'mag_cal': 'projection'
             },
             extra_opt='.')
    # show calibration params:
    print('true soft iron is:')
    print(np.linalg.inv(mag_error['si']))
    print('estimated soft iron is:')
    print(sim.dmgr.soft_iron.data)
    print('true hard iron is:')
    print(mag_error['hi'])
    print('estimated hard iron is:')
    print(sim.dmgr.hard_iron.data)
Пример #16
0
def test_free_integration():
    '''
    test Sim
    '''
    #### do not need an IMU model
    imu = None

    #### Algorithm
    # Free integration in a virtual inertial frame
    from demo_algorithms import free_integration
    ini_pos_vel_att = np.genfromtxt(log_dir + "ini.txt")
    # add initial states error if needed
    ini_vel_err = np.array([0.0, 0.0, 0.0
                            ])  # initial velocity error in the body frame, m/s
    ini_att_err = np.array([0.0, 0.0, 0.0])  # initial Euler angles error, deg
    ini_pos_vel_att[0:2] = ini_pos_vel_att[0:2] * attitude.D2R
    ini_pos_vel_att[3:6] += ini_vel_err
    ini_pos_vel_att[6:9] = (ini_pos_vel_att[6:9] + ini_att_err) * attitude.D2R
    if not using_external_g:
        ini_pos_vel_att = ini_pos_vel_att[0:9]
    # create the algorithm object
    algo = free_integration.FreeIntegration(ini_pos_vel_att, earth_rot=False)

    from demo_algorithms import inclinometer_acc
    algo2 = inclinometer_acc.TiltAcc()

    #### start simulation
    sim = ins_sim.Sim([fs, 0.0, 0.0],
                      log_dir,
                      ref_frame=0,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=[algo, algo2])
    # run the simulation
    sim.run(1)
    # generate simulation results, summary
    sim.results('', end_point=True, extra_opt='ned')
    # plot
    sim.plot(['pos', 'vel', 'att_euler', 'accel'],
             opt={
                 'pos': 'error',
                 'vel': 'error',
                 'att_euler': 'error'
             })
def test_gen_data_from_files():
    '''
    test data generation from files.
    '''
    #### start simulation
    sim = ins_sim.Sim(
        [fs, fs_gps, fs_mag],
        'e://Projects//gnss-ins-sim//demo_saved_data//2018-05-16-14-23-34//',
        ref_frame=1,
        imu=None,
        mode=None,
        env=None,
        algorithm=None)
    sim.run()
    # save simulation data to files
    sim.results()
    # plot data
    sim.plot(['ref_att_euler', 'att_euler'], opt={'att_euler': 'error'})
Пример #18
0
def gen_data_first(data_dir, fileName="motion_tmp.csv"):
    '''
    Generate data that will be used by test_gen_data_from_files()
    '''
    # imu model
    imu = imu_model.IMU(accuracy='mid-accuracy', axis=6, gps=False)

    # start simulation
    sim = ins_sim.Sim([fs, fs_gps, fs_mag],
                      motion_def_path + "//" + fileName,
                      ref_frame=0,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=None)
    sim.run(10)
    # save simulation data to files
    sim.results(data_dir, stdout=False)
Пример #19
0
def test_dmu380_sim():
    '''
    test Sim with DMU380 algorithm.
    '''
    print("This demo only runs on Ubuntu x64.")
    #### choose a built-in IMU model, typical for IMU380
    imu_err = {
        'gyro_b': np.array([1.0, -1.0, 0.5]) * 1.0,
        'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
        'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
        'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
        'accel_b': np.array([50.0e-3, 50.0e-3, 50.0e-3]),
        'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
        'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
        'accel_b_corr': np.array([200.0, 200.0, 200.0]),
        'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
    }
    # do not generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=False)

    #### Algorithm
    # DMU380 algorithm
    from demo_algorithms import dmu380_sim
    cfg_file = os.path.abspath(
        './/demo_algorithms//dmu380_sim_lib//ekfSim_tilt.cfg')
    algo = dmu380_sim.DMU380Sim(cfg_file)

    #### start simulation
    sim = ins_sim.Sim(
        [fs, 0.0, fs],
        #   motion_def_path+"//motion_def-static.csv",
        "//mnt//share//jd_figure8.csv",
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,  #'[0.1 0.01 0.11]g-random',
        algorithm=algo)
    sim.run(1)
    # generate simulation results, summary, and save data to files
    sim.results('aa')  # do not save data
    # plot data
    sim.plot(['att_euler', 'ref_pos'], opt={'ref_pos': 'projection'})
Пример #20
0
def test_dmu380_sim():
    '''
    test Sim with DMU380 algorithm.
    '''
    #### choose a built-in IMU model, typical for IMU380
    imu_err = {
        'gyro_b': np.array([1.0, -1.0, 0.5]) * 1.0,
        'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
        'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
        'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
        'accel_b': np.array([50.0e-3, 50.0e-3, 50.0e-3]) * 0.0,
        'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
        'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
        'accel_b_corr': np.array([200.0, 200.0, 200.0]),
        'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
    }
    # do not generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)

    #### Algorithm
    # DMU380 algorithm
    from demo_algorithms import aceinna_vg
    cfg_file = os.path.abspath(
        './/demo_algorithms//dmu380_sim_lib//ekfSim_tilt.cfg')
    algo = aceinna_vg.DMU380Sim(cfg_file)

    #### start simulation
    sim = ins_sim.Sim(
        [fs, 0.0, fs],
        motion_def_path + "//motion_def-static.csv",
        #   ".//demo_saved_data//car_test_20180929//",
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,  #'[0.1 0.01 0.11]g-random',
        algorithm=algo)
    sim.run(1)
    # generate simulation results, summary, and save data to files
    sim.results()  # do not save data
    # plot data
    sim.plot(['att_euler', 'ref_pos'], opt={'att_euler': 'error'})
Пример #21
0
def test_gen_data_from_files():
    '''
    test only path generation in Sim.
    '''
    #### choose a built-in IMU model, typical for IMU381
    imu = None

    #### start simulation
    sim = ins_sim.Sim(
        [fs, fs_gps, fs_mag],
        'e://Projects//gnss-ins-sim//demo_saved_data//2018-05-16-14-23-34//',
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,
        algorithm=None)
    sim.run()
    # save simulation data to files
    sim.results()
    # plot data, 3d plot of reference positoin, 2d plots of gyro and accel
    sim.plot(['ref_att_euler', 'att_euler'], opt={'att_euler': 'error'})
def run_and_save_results(args, motion_def):
    resultsdir = args.outdir
    stagingdir = os.path.join(resultsdir, "staging")
    # Before running, warn if the resultsdir holds existing results.
    if glob.glob("{}/dr_*.csv".format(resultsdir)):
        print(
            "WARNING: DR trajectory results already exist in the given results directory."
        )
        print("         You may end up with a mismatched set.")
    # Setup and run N simulations.
    imu = imu_model.IMU(accuracy=IMU_MODELS[args.imu], axis=6, gps=False)
    ini_pos_vel_att = read_initial_condition(motion_def)
    for i in range(args.N):
        if args.enable_init_error:
            init_cond = perturbed_initial_condition(ini_pos_vel_att)
        else:
            init_cond = ini_pos_vel_att
        if args.enable_vibrations:
            env = DEFAULT_VIBRATIONS
        else:
            env = None
        algo = FreeIntegrationWithVel(init_cond,
                                      meas_vel_stddev=args.odom_sigma)
        sim = ins_sim.Sim([args.fs, 0.0, 0.0],
                          motion_def,
                          ref_frame=0,
                          imu=imu,
                          mode=None,
                          env=env,
                          algorithm=algo)
        sim.run(1)
        # We don't care for the printed results.
        with open(os.devnull, 'w') as devnull:
            stdout = sys.stdout
            sys.stdout = devnull
            sim.results(stagingdir, end_point=True)
            sys.stdout = stdout
        collate_sim_results(stagingdir,
                            os.path.join(resultsdir, "dr_{}.csv".format(i)))
    shutil.rmtree(stagingdir)
def test_gen_data_from_files(data_dir):
    '''
    test data generation from files.
    '''
    #### start simulation
    #### Algorithm
    # Free integration in a virtual inertial frame
    from demo_algorithms import free_integration
    '''
    Free integration requires initial states (position, velocity and attitude). You should provide
    theses values when you create the algorithm object.
    '''
    ini_pos_vel_att = np.genfromtxt(motion_def_path+"//motion_def-90deg_turn.csv",\
                                    delimiter=',', skip_header=1, max_rows=1)
    ini_pos_vel_att[0] = ini_pos_vel_att[0] * D2R
    ini_pos_vel_att[1] = ini_pos_vel_att[1] * D2R
    ini_pos_vel_att[6:9] = ini_pos_vel_att[6:9] * D2R
    # add initial states error if needed
    ini_vel_err = np.array([0.0, 0.0, 0.0
                            ])  # initial velocity error in the body frame, m/s
    ini_att_err = np.array([0.0, 0.0, 0.0])  # initial Euler angles error, deg
    ini_pos_vel_att[3:6] += ini_vel_err
    ini_pos_vel_att[6:9] += ini_att_err * D2R
    # create the algorith object
    algo = free_integration.FreeIntegration(ini_pos_vel_att)

    #### start simulation
    sim = ins_sim.Sim([fs, 0.0, 0.0],
                      data_dir,
                      ref_frame=0,
                      imu=None,
                      mode=None,
                      env=None,
                      algorithm=None)
    # run the simulation for 1000 times
    sim.run(1)
    # generate simulation results, summary
    # do not save data since the simulation runs for 1000 times and generates too many results
    sim.results('', err_stats_start=-1, gen_kml=True)
    sim.plot(['att_euler'])
Пример #24
0
def test_path_gen():
    '''
    test only path generation in Sim.
    '''
    #### choose a built-in IMU model, typical for IMU381
    imu_err = 'mid-accuracy'
    # generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)

    #### start simulation
    sim = ins_sim.Sim([fs, fs_gps, fs_mag],
                      motion_def_path + "//my_test.csv",
                      ref_frame=0,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=None)
    sim.run(1)
    # save simulation data to files
    sim.results('')
    # plot data, 3d plot of reference positoin, 2d plots of gyro and accel
    sim.plot(['ref_pos', 'gyro', "ref_vel"], opt={'ref_pos': '3d'})
Пример #25
0
def test_ins_loose():
    '''
    test Sim
    '''
    #### IMU model, typical for IMU381
    imu_err = {
        'gyro_b': np.array([0.0, 0.0, 0.0]),
        'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
        'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
        'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
        'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
        'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
        'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
        'accel_b_corr': np.array([200.0, 200.0, 200.0]),
        'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
    }
    # do not generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True)

    #### Algorithm
    # loosely couple INS algorithm
    from demo_algorithms import ins_loose
    algo = ins_loose.InsLoose()

    #### start simulation
    sim = ins_sim.Sim([fs, fs_gps, 0.0],
                      motion_def_path + "//motion_def-90deg_turn.csv",
                      ref_frame=0,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=algo)
    # run the simulation for 1000 times
    sim.run()
    # generate simulation results, summary
    # do not save data since the simulation runs for 1000 times and generates too many results
    sim.results(end_point=True)
    sim.plot(['ref_pos', 'pos'], opt={'ref_pos': '3d'})
def test_inclinometer_mahony():
    '''
    test Sim with a inclinometer algorithm based on Mahony's theory.
    '''
    #### choose a built-in IMU model, typical for IMU380
    imu_err = {
        'gyro_b': np.array([5.0, -5.0, 2.5]) * 3600.0,
        'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
        'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
        'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
        'accel_b': np.array([50.0e-3, 50.0e-3, 50.0e-3]) * 0.0,
        'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
        'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
        'accel_b_corr': np.array([200.0, 200.0, 200.0]),
        'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
    }
    # do not generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)

    #### Algorithm
    # ECF based inclinometer
    from demo_algorithms import inclinometer_mahony
    algo = inclinometer_mahony.MahonyFilter()

    #### start simulation
    sim = ins_sim.Sim(
        [fs, 0.0, 0.0],
        motion_def_path + "//motion_def.csv",
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,  #'[0.01 0.01 0.11]g-random',
        algorithm=algo)
    sim.run()
    # generate simulation results, summary, and save data to files
    sim.results()  # do not save data
    # plot data
    sim.plot(['att_euler', 'wb', 'ab'], opt={'att_euler': 'error'})
Пример #27
0
def test_allan():
    '''
    An Allan analysis demo for Sim.
    '''
    #### Customized IMU model parameters, typical for IMU381
    imu_err = {
        'gyro_b': np.array([0.0, 0.0, 0.0]),
        'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
        'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
        'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
        'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
        'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
        'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
        'accel_b_corr': np.array([200.0, 200.0, 200.0]),
        'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
    }
    # do not generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)

    #### Allan analysis algorithm
    from demo_algorithms import allan_analysis
    algo = allan_analysis.Allan()

    #### start simulation
    sim = ins_sim.Sim([fs, 0.0, 0.0],
                      motion_def_path + "//motion_def-Allan.csv",
                      ref_frame=1,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=algo)
    sim.run()
    # generate simulation results, summary, and save data to files
    sim.results()  # save data files
    # plot data
    sim.plot(['ad_accel', 'ad_gyro'])
Пример #28
0
def run_simulation(data, fileName, request_body):
    '''
    An Allan analysis demo for Sim.
    '''
    #### Customized IMU model parameters, typical for IMU381
    rateJSON = json.loads(data.rateJSON, object_hook=JSONObject)
    accelJSON = json.loads(data.accelJSON, object_hook=JSONObject)
    axisNum = 6
    gpsFlag = False
    gpsObj = {}
    imu_err = {
        'gyro_b':
        np.array(
            [float(rateJSON.b[0]),
             float(rateJSON.b[1]),
             float(rateJSON.b[2])]),
        'gyro_arw':
        np.array([
            float(rateJSON.arw[0]),
            float(rateJSON.arw[1]),
            float(rateJSON.arw[2])
        ]) * 1.0,
        'gyro_b_stability':
        np.array([
            float(rateJSON.b_drift[0]),
            float(rateJSON.b_drift[1]),
            float(rateJSON.b_drift[2])
        ]) * 1.0,
        'gyro_b_corr':
        np.array([
            float(rateJSON.b_corr[0]),
            float(rateJSON.b_corr[1]),
            float(rateJSON.b_corr[2])
        ]),
        'accel_b':
        np.array([
            float(accelJSON.b[0]),
            float(accelJSON.b[1]),
            float(accelJSON.b[2])
        ]),
        'accel_vrw':
        np.array([
            float(accelJSON.vrw[0]),
            float(accelJSON.vrw[1]),
            float(accelJSON.vrw[2])
        ]) * 1.0,
        'accel_b_stability':
        np.array([
            float(accelJSON.b_drift[0]),
            float(accelJSON.b_drift[1]),
            float(accelJSON.b_drift[2])
        ]) * 1.0,
        'accel_b_corr':
        np.array([
            float(accelJSON.b_corr[0]),
            float(accelJSON.b_corr[1]),
            float(accelJSON.b_corr[2])
        ])
    }

    if 'magJSON' in request_body:
        axisNum = 9
        magJSON = json.loads(data.magJSON, object_hook=JSONObject)
        imu_err['mag_std'] = np.array([
            float(magJSON.std[0]),
            float(magJSON.std[1]),
            float(magJSON.std[2])
        ]) * 1.0
        if 'si' in data.magJSON:
            imu_err['mag_si'] = np.array([[
                float(magJSON.si[0]),
                float(magJSON.si[1]),
                float(magJSON.si[2])
            ],
                                          [
                                              float(magJSON.si[3]),
                                              float(magJSON.si[4]),
                                              float(magJSON.si[5])
                                          ],
                                          [
                                              float(magJSON.si[6]),
                                              float(magJSON.si[7]),
                                              float(magJSON.si[8])
                                          ]])
        if 'hi' in data.magJSON:
            imu_err['mag_hi'] = np.array([
                float(magJSON.hi[0]),
                float(magJSON.hi[1]),
                float(magJSON.hi[2])
            ])
    if 'gpsJSON' in request_body:
        gpsFlag = True
        gpsJSON = json.loads(data.gpsJSON, object_hook=JSONObject)
        gpsObj['stdp'] = np.array([
            float(gpsJSON.stdp[0]),
            float(gpsJSON.stdp[1]),
            float(gpsJSON.stdp[2])
        ])
        gpsObj['stdv'] = np.array([
            float(gpsJSON.stdv[0]),
            float(gpsJSON.stdv[1]),
            float(gpsJSON.stdv[2])
        ])
        gpsObj['avail'] = 0.95
    # do not generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err,
                        axis=axisNum,
                        gps=gpsFlag,
                        gps_opt=gpsObj)
    if data.algorithmName == 'Allan':
        Allanfs = 100.0  # IMU sample frequency
        #### Allan analysis algorithm
        from demo_algorithms import allan_analysis
        algo = allan_analysis.Allan()
        #### start simulation
        sim = ins_sim.Sim([Allanfs, Allanfs, 0.0],
                          motion_def_path + "//motion_def-Allan.csv",
                          ref_frame=data.ref_frame,
                          imu=imu,
                          mode=None,
                          env=None,
                          algorithm=algo,
                          fileName=fileName,
                          data=data)
        sim.run(data.algorithmRunTimes)
        # generate simulation results, summary, and save data to files
        sim.results(fileName, gen_kml=True,
                    update_flag=True)  # save data files
        # plot data
        #sim.plot(['ad_accel', 'ad_gyro'])

    elif data.algorithmName == 'FreeIntegration':
        Freefs = 100.0  # IMU sample frequency
        # Free integration in a virtual inertial frame
        ini_pos_vel_att = np.fromstring(data.algorithmParams,
                                        dtype=float,
                                        sep=',')
        #ini_pos_vel_att[0] = ini_pos_vel_att[0] * D2R
        #ini_pos_vel_att[1] = ini_pos_vel_att[1] * D2R
        #ini_pos_vel_att[6:9] = ini_pos_vel_att[6:9] * D2R
        # add initial states error if needed
        #ini_vel_err = np.array([0.0, 0.0, 0.0]) # initial velocity error in the body frame, m/s
        #ini_att_err = np.array([0.0, 0.0, 0.0]) # initial Euler angles error, deg
        #ini_pos_vel_att[3:6] += ini_vel_err
        #ini_pos_vel_att[6:9] += ini_att_err * D2R

        from demo_algorithms import free_integration
        algo = free_integration.FreeIntegration(ini_pos_vel_att)
        staticsFlag = data.algorithmStatistics == 'end-point'
        sim = ins_sim.Sim([Freefs, Freefs, 0.0],
                          motion_def_path + "//motion_def-90deg_turn.csv",
                          ref_frame=data.ref_frame,
                          imu=imu,
                          mode=None,
                          env=None,
                          algorithm=algo,
                          fileName=fileName,
                          data=data)
        # run the simulation for 1000 times
        sim.run(data.algorithmRunTimes)
        # generate simulation results, summary
        # do not save data since the simulation runs for 1000 times and generates too many results
        sim.results(fileName,
                    gen_kml=True,
                    end_point=staticsFlag,
                    update_flag=True,
                    extra_opt='ned')

    elif data.algorithmName == 'VG':
        VGfs = 200.0  # IMU sample frequency
        from demo_algorithms import aceinna_vg
        cfg_file = os.path.abspath(
            './/demo_algorithms//dmu380_sim_lib//ekfSim_tilt.cfg')
        algo = aceinna_vg.DMU380Sim(cfg_file)
        sim = ins_sim.Sim(
            [VGfs, VGfs, VGfs],
            "//mnt//share//jd_figure8.csv",
            ref_frame=data.ref_frame,
            imu=imu,
            mode=None,
            env=None,  #'[0.1 0.01 0.11]g-random',
            algorithm=algo,
            fileName=fileName,
            data=data)
        sim.run(data.algorithmRunTimes)
        # generate simulation results, summary, and save data to files
        sim.results(
            fileName,
            gen_kml=True,
            update_flag=True,
        )  # do not save data

    elif data.algorithmName == 'INS':
        INSfs = 200.0  # IMU sample frequency
        from demo_algorithms import aceinna_ins
        cfg_file = os.path.abspath(
            './/demo_algorithms//dmu380_sim_lib//ekfSim_ins.cfg')
        algo = aceinna_ins.DMU380Sim(cfg_file)
        sim = ins_sim.Sim(
            [INSfs, INSfs, INSfs],
            "//mnt//share//jd_figure8.csv",
            ref_frame=data.ref_frame,
            imu=imu,
            mode=None,
            env=None,  #'[0.1 0.01 0.11]g-random',
            algorithm=algo,
            fileName=fileName,
            data=data)
        sim.run(data.algorithmRunTimes)
        # generate simulation results, summary, and save data to files
        sim.results(
            fileName,
            gen_kml=True,
            update_flag=True,
        )  # do not save data
Пример #29
0
def get_gnss_ins_sim(motion_def_file, fs_imu, fs_gps):
    '''
    Generate simulated GNSS/IMU data using specified trajectory.
    '''
    # set IMU model:
    D2R = math.pi/180.0
    # imu_err = 'low-accuracy'
    imu_err = {
        # 1. gyro:
        # a. random noise:
        # gyro angle random walk, deg/rt-hr
        'gyro_arw': np.array([0.75, 0.75, 0.75]),
        # gyro bias instability, deg/hr
        'gyro_b_stability': np.array([10.0, 10.0, 10.0]),
        # gyro bias isntability correlation time, sec
        'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
        # b. deterministic error:
        'gyro_b': np.array([0.0, 0.0, 0.0]),
        'gyro_k': np.array([1.0, 1.0, 1.0]),
        'gyro_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
        # 2. accel:
        # a. random noise:
        # accel velocity random walk, m/s/rt-hr
        'accel_vrw': np.array([0.05, 0.05, 0.05]),
        # accel bias instability, m/s2
        'accel_b_stability': np.array([2.0e-4, 2.0e-4, 2.0e-4]),
        # accel bias isntability correlation time, sec
        'accel_b_corr': np.array([100.0, 100.0, 100.0]),
        # b. deterministic error:
        'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
        'accel_k': np.array([1.0, 1.0, 1.0]),
        'accel_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
        # 3. mag:
        'mag_si': np.eye(3) + np.random.randn(3, 3)*0.0, 
        'mag_hi': np.array([10.0, 10.0, 10.0])*0.0,
        'mag_std': np.array([0.1, 0.1, 0.1])
    }
    
    
    # generate GPS and magnetometer data:
    imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)

    # init simulation:
    sim = ins_sim.Sim(
        [fs_imu, fs_gps, fs_imu],
        motion_def_file,
        ref_frame=1,
        imu=imu,
        mode=None,
        env=None,
        algorithm=None
    )
    
    # run:
    sim.run(1)

    # get simulated data:
    rospy.logwarn(
        "Simulated data size {}".format(
            len(sim.dmgr.get_data_all('gyro').data[0])
        )
    )

    # imu measurements:
    step_size = 1.0 / fs_imu
    for i, (gyro, accel, ref_pos, ref_vel, ref_att_quat) in enumerate(
        zip(
            # a. gyro
            sim.dmgr.get_data_all('ref_gyro').data, 
            # b. accel
            sim.dmgr.get_data_all('ref_accel').data,
            # c. ref_pos
            sim.dmgr.get_data_all('ref_pos').data,
            # d. ref_vel
            sim.dmgr.get_data_all('ref_vel').data,
            # e. ref_att_quat
            sim.dmgr.get_data_all('ref_att_quat').data
        )
    ):
        yield {
            'stamp': i * step_size,
            'data': {
                # a. gyro:
                'gyro_x': gyro[0],
                'gyro_y': gyro[1],
                'gyro_z': gyro[2],
                # b. accel:
                'accel_x': accel[0],
                'accel_y': accel[1],
                'accel_z': accel[2],
                # c. ref_pos:
                'ref_pos_x': ref_pos[0],
                'ref_pos_y': ref_pos[1],
                'ref_pos_z': ref_pos[2],
                # d. ref_vel:
                'ref_vel_x': ref_vel[0],
                'ref_vel_y': ref_vel[1],
                'ref_vel_z': ref_vel[2],
                # e. ref_att_quat:
                'ref_att_quat_w': ref_att_quat[0],
                'ref_att_quat_x': ref_att_quat[1],
                'ref_att_quat_y': ref_att_quat[2], 
                'ref_att_quat_z': ref_att_quat[3]
            }
        }
Пример #30
0
def get_gnss_ins_sim(motion_def_file,
                     fs_imu,
                     fs_gps,
                     imu_error_level='high_accuracy',
                     mag_error_level='mid_accuracy',
                     gps_error_level='mid_accuracy',
                     odo_error_level='mid_accuracy'):
    """
    Generate simulated 
        IMU, 
        magnetometer, 
        GPS, 
        odometer 
    data using specified trajectory
    """
    #
    # set IMU model:
    #

    # for error-state Kalman filter observability & the degree of observability analysis
    # remove deterministic error and random noise:
    imu_err = config['imu'][imu_error_level].copy()
    imu_err.update(config['mag'][mag_error_level])

    gps_err = config['gps'][gps_error_level]
    odo_err = config['odo'][odo_error_level]

    # show device error level config:
    for k in imu_err:
        rospy.logwarn("{}: {}".format(k, imu_err[k]))
    for k in gps_err:
        rospy.logwarn("{}: {}".format(k, gps_err[k]))
    for k in odo_err:
        rospy.logwarn("{}: {}".format(k, odo_err[k]))

    # generate GPS and magnetometer data:
    imu = imu_model.IMU(accuracy=imu_err,
                        axis=9,
                        gps=True,
                        gps_opt=gps_err,
                        odo=True,
                        odo_opt=odo_err)

    # init simulation:
    sim = ins_sim.Sim(
        # here sync GPS with other measurements as marker:
        [fs_imu, fs_imu, fs_imu],
        motion_def_file,
        # use NED frame:
        ref_frame=0,
        imu=imu,
        mode=None,
        env=None,
        algorithm=None)

    # run:
    sim.run(1)

    # get simulated data:
    rospy.logwarn("""
        GNSS-INS-Sim Summary:
        \tIMU Measurement:
        \t\tGyroscope: {}
        \t\tAccelerometer: {}
        \t\tMagnetometer: {}
        \tGNSS Measurement:
        \t\tLLA: {}
        \t\tNED Velocity: {}
        \tOdometry:
        \t\tVelocity: {}
        \tReference Trajectory:
        \t\tPosition: {}
        \t\tVelocity: {}
        \t\tOrientation in Quaternion: {}
        """.format(
        # a. IMU:
        repr(sim.dmgr.get_data_all('gyro').data[0].shape),
        repr(sim.dmgr.get_data_all('accel').data[0].shape),
        repr(sim.dmgr.get_data_all('mag').data[0].shape),
        # b. GNSS:
        repr(sim.dmgr.get_data_all('gps').data[0][:, :3].shape),
        repr(sim.dmgr.get_data_all('gps').data[0][:, 3:].shape),
        # c. odometry:
        repr(sim.dmgr.get_data_all('odo').data[0].shape),
        # d. reference trajectory:
        repr(sim.dmgr.get_data_all('ref_pos').data.shape),
        repr(sim.dmgr.get_data_all('ref_vel').data.shape),
        repr(sim.dmgr.get_data_all('ref_att_quat').data.shape),
    ))

    # init timer:
    timestamp_start = rospy.Time.now()
    STEP_SIZE = 1.0 / fs_imu

    # yield init pose:
    init_pose_msg = get_init_pose(timestamp_start, motion_def_file)
    yield init_pose_msg

    # yield measurements:
    for i, (
            # a. IMU:
            gyro,
            accel,
            mag,
            # b. GNSS:
            gps_pos,
            gps_vel,
            # c. odometry:
            odo_x,
            # d. reference trajectory:
            ref_pos,
            ref_vel,
            ref_att_quat) in enumerate(
                zip(
                    # a. IMU:
                    sim.dmgr.get_data_all('gyro').data[0],
                    sim.dmgr.get_data_all('accel').data[0],
                    sim.dmgr.get_data_all('mag').data[0],
                    # b. GNSS:
                    sim.dmgr.get_data_all('gps').data[0][:, :3],
                    sim.dmgr.get_data_all('gps').data[0][:, 3:],
                    # c. odometry velocity:
                    sim.dmgr.get_data_all('odo').data[0],
                    # d. reference trajectory:
                    sim.dmgr.get_data_all('ref_pos').data,
                    sim.dmgr.get_data_all('ref_vel').data,
                    sim.dmgr.get_data_all('ref_att_quat').data)):
        # generate timestamp:
        stamp = timestamp_start + rospy.Duration.from_sec(i * STEP_SIZE)

        # a. IMU:
        imu_msg = get_imu_msg(stamp, gyro, accel)

        # b. magnetometer:
        mag_msg = get_mag_msg(stamp, mag)

        # c. GNSS:
        gps_pos_msg = get_gps_pos_msg(stamp, gps_pos)
        gps_vel_msg = get_gps_vel_msg(stamp, gps_vel)

        # d. odometer:
        odo_msg = get_odo_msg(
            stamp,
            # measurement is only available at forward direction
            np.array([odo_x, 0.0, 0.0]))

        # e. reference trajectory:
        reference_pose_msg = get_pose_msg(stamp, ref_pos, ref_vel,
                                          ref_att_quat)

        yield (imu_msg, mag_msg, gps_pos_msg, gps_vel_msg, odo_msg,
               reference_pose_msg)